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