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