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