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 + 1) * sizeof(*prop) > (size_t)len)
342 return TEE_ERROR_ITEM_NOT_FOUND;
343
344 phandle_node = fdt_node_offset_by_phandle(fdt, phandle);
345
346 return device_from_provider_prop(prv, fdt, phandle_node,
347 prop + prop_index, device_ref,
348 nodeoffs);
349 }
350
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)351 TEE_Result dt_driver_device_from_node_idx_prop(const char *prop_name,
352 const void *fdt, int nodeoffset,
353 unsigned int prop_idx,
354 enum dt_driver_type type,
355 void *device_ref)
356 {
357 int len = 0;
358 int idx = 0;
359 int idx32 = 0;
360 int prv_cells = 0;
361 int phandle_node = -1;
362 uint32_t phandle = 0;
363 const uint32_t *prop = NULL;
364 int consumer_node = nodeoffset;
365 struct dt_driver_provider *prv = NULL;
366
367 prop = fdt_getprop(fdt, nodeoffset, prop_name, &len);
368 if (!prop) {
369 DMSG("Property %s missing in node %s", prop_name,
370 fdt_get_name(fdt, nodeoffset, NULL));
371 return TEE_ERROR_ITEM_NOT_FOUND;
372 }
373
374 while (idx < len) {
375 idx32 = idx / sizeof(uint32_t);
376 phandle = fdt32_to_cpu(prop[idx32]);
377 if (!phandle) {
378 if (!prop_idx)
379 break;
380 idx += sizeof(phandle);
381 prop_idx--;
382 continue;
383 }
384
385 /*
386 * In some cases, pinctrl, i2c, nvmem, etc, the consumer phandle
387 * points directly to a subnode of the parent. In such cases,
388 * the provider does not have any "-cells" property and
389 * potentially no "phandle" property.
390 */
391 if (dt_driver_use_parent_controller(type)) {
392 phandle_node = fdt_node_offset_by_phandle(fdt, phandle);
393 if (phandle_node < 0)
394 return TEE_ERROR_GENERIC;
395
396 nodeoffset = fdt_parent_offset(fdt, phandle_node);
397 if (nodeoffset < 0)
398 return TEE_ERROR_GENERIC;
399
400 prv = dt_driver_get_provider_by_node(nodeoffset, type);
401 } else {
402 prv = dt_driver_get_provider_by_phandle(phandle, type);
403 if (prv)
404 phandle_node = prv->nodeoffset;
405 }
406
407 if (prv) {
408 prv_cells = dt_driver_provider_cells(prv);
409 } else if (prop_idx) {
410 /*
411 * When we need to skip another provider phandle
412 * arguments cells (aka when prop_idx != 0), we don't
413 * really need the skipped provider to be already
414 * registered, we can look straight in its DT node.
415 */
416 phandle_node = fdt_node_offset_by_phandle(fdt, phandle);
417 if (phandle_node < 0) {
418 DMSG("Can't find node for phandle %"PRIu32,
419 phandle);
420 return TEE_ERROR_GENERIC;
421 }
422
423 prv_cells = fdt_get_dt_driver_cells(fdt, phandle_node,
424 type);
425 if (prv_cells < 0) {
426 DMSG("Can't find cells count on node %s: %d",
427 fdt_get_name(fdt, phandle_node, NULL),
428 prv_cells);
429 return TEE_ERROR_GENERIC;
430 }
431 }
432
433 if (prop_idx) {
434 prop_idx--;
435 idx += sizeof(phandle) + prv_cells * sizeof(uint32_t);
436 continue;
437 }
438
439 if (!prv)
440 return TEE_ERROR_DEFER_DRIVER_INIT;
441
442 /* Skip property cell with the phandle, already handled */
443 idx32++;
444
445 return device_from_provider_prop(prv, fdt, phandle_node,
446 prop + idx32, device_ref,
447 consumer_node);
448 }
449
450 return TEE_ERROR_ITEM_NOT_FOUND;
451 }
452
print_probe_list(const void * fdt __maybe_unused)453 static void __maybe_unused print_probe_list(const void *fdt __maybe_unused)
454 {
455 struct dt_driver_probe *elt = NULL;
456 unsigned int __maybe_unused count = 0;
457
458 TAILQ_FOREACH(elt, &dt_driver_probe_list, link)
459 count++;
460
461 DMSG("Probe list: %u elements", count);
462 TAILQ_FOREACH(elt, &dt_driver_probe_list, link)
463 DMSG("|- Driver %s probes on node %s",
464 elt->dt_drv->name,
465 fdt_get_name(fdt, elt->nodeoffset, NULL));
466
467 DMSG("`- Probe list end");
468
469 count = 0;
470 TAILQ_FOREACH(elt, &dt_driver_failed_list, link)
471 count++;
472
473 DMSG("Failed list: %u elements", count);
474 TAILQ_FOREACH(elt, &dt_driver_failed_list, link)
475 EMSG("|- Driver %s on node %s failed", elt->dt_drv->name,
476 fdt_get_name(fdt, elt->nodeoffset, NULL));
477
478 DMSG("`- Failed list end");
479 }
480
481 /*
482 * Probe element: push to ready list if succeeds, push to probe list if probe
483 * if deferred, panic with an error trace otherwise.
484 */
probe_driver_node(const void * fdt,struct dt_driver_probe * elt)485 static TEE_Result probe_driver_node(const void *fdt,
486 struct dt_driver_probe *elt)
487 {
488 TEE_Result res = TEE_ERROR_GENERIC;
489 const char __maybe_unused *drv_name = NULL;
490 const char __maybe_unused *node_name = NULL;
491
492 node_name = fdt_get_name(fdt, elt->nodeoffset, NULL);
493 drv_name = elt->dt_drv->name;
494
495 if (!elt->dt_drv->probe) {
496 DMSG("No probe operator for driver %s, skipped", drv_name);
497 return TEE_SUCCESS;
498 }
499
500 FMSG("Probing %s on node %s", drv_name, node_name);
501
502 res = elt->dt_drv->probe(fdt, elt->nodeoffset, elt->dm->compat_data);
503 switch (res) {
504 case TEE_SUCCESS:
505 TAILQ_INSERT_HEAD(&dt_driver_ready_list, elt, link);
506
507 DMSG("element: %s on node %s initialized", drv_name, node_name);
508 break;
509 case TEE_ERROR_DEFER_DRIVER_INIT:
510 elt->deferrals++;
511 TAILQ_INSERT_TAIL(&dt_driver_probe_list, elt, link);
512
513 DMSG("element: %s on node %s deferred %u time(s)", drv_name,
514 node_name, elt->deferrals);
515 break;
516 case TEE_ERROR_NODE_DISABLED:
517 DMSG("element: %s on node %s is disabled", drv_name, node_name);
518 break;
519 default:
520 TAILQ_INSERT_HEAD(&dt_driver_failed_list, elt, link);
521
522 EMSG("Failed to probe %s on node %s: %#"PRIx32,
523 drv_name, node_name, res);
524 break;
525 }
526
527 return res;
528 }
529
alloc_elt_and_probe(const void * fdt,int node,const struct dt_driver * dt_drv,const struct dt_device_match * dm)530 static TEE_Result alloc_elt_and_probe(const void *fdt, int node,
531 const struct dt_driver *dt_drv,
532 const struct dt_device_match *dm)
533 {
534 struct dt_driver_probe *elt = NULL;
535
536 /* Will be freed when lists are released */
537 elt = calloc(1, sizeof(*elt));
538 if (!elt)
539 return TEE_ERROR_OUT_OF_MEMORY;
540
541 elt->nodeoffset = node;
542 elt->dt_drv = dt_drv;
543 elt->dm = dm;
544 elt->type = dt_drv->type;
545
546 return probe_driver_node(fdt, elt);
547 }
548
549 /* 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)550 static TEE_Result probe_device_by_compat(const void *fdt, int node,
551 const char *compat,
552 enum dt_driver_type type)
553 {
554 const struct dt_driver *drv = NULL;
555 const struct dt_device_match *dm = NULL;
556
557 for_each_dt_driver(drv) {
558 if (drv->type != type)
559 continue;
560
561 for (dm = drv->match_table; dm && dm->compatible; dm++)
562 if (strcmp(dm->compatible, compat) == 0)
563 return alloc_elt_and_probe(fdt, node, drv, dm);
564 }
565
566 return TEE_ERROR_ITEM_NOT_FOUND;
567 }
568
569 /*
570 * Lookup the best matching compatible driver, possibly of a specific @type,
571 * for the FDT node.
572 */
dt_driver_probe_device_by_node(const void * fdt,int nodeoffset,enum dt_driver_type type)573 TEE_Result dt_driver_probe_device_by_node(const void *fdt, int nodeoffset,
574 enum dt_driver_type type)
575 {
576 int idx = 0;
577 int len = 0;
578 int count = 0;
579 const char *compat = NULL;
580 TEE_Result res = TEE_ERROR_GENERIC;
581
582 assert_type_is_valid(type);
583
584 count = fdt_stringlist_count(fdt, nodeoffset, "compatible");
585 if (count < 0)
586 return TEE_ERROR_ITEM_NOT_FOUND;
587
588 for (idx = 0; idx < count; idx++) {
589 compat = fdt_stringlist_get(fdt, nodeoffset, "compatible",
590 idx, &len);
591 if (!compat)
592 return TEE_ERROR_GENERIC;
593
594 res = probe_device_by_compat(fdt, nodeoffset, compat, type);
595
596 if (res != TEE_ERROR_ITEM_NOT_FOUND)
597 return res;
598 }
599
600 return TEE_ERROR_ITEM_NOT_FOUND;
601 }
602
process_probe_list(const void * fdt)603 static TEE_Result process_probe_list(const void *fdt)
604 {
605 struct dt_driver_probe *elt = NULL;
606 struct dt_driver_probe *prev = NULL;
607 static unsigned int __maybe_unused loop_count;
608 static unsigned int __maybe_unused deferral_loop_count;
609 bool __maybe_unused one_deferred = false;
610 bool one_probed_ok = false;
611
612 do {
613 loop_count++;
614 FMSG("Probe loop %u after %u for deferral(s)", loop_count,
615 deferral_loop_count);
616
617 /* Hack here for TRACE_DEBUG messages on probe list elements */
618 if (TRACE_LEVEL >= TRACE_FLOW)
619 print_probe_list(fdt);
620
621 if (TAILQ_EMPTY(&dt_driver_probe_list))
622 return TEE_SUCCESS;
623
624 /*
625 * Probe from current end to top. Deferred probed node are
626 * pushed back after current tail for the next probe round.
627 * Reset probe result flags and see status after probe round.
628 */
629 one_deferred = false;
630 one_probed_ok = false;
631 added_node = false;
632
633 TAILQ_FOREACH_REVERSE_SAFE(elt, &dt_driver_probe_list,
634 dt_driver_probe_head, link, prev) {
635 TAILQ_REMOVE(&dt_driver_probe_list, elt, link);
636
637 switch (probe_driver_node(fdt, elt)) {
638 case TEE_SUCCESS:
639 one_probed_ok = true;
640 break;
641 case TEE_ERROR_DEFER_DRIVER_INIT:
642 one_deferred = true;
643 break;
644 default:
645 break;
646 }
647 }
648
649 if (one_deferred)
650 deferral_loop_count++;
651
652 } while (added_node || one_probed_ok);
653
654 DMSG("Unresolved dependencies after %u rounds, %u deferred",
655 loop_count, deferral_loop_count);
656
657 if (one_deferred)
658 return TEE_ERROR_DEFER_DRIVER_INIT;
659 else
660 return TEE_ERROR_GENERIC;
661 }
662
driver_probe_compare(struct dt_driver_probe * candidate,struct dt_driver_probe * elt)663 static int driver_probe_compare(struct dt_driver_probe *candidate,
664 struct dt_driver_probe *elt)
665 {
666 if (candidate->nodeoffset != elt->nodeoffset ||
667 candidate->type != elt->type)
668 return 1;
669
670 assert(elt->dt_drv == candidate->dt_drv);
671 return 0;
672 }
673
674 /*
675 * Return TEE_SUCCESS if compatible found
676 * TEE_ERROR_OUT_OF_MEMORY if heap is exhausted
677 */
add_node_to_probe(const void * fdt,int node,const struct dt_driver * dt_drv,const struct dt_device_match * dm)678 static TEE_Result add_node_to_probe(const void *fdt, int node,
679 const struct dt_driver *dt_drv,
680 const struct dt_device_match *dm)
681 {
682 const char __maybe_unused *node_name = fdt_get_name(fdt, node, NULL);
683 const char __maybe_unused *drv_name = dt_drv->name;
684 struct dt_driver_probe *elt = NULL;
685 struct dt_driver_probe elt_new = {
686 .dm = dm,
687 .dt_drv = dt_drv,
688 .nodeoffset = node,
689 .type = dt_drv->type,
690 };
691
692 /* If node/type found in probe list or ready list, nothing to do */
693 TAILQ_FOREACH(elt, &dt_driver_probe_list, link)
694 if (!driver_probe_compare(&elt_new, elt))
695 return TEE_SUCCESS;
696
697 TAILQ_FOREACH(elt, &dt_driver_ready_list, link)
698 if (!driver_probe_compare(&elt_new, elt))
699 return TEE_SUCCESS;
700
701 elt = malloc(sizeof(*elt));
702 if (!elt)
703 return TEE_ERROR_OUT_OF_MEMORY;
704
705 DMSG("element: %s on node %s", drv_name, node_name);
706
707 memcpy(elt, &elt_new, sizeof(*elt));
708
709 added_node = true;
710
711 TAILQ_INSERT_TAIL(&dt_driver_probe_list, elt, link);
712
713 /* Hack here for TRACE_DEBUG messages on current probe list elements */
714 if (TRACE_LEVEL >= TRACE_FLOW)
715 print_probe_list(fdt);
716
717 return TEE_SUCCESS;
718 }
719
720 /*
721 * Add a node to the probe list if a dt_driver matches target compatible.
722 *
723 * If @type is DT_DRIVER_ANY, probe list can hold only 1 driver to probe for
724 * the node. A node may probe several drivers if have a unique driver type.
725 *
726 * Return TEE_SUCCESS if compatible found
727 * TEE_ERROR_ITEM_NOT_FOUND if no matching driver
728 * TEE_ERROR_OUT_OF_MEMORY if heap is exhausted
729 */
add_probe_node_by_compat(const void * fdt,int node,const char * compat)730 static TEE_Result add_probe_node_by_compat(const void *fdt, int node,
731 const char *compat)
732 {
733 TEE_Result res = TEE_ERROR_ITEM_NOT_FOUND;
734 const struct dt_driver *dt_drv = NULL;
735 const struct dt_device_match *dm = NULL;
736 uint32_t found_types = 0;
737
738 for_each_dt_driver(dt_drv) {
739 for (dm = dt_drv->match_table; dm && dm->compatible; dm++) {
740 if (strcmp(dm->compatible, compat) == 0) {
741 assert(dt_drv->type < 32);
742
743 res = add_node_to_probe(fdt, node, dt_drv, dm);
744 if (res)
745 return res;
746
747 if (found_types & BIT(dt_drv->type)) {
748 EMSG("Driver %s multi hit on type %u",
749 dt_drv->name, dt_drv->type);
750 panic();
751 }
752 found_types |= BIT(dt_drv->type);
753
754 /* Matching found for this driver, try next */
755 break;
756 }
757 }
758 }
759
760 return res;
761 }
762
763 /*
764 * Add the node to the probe list if matching compatible drivers are found.
765 * Follow node's compatible property list ordering to find matching driver.
766 */
dt_driver_maybe_add_probe_node(const void * fdt,int node)767 TEE_Result dt_driver_maybe_add_probe_node(const void *fdt, int node)
768 {
769 int idx = 0;
770 int len = 0;
771 int count = 0;
772 const char *compat = NULL;
773 TEE_Result res = TEE_ERROR_GENERIC;
774
775 if (fdt_get_status(fdt, node) == DT_STATUS_DISABLED)
776 return TEE_SUCCESS;
777
778 count = fdt_stringlist_count(fdt, node, "compatible");
779 if (count < 0)
780 return TEE_SUCCESS;
781
782 for (idx = 0; idx < count; idx++) {
783 compat = fdt_stringlist_get(fdt, node, "compatible", idx, &len);
784 assert(compat && len > 0);
785
786 res = add_probe_node_by_compat(fdt, node, compat);
787
788 /* Stop lookup if something was found */
789 if (res != TEE_ERROR_ITEM_NOT_FOUND)
790 return res;
791 }
792
793 return TEE_SUCCESS;
794 }
795
parse_node(const void * fdt,int node)796 static void parse_node(const void *fdt, int node)
797 {
798 TEE_Result __maybe_unused res = TEE_ERROR_GENERIC;
799 int subnode = 0;
800
801 fdt_for_each_subnode(subnode, fdt, node) {
802 res = dt_driver_maybe_add_probe_node(fdt, subnode);
803 if (res) {
804 EMSG("Failed on node %s with %#"PRIx32,
805 fdt_get_name(fdt, subnode, NULL), res);
806 panic();
807 }
808
809 /*
810 * Rescursively parse the FDT, skipping disabled nodes.
811 * FDT is expected reliable and core shall have sufficient
812 * stack depth to possibly parse all DT nodes.
813 */
814 if (IS_ENABLED(CFG_DRIVERS_DT_RECURSIVE_PROBE)) {
815 if (fdt_get_status(fdt, subnode) == DT_STATUS_DISABLED)
816 continue;
817
818 parse_node(fdt, subnode);
819 }
820 }
821 }
822
823 /*
824 * Parse FDT for nodes and save in probe list the node for which a dt_driver
825 * matches node's compatible property.
826 */
probe_dt_drivers_early(void)827 static TEE_Result probe_dt_drivers_early(void)
828 {
829 TEE_Result res = TEE_ERROR_GENERIC;
830 const void *fdt = NULL;
831
832 fdt = get_secure_dt();
833 if (!fdt)
834 return TEE_SUCCESS;
835
836 parse_node(fdt, fdt_path_offset(fdt, "/"));
837
838 res = process_probe_list(fdt);
839 if (res == TEE_ERROR_DEFER_DRIVER_INIT) {
840 DMSG("Deferred drivers probing");
841 print_probe_list(fdt);
842 res = TEE_SUCCESS;
843 }
844
845 return res;
846 }
847
probe_dt_drivers(void)848 static TEE_Result probe_dt_drivers(void)
849 {
850 TEE_Result res = TEE_ERROR_GENERIC;
851 const void *fdt = NULL;
852
853 fdt = get_secure_dt();
854 if (!fdt)
855 return TEE_SUCCESS;
856
857 res = process_probe_list(fdt);
858 if (res || !TAILQ_EMPTY(&dt_driver_failed_list)) {
859 EMSG("Probe sequence result: %#"PRIx32, res);
860 print_probe_list(fdt);
861 panic();
862 }
863
864 return TEE_SUCCESS;
865 }
866
867 early_init_late(probe_dt_drivers_early);
868 driver_init(probe_dt_drivers);
869
release_probe_lists(void)870 static TEE_Result release_probe_lists(void)
871 {
872 struct dt_driver_probe *elt = NULL;
873 struct dt_driver_probe *next = NULL;
874 struct dt_driver_provider *prov = NULL;
875 struct dt_driver_provider *next_prov = NULL;
876 const void *fdt = NULL;
877
878 fdt = get_secure_dt();
879 if (!fdt)
880 return TEE_SUCCESS;
881
882 assert(fdt && TAILQ_EMPTY(&dt_driver_probe_list));
883
884 TAILQ_FOREACH_SAFE(elt, &dt_driver_ready_list, link, next)
885 free(elt);
886
887 TAILQ_FOREACH_SAFE(elt, &dt_driver_failed_list, link, next)
888 free(elt);
889
890 SLIST_FOREACH_SAFE(prov, &dt_driver_provider_list, link, next_prov)
891 free(prov);
892
893 return TEE_SUCCESS;
894 }
895
896 release_init_resource(release_probe_lists);
897
898 /*
899 * Simple bus support: handy to parse subnodes
900 */
simple_bus_probe(const void * fdt,int node,const void * compat_data __unused)901 static TEE_Result simple_bus_probe(const void *fdt, int node,
902 const void *compat_data __unused)
903 {
904 TEE_Result res = TEE_ERROR_GENERIC;
905 int subnode = 0;
906
907 fdt_for_each_subnode(subnode, fdt, node) {
908 res = dt_driver_maybe_add_probe_node(fdt, subnode);
909 if (res) {
910 EMSG("Failed on node %s with %#"PRIx32,
911 fdt_get_name(fdt, subnode, NULL), res);
912 panic();
913 }
914 }
915
916 return TEE_SUCCESS;
917 }
918
919 static const struct dt_device_match simple_bus_match_table[] = {
920 { .compatible = "simple-bus" },
921 { }
922 };
923
924 DEFINE_DT_DRIVER(simple_bus_dt_driver) = {
925 .name = "simple-bus",
926 .match_table = simple_bus_match_table,
927 .probe = simple_bus_probe,
928 };
929