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