xref: /optee_os/core/kernel/dt_driver.c (revision 92d75aefed568a557dbd9152d749a4bc320fa9f2)
1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3  * Copyright (c) 2021, Linaro Limited
4  * Copyright (c) 2021, Bootlin
5  * Copyright (c) 2021, Linaro Limited
6  * Copyright (c) 2021, STMicroelectronics
7  */
8 
9 #include <assert.h>
10 #include <config.h>
11 #include <initcall.h>
12 #include <kernel/boot.h>
13 #include <kernel/dt.h>
14 #include <kernel/dt_driver.h>
15 #include <libfdt.h>
16 #include <malloc.h>
17 #include <sys/queue.h>
18 #include <tee_api_defines_extensions.h>
19 #include <tee_api_types.h>
20 
21 /*
22  * struct dt_driver_probe - Node instance in secure FDT to probe a driver for
23  *
24  * @link: List hook
25  * @nodeoffset: Node offset of device referenced in the FDT
26  * @type: One of DT_DRIVER_* or DT_DRIVER_NOTYPE.
27  * @deferrals: Driver probe deferrals count
28  * @dt_drv: Matching driver to probe if found or NULL
29  * @dm: Matching reference if applicable or NULL
30  */
31 struct dt_driver_probe {
32 	int nodeoffset;
33 	enum dt_driver_type type;
34 	unsigned int deferrals;
35 	const struct dt_driver *dt_drv;
36 	const struct dt_device_match *dm;
37 	TAILQ_ENTRY(dt_driver_probe) link;
38 };
39 
40 /*
41  * struct dt_driver_provider - DT related info on probed device
42  *
43  * Saves information on the probed device so that device
44  * drivers can get resources from DT phandle and related arguments.
45  *
46  * @nodeoffset: Node offset of device referenced in the FDT
47  * @type: One of DT_DRIVER_* or DT_DRIVER_NOTYPE.
48  * @provider_cells: Cells count in the FDT used by the driver's references
49  * @get_of_device: Function to get driver's device ref from phandle data
50  * @priv_data: Driver private data passed as @get_of_device argument
51  * @link: Reference in DT driver providers list
52  */
53 struct dt_driver_provider {
54 	int nodeoffset;
55 	enum dt_driver_type type;
56 	unsigned int provider_cells;
57 	uint32_t phandle;
58 	get_of_device_func get_of_device;
59 	void *priv_data;
60 	SLIST_ENTRY(dt_driver_provider) link;
61 };
62 
63 /*
64  * Device driver providers are able to provide a driver specific instance
65  * related to device phandle arguments found in the secure embedded FDT.
66  */
67 static SLIST_HEAD(, dt_driver_provider) dt_driver_provider_list =
68 	SLIST_HEAD_INITIALIZER(dt_driver_provider_list);
69 
70 /* FDT nodes for which a matching driver is to be probed */
71 static TAILQ_HEAD(dt_driver_probe_head, dt_driver_probe) dt_driver_probe_list =
72 	TAILQ_HEAD_INITIALIZER(dt_driver_probe_list);
73 
74 /* FDT nodes for which a matching driver has been successfully probed */
75 static TAILQ_HEAD(, dt_driver_probe) dt_driver_ready_list =
76 	TAILQ_HEAD_INITIALIZER(dt_driver_ready_list);
77 
78 /* List of the nodes for which a compatible driver but reported a failure */
79 static TAILQ_HEAD(, dt_driver_probe) dt_driver_failed_list =
80 	TAILQ_HEAD_INITIALIZER(dt_driver_failed_list);
81 
82 /* Flag enabled when a new node (possibly typed) is added in the probe list */
83 static bool added_node;
84 
85 /* Resolve drivers dependencies on core crypto layer */
86 static bool tee_crypt_is_ready;
87 
88 void dt_driver_crypt_init_complete(void)
89 {
90 	assert(!tee_crypt_is_ready);
91 	tee_crypt_is_ready = true;
92 }
93 
94 TEE_Result dt_driver_get_crypto(void)
95 {
96 	if (tee_crypt_is_ready)
97 		return TEE_SUCCESS;
98 	else
99 		return TEE_ERROR_DEFER_DRIVER_INIT;
100 }
101 
102 static void assert_type_is_valid(enum dt_driver_type type)
103 {
104 	switch (type) {
105 	case DT_DRIVER_NOTYPE:
106 	case DT_DRIVER_CLK:
107 	case DT_DRIVER_RSTCTRL:
108 	case DT_DRIVER_UART:
109 	case DT_DRIVER_I2C:
110 		return;
111 	default:
112 		assert(0);
113 	}
114 }
115 
116 /*
117  * Driver provider registering API functions
118  */
119 
120 TEE_Result dt_driver_register_provider(const void *fdt, int nodeoffset,
121 				       get_of_device_func get_of_device,
122 				       void *priv, enum dt_driver_type type)
123 {
124 	struct dt_driver_provider *prv = NULL;
125 	int provider_cells = 0;
126 	uint32_t phandle = 0;
127 
128 	assert_type_is_valid(type);
129 
130 	provider_cells = fdt_get_dt_driver_cells(fdt, nodeoffset, type);
131 	if (provider_cells < 0) {
132 		DMSG("Failed to find provider cells: %d", provider_cells);
133 		return TEE_ERROR_GENERIC;
134 	}
135 
136 	phandle = fdt_get_phandle(fdt, nodeoffset);
137 	if (phandle == (uint32_t)-1) {
138 		DMSG("Failed to find provide phandle");
139 		return TEE_ERROR_GENERIC;
140 	}
141 
142 	prv = calloc(1, sizeof(*prv));
143 	if (!prv)
144 		return TEE_ERROR_OUT_OF_MEMORY;
145 
146 	prv->nodeoffset = nodeoffset;
147 	prv->type = type;
148 	prv->provider_cells = provider_cells;
149 	prv->phandle = phandle;
150 	prv->get_of_device = get_of_device;
151 	prv->priv_data = priv;
152 
153 	SLIST_INSERT_HEAD(&dt_driver_provider_list, prv, link);
154 
155 	return TEE_SUCCESS;
156 }
157 
158 /*
159  * Helper functions for dt_drivers querying driver provider information
160  */
161 
162 int fdt_get_dt_driver_cells(const void *fdt, int nodeoffset,
163 			    enum dt_driver_type type)
164 {
165 	const char *cells_name = NULL;
166 	const fdt32_t *c = NULL;
167 	int len = 0;
168 
169 	switch (type) {
170 	case DT_DRIVER_CLK:
171 		cells_name = "#clock-cells";
172 		break;
173 	case DT_DRIVER_RSTCTRL:
174 		cells_name = "#reset-cells";
175 		break;
176 	case DT_DRIVER_I2C:
177 		return 0;
178 	default:
179 		panic();
180 	}
181 
182 	c = fdt_getprop(fdt, nodeoffset, cells_name, &len);
183 	if (!c)
184 		return len;
185 
186 	if (len != sizeof(*c))
187 		return -FDT_ERR_BADNCELLS;
188 
189 	return fdt32_to_cpu(*c);
190 }
191 
192 unsigned int dt_driver_provider_cells(struct dt_driver_provider *prv)
193 {
194 	return prv->provider_cells;
195 }
196 
197 struct dt_driver_provider *
198 dt_driver_get_provider_by_node(int nodeoffset, enum dt_driver_type type)
199 {
200 	struct dt_driver_provider *prv = NULL;
201 
202 	SLIST_FOREACH(prv, &dt_driver_provider_list, link)
203 		if (prv->nodeoffset == nodeoffset && prv->type == type)
204 			return prv;
205 
206 	return NULL;
207 }
208 
209 struct dt_driver_provider *
210 dt_driver_get_provider_by_phandle(uint32_t phandle, enum dt_driver_type type)
211 {
212 	struct dt_driver_provider *prv = NULL;
213 
214 	SLIST_FOREACH(prv, &dt_driver_provider_list, link)
215 		if (prv->phandle == phandle && prv->type == type)
216 			return prv;
217 
218 	return NULL;
219 }
220 
221 static void *device_from_provider_prop(struct dt_driver_provider *prv,
222 				       const void *fdt, int phandle_node,
223 				       const uint32_t *prop, TEE_Result *res)
224 {
225 	struct dt_driver_phandle_args *pargs = NULL;
226 	unsigned int n = 0;
227 	void *device = NULL;
228 
229 	pargs = calloc(1, prv->provider_cells * sizeof(uint32_t *) +
230 		       sizeof(*pargs));
231 	if (!pargs) {
232 		*res = TEE_ERROR_OUT_OF_MEMORY;
233 		return NULL;
234 	}
235 
236 	pargs->fdt = fdt;
237 	pargs->phandle_node = phandle_node;
238 	pargs->args_count = prv->provider_cells;
239 	for (n = 0; n < prv->provider_cells; n++)
240 		pargs->args[n] = fdt32_to_cpu(prop[n + 1]);
241 
242 	device = prv->get_of_device(pargs, prv->priv_data, res);
243 
244 	free(pargs);
245 
246 	return device;
247 }
248 
249 void *dt_driver_device_from_parent(const void *fdt, int nodeoffset,
250 				   enum dt_driver_type type, TEE_Result *res)
251 {
252 	int parent = -1;
253 	struct dt_driver_provider *prv = NULL;
254 
255 	assert(fdt == get_secure_dt());
256 
257 	parent = fdt_parent_offset(fdt, nodeoffset);
258 	if (parent < 0) {
259 		*res =  TEE_ERROR_GENERIC;
260 		return NULL;
261 	}
262 
263 	prv = dt_driver_get_provider_by_node(parent, type);
264 	if (!prv) {
265 		/* No provider registered yet */
266 		*res = TEE_ERROR_DEFER_DRIVER_INIT;
267 		return NULL;
268 	}
269 
270 	return device_from_provider_prop(prv, fdt, nodeoffset, NULL, res);
271 }
272 
273 void *dt_driver_device_from_node_idx_prop(const char *prop_name,
274 					  const void *fdt, int nodeoffset,
275 					  unsigned int prop_idx,
276 					  enum dt_driver_type type,
277 					  TEE_Result *res)
278 {
279 	int len = 0;
280 	int idx = 0;
281 	int idx32 = 0;
282 	int prv_cells = 0;
283 	int phandle_node = -1;
284 	uint32_t phandle = 0;
285 	const uint32_t *prop = NULL;
286 	struct dt_driver_provider *prv = NULL;
287 
288 	prop = fdt_getprop(fdt, nodeoffset, prop_name, &len);
289 	if (!prop) {
290 		DMSG("Property %s missing in node %s", prop_name,
291 		     fdt_get_name(fdt, nodeoffset, NULL));
292 		*res = TEE_ERROR_ITEM_NOT_FOUND;
293 		return NULL;
294 	}
295 
296 	while (idx < len) {
297 		idx32 = idx / sizeof(uint32_t);
298 		phandle = fdt32_to_cpu(prop[idx32]);
299 		if (!phandle) {
300 			if (!prop_idx)
301 				break;
302 			idx += sizeof(phandle);
303 			prop_idx--;
304 			continue;
305 		}
306 
307 		prv = dt_driver_get_provider_by_phandle(phandle, type);
308 		if (!prv) {
309 			/* No provider registered yet */
310 			*res = TEE_ERROR_DEFER_DRIVER_INIT;
311 			return NULL;
312 		}
313 
314 		prv_cells = dt_driver_provider_cells(prv);
315 		if (prop_idx) {
316 			prop_idx--;
317 			idx += sizeof(phandle) + prv_cells * sizeof(uint32_t);
318 			continue;
319 		}
320 
321 		return device_from_provider_prop(prv, fdt, phandle_node,
322 						 prop + idx32, res);
323 	}
324 
325 	*res = TEE_ERROR_ITEM_NOT_FOUND;
326 	return NULL;
327 }
328 
329 static void __maybe_unused print_probe_list(const void *fdt __maybe_unused)
330 {
331 	struct dt_driver_probe *elt = NULL;
332 	unsigned int count = 0;
333 
334 	TAILQ_FOREACH(elt, &dt_driver_probe_list, link)
335 		count++;
336 
337 	DMSG("Probe list: %u elements", count);
338 	TAILQ_FOREACH(elt, &dt_driver_probe_list, link)
339 		DMSG("|- Driver %s probes on node %s",
340 		     elt->dt_drv->name,
341 		     fdt_get_name(fdt, elt->nodeoffset, NULL));
342 
343 	DMSG("`- Probe list end");
344 
345 	count = 0;
346 	TAILQ_FOREACH(elt, &dt_driver_failed_list, link)
347 		count++;
348 
349 	DMSG("Failed list: %u elements", count);
350 	TAILQ_FOREACH(elt, &dt_driver_failed_list, link)
351 		EMSG("|- Driver %s on node %s failed", elt->dt_drv->name,
352 		     fdt_get_name(fdt, elt->nodeoffset, NULL));
353 
354 	DMSG("`- Failed list end");
355 }
356 
357 /*
358  * Probe element: push to ready list if succeeds, push to probe list if probe
359  * if deferred, panic with an error trace otherwise.
360  */
361 static TEE_Result probe_driver_node(const void *fdt,
362 				    struct dt_driver_probe *elt)
363 {
364 	TEE_Result res = TEE_ERROR_GENERIC;
365 	const char __maybe_unused *drv_name = NULL;
366 	const char __maybe_unused *node_name = NULL;
367 
368 	node_name = fdt_get_name(fdt, elt->nodeoffset, NULL);
369 	drv_name = elt->dt_drv->name;
370 
371 	if (!elt->dt_drv->probe) {
372 		DMSG("No probe operator for driver %s, skipped", drv_name);
373 		return TEE_SUCCESS;
374 	}
375 
376 	FMSG("Probing %s on node %s", drv_name, node_name);
377 
378 	res = elt->dt_drv->probe(fdt, elt->nodeoffset, elt->dm->compat_data);
379 	switch (res) {
380 	case TEE_SUCCESS:
381 		TAILQ_INSERT_HEAD(&dt_driver_ready_list, elt, link);
382 
383 		DMSG("element: %s on node %s initialized", drv_name, node_name);
384 		break;
385 	case TEE_ERROR_DEFER_DRIVER_INIT:
386 		elt->deferrals++;
387 		TAILQ_INSERT_TAIL(&dt_driver_probe_list, elt, link);
388 
389 		DMSG("element: %s on node %s deferred %u time(s)", drv_name,
390 		     node_name, elt->deferrals);
391 		break;
392 	case TEE_ERROR_NODE_DISABLED:
393 		DMSG("element: %s on node %s is disabled", drv_name, node_name);
394 		break;
395 	default:
396 		TAILQ_INSERT_HEAD(&dt_driver_failed_list, elt, link);
397 
398 		EMSG("Failed to probe %s on node %s: %#"PRIx32,
399 		     drv_name, node_name, res);
400 		break;
401 	}
402 
403 	return res;
404 }
405 
406 static TEE_Result alloc_elt_and_probe(const void *fdt, int node,
407 				      const struct dt_driver *dt_drv,
408 				      const struct dt_device_match *dm)
409 {
410 	struct dt_driver_probe *elt = NULL;
411 
412 	/* Will be freed when lists are released */
413 	elt = calloc(1, sizeof(*elt));
414 	if (!elt)
415 		return TEE_ERROR_OUT_OF_MEMORY;
416 
417 	elt->nodeoffset = node;
418 	elt->dt_drv = dt_drv;
419 	elt->dm = dm;
420 	elt->type = dt_drv->type;
421 
422 	return probe_driver_node(fdt, elt);
423 }
424 
425 /* Lookup a compatible driver, possibly of a specific @type, for the FDT node */
426 static TEE_Result probe_device_by_compat(const void *fdt, int node,
427 					 const char *compat,
428 					 enum dt_driver_type type)
429 {
430 	const struct dt_driver *drv = NULL;
431 	const struct dt_device_match *dm = NULL;
432 
433 	for_each_dt_driver(drv) {
434 		if (drv->type != type)
435 			continue;
436 
437 		for (dm = drv->match_table; dm && dm->compatible; dm++)
438 			if (strcmp(dm->compatible, compat) == 0)
439 				return alloc_elt_and_probe(fdt, node, drv, dm);
440 	}
441 
442 	return TEE_ERROR_ITEM_NOT_FOUND;
443 }
444 
445 /*
446  * Lookup the best matching compatible driver, possibly of a specific @type,
447  * for the FDT node.
448  */
449 TEE_Result dt_driver_probe_device_by_node(const void *fdt, int nodeoffset,
450 					  enum dt_driver_type type)
451 {
452 	int idx = 0;
453 	int len = 0;
454 	int count = 0;
455 	const char *compat = NULL;
456 	TEE_Result res = TEE_ERROR_GENERIC;
457 
458 	assert_type_is_valid(type);
459 
460 	count = fdt_stringlist_count(fdt, nodeoffset, "compatible");
461 	if (count < 0)
462 		return TEE_ERROR_ITEM_NOT_FOUND;
463 
464 	for (idx = 0; idx < count; idx++) {
465 		compat = fdt_stringlist_get(fdt, nodeoffset, "compatible",
466 					    idx, &len);
467 		if (!compat)
468 			return TEE_ERROR_GENERIC;
469 
470 		res = probe_device_by_compat(fdt, nodeoffset, compat, type);
471 
472 		if (res != TEE_ERROR_ITEM_NOT_FOUND)
473 			return res;
474 	}
475 
476 	return TEE_ERROR_ITEM_NOT_FOUND;
477 }
478 
479 static TEE_Result process_probe_list(const void *fdt)
480 {
481 	struct dt_driver_probe *elt = NULL;
482 	struct dt_driver_probe *prev = NULL;
483 	static unsigned int __maybe_unused loop_count;
484 	static unsigned int __maybe_unused deferral_loop_count;
485 	bool __maybe_unused one_deferred = false;
486 	bool one_probed_ok = false;
487 
488 	do {
489 		loop_count++;
490 		FMSG("Probe loop %u after %u for deferral(s)", loop_count,
491 		     deferral_loop_count);
492 
493 		/* Hack here for TRACE_DEBUG messages on probe list elements */
494 		if (TRACE_LEVEL >= TRACE_FLOW)
495 			print_probe_list(fdt);
496 
497 		if (TAILQ_EMPTY(&dt_driver_probe_list))
498 			return TEE_SUCCESS;
499 
500 		/*
501 		 * Probe from current end to top. Deferred probed node are
502 		 * pushed back after current tail for the next probe round.
503 		 * Reset probe result flags and see status after probe round.
504 		 */
505 		one_deferred = false;
506 		one_probed_ok = false;
507 		added_node = false;
508 
509 		TAILQ_FOREACH_REVERSE_SAFE(elt, &dt_driver_probe_list,
510 					   dt_driver_probe_head, link, prev) {
511 			TAILQ_REMOVE(&dt_driver_probe_list, elt, link);
512 
513 			switch (probe_driver_node(fdt, elt)) {
514 			case TEE_SUCCESS:
515 				one_probed_ok = true;
516 				break;
517 			case TEE_ERROR_DEFER_DRIVER_INIT:
518 				one_deferred = true;
519 				break;
520 			default:
521 				break;
522 			}
523 		}
524 
525 		if (one_deferred)
526 			deferral_loop_count++;
527 
528 	} while (added_node || one_probed_ok);
529 
530 	DMSG("Unresolved dependencies after %u rounds, %u deferred",
531 	     loop_count, deferral_loop_count);
532 
533 	if (one_deferred)
534 		return TEE_ERROR_DEFER_DRIVER_INIT;
535 	else
536 		return TEE_ERROR_GENERIC;
537 }
538 
539 static int driver_probe_compare(struct dt_driver_probe *candidate,
540 				struct dt_driver_probe *elt)
541 {
542 	if (candidate->nodeoffset != elt->nodeoffset ||
543 	    candidate->type != elt->type)
544 		return 1;
545 
546 	assert(elt->dt_drv == candidate->dt_drv);
547 	return 0;
548 }
549 
550 /*
551  * Return TEE_SUCCESS if compatible found
552  *	  TEE_ERROR_OUT_OF_MEMORY if heap is exhausted
553  */
554 static TEE_Result add_node_to_probe(const void *fdt, int node,
555 				    const struct dt_driver *dt_drv,
556 				    const struct dt_device_match *dm)
557 {
558 	const char __maybe_unused *node_name = fdt_get_name(fdt, node, NULL);
559 	const char __maybe_unused *drv_name = dt_drv->name;
560 	struct dt_driver_probe *elt = NULL;
561 	struct dt_driver_probe elt_new = {
562 		.dm = dm,
563 		.dt_drv = dt_drv,
564 		.nodeoffset = node,
565 		.type = dt_drv->type,
566 	};
567 
568 	/* If node/type found in probe list or ready list, nothing to do */
569 	TAILQ_FOREACH(elt, &dt_driver_probe_list, link)
570 		if (!driver_probe_compare(&elt_new, elt))
571 			return TEE_SUCCESS;
572 
573 	TAILQ_FOREACH(elt, &dt_driver_ready_list, link)
574 		if (!driver_probe_compare(&elt_new, elt))
575 			return TEE_SUCCESS;
576 
577 	elt = malloc(sizeof(*elt));
578 	if (!elt)
579 		return TEE_ERROR_OUT_OF_MEMORY;
580 
581 	DMSG("element: %s on node %s", node_name, drv_name);
582 
583 	memcpy(elt, &elt_new, sizeof(*elt));
584 
585 	added_node = true;
586 
587 	TAILQ_INSERT_TAIL(&dt_driver_probe_list, elt, link);
588 
589 	/* Hack here for TRACE_DEBUG messages on current probe list elements */
590 	if (TRACE_LEVEL >= TRACE_FLOW)
591 		print_probe_list(fdt);
592 
593 	return TEE_SUCCESS;
594 }
595 
596 /*
597  * Add a node to the probe list if a dt_driver matches target compatible.
598  *
599  * If @type is DT_DRIVER_ANY, probe list can hold only 1 driver to probe for
600  * the node. A node may probe several drivers if have a unique driver type.
601  *
602  * Return TEE_SUCCESS if compatible found
603  *	  TEE_ERROR_ITEM_NOT_FOUND if no matching driver
604  *	  TEE_ERROR_OUT_OF_MEMORY if heap is exhausted
605  */
606 static TEE_Result add_probe_node_by_compat(const void *fdt, int node,
607 					   const char *compat)
608 {
609 	TEE_Result res = TEE_ERROR_ITEM_NOT_FOUND;
610 	const struct dt_driver *dt_drv = NULL;
611 	const struct dt_device_match *dm = NULL;
612 	uint32_t found_types = 0;
613 
614 	for_each_dt_driver(dt_drv) {
615 		for (dm = dt_drv->match_table; dm && dm->compatible; dm++) {
616 			if (strcmp(dm->compatible, compat) == 0) {
617 				assert(dt_drv->type < 32);
618 
619 				res = add_node_to_probe(fdt, node, dt_drv, dm);
620 				if (res)
621 					return res;
622 
623 				if (found_types & BIT(dt_drv->type)) {
624 					EMSG("Driver %s multi hit on type %u",
625 					     dt_drv->name, dt_drv->type);
626 					panic();
627 				}
628 				found_types |= BIT(dt_drv->type);
629 
630 				/* Matching found for this driver, try next */
631 				break;
632 			}
633 		}
634 	}
635 
636 	return res;
637 }
638 
639 /*
640  * Add the node to the probe list if matching compatible drivers are found.
641  * Follow node's compatible property list ordering to find matching driver.
642  */
643 TEE_Result dt_driver_maybe_add_probe_node(const void *fdt, int node)
644 {
645 	int idx = 0;
646 	int len = 0;
647 	int count = 0;
648 	const char *compat = NULL;
649 	TEE_Result res = TEE_ERROR_GENERIC;
650 
651 	if (fdt_get_status(fdt, node) == DT_STATUS_DISABLED)
652 		return TEE_SUCCESS;
653 
654 	count = fdt_stringlist_count(fdt, node, "compatible");
655 	if (count < 0)
656 		return TEE_SUCCESS;
657 
658 	for (idx = 0; idx < count; idx++) {
659 		compat = fdt_stringlist_get(fdt, node, "compatible", idx, &len);
660 		assert(compat && len > 0);
661 
662 		res = add_probe_node_by_compat(fdt, node, compat);
663 
664 		/* Stop lookup if something was found */
665 		if (res != TEE_ERROR_ITEM_NOT_FOUND)
666 			return res;
667 	}
668 
669 	return TEE_SUCCESS;
670 }
671 
672 static void parse_node(const void *fdt, int node)
673 {
674 	TEE_Result __maybe_unused res = TEE_ERROR_GENERIC;
675 	int subnode = 0;
676 
677 	fdt_for_each_subnode(subnode, fdt, node) {
678 		res = dt_driver_maybe_add_probe_node(fdt, subnode);
679 		if (res) {
680 			EMSG("Failed on node %s with %#"PRIx32,
681 			     fdt_get_name(fdt, subnode, NULL), res);
682 			panic();
683 		}
684 
685 		/*
686 		 * Rescursively parse the FDT, skipping disabled nodes.
687 		 * FDT is expected reliable and core shall have sufficient
688 		 * stack depth to possibly parse all DT nodes.
689 		 */
690 		if (IS_ENABLED(CFG_DRIVERS_DT_RECURSIVE_PROBE)) {
691 			if (fdt_get_status(fdt, subnode) == DT_STATUS_DISABLED)
692 				continue;
693 
694 			parse_node(fdt, subnode);
695 		}
696 	}
697 }
698 
699 /*
700  * Parse FDT for nodes and save in probe list the node for which a dt_driver
701  * matches node's compatible property.
702  */
703 static TEE_Result probe_dt_drivers_early(void)
704 {
705 	TEE_Result res = TEE_ERROR_GENERIC;
706 	const void *fdt = NULL;
707 
708 	fdt = get_secure_dt();
709 	if (!fdt)
710 		return TEE_SUCCESS;
711 
712 	parse_node(fdt, fdt_path_offset(fdt, "/"));
713 
714 	res = process_probe_list(fdt);
715 	if (res == TEE_ERROR_DEFER_DRIVER_INIT) {
716 		DMSG("Deferred drivers probing");
717 		print_probe_list(fdt);
718 		res = TEE_SUCCESS;
719 	}
720 
721 	return res;
722 }
723 
724 static TEE_Result probe_dt_drivers(void)
725 {
726 	TEE_Result res = TEE_ERROR_GENERIC;
727 	const void *fdt = NULL;
728 
729 	fdt = get_secure_dt();
730 	if (!fdt)
731 		return TEE_SUCCESS;
732 
733 	res = process_probe_list(fdt);
734 	if (res || !TAILQ_EMPTY(&dt_driver_failed_list)) {
735 		EMSG("Probe sequence result: %#"PRIx32, res);
736 		print_probe_list(fdt);
737 	}
738 	if (res)
739 		panic();
740 
741 	return TEE_SUCCESS;
742 }
743 
744 early_init_late(probe_dt_drivers_early);
745 driver_init(probe_dt_drivers);
746 
747 static TEE_Result release_probe_lists(void)
748 {
749 	struct dt_driver_probe *elt = NULL;
750 	struct dt_driver_probe *next = NULL;
751 	struct dt_driver_provider *prov = NULL;
752 	struct dt_driver_provider *next_prov = NULL;
753 	const void *fdt = NULL;
754 
755 	fdt = get_secure_dt();
756 	if (!fdt)
757 		return TEE_SUCCESS;
758 
759 	assert(fdt && TAILQ_EMPTY(&dt_driver_probe_list));
760 
761 	TAILQ_FOREACH_SAFE(elt, &dt_driver_ready_list, link, next)
762 		free(elt);
763 
764 	TAILQ_FOREACH_SAFE(elt, &dt_driver_failed_list, link, next)
765 	       free(elt);
766 
767 	SLIST_FOREACH_SAFE(prov, &dt_driver_provider_list, link, next_prov)
768 	       free(prov);
769 
770 	return TEE_SUCCESS;
771 }
772 
773 release_init_resource(release_probe_lists);
774 
775 /*
776  * Simple bus support: handy to parse subnodes
777  */
778 static TEE_Result simple_bus_probe(const void *fdt, int node,
779 				   const void *compat_data __unused)
780 {
781 	TEE_Result res = TEE_ERROR_GENERIC;
782 	int subnode = 0;
783 
784 	fdt_for_each_subnode(subnode, fdt, node) {
785 		res = dt_driver_maybe_add_probe_node(fdt, subnode);
786 		if (res) {
787 			EMSG("Failed on node %s with %#"PRIx32,
788 			     fdt_get_name(fdt, subnode, NULL), res);
789 			panic();
790 		}
791 	}
792 
793 	return TEE_SUCCESS;
794 }
795 
796 static const struct dt_device_match simple_bus_match_table[] = {
797 	{ .compatible = "simple-bus" },
798 	{ }
799 };
800 
801 DEFINE_DT_DRIVER(simple_bus_dt_driver) = {
802 	.name = "simple-bus",
803 	.match_table = simple_bus_match_table,
804 	.probe = simple_bus_probe,
805 };
806