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