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 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 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 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 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 224 unsigned int dt_driver_provider_cells(struct dt_driver_provider *prv) 225 { 226 return prv->provider_cells; 227 } 228 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 * 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 * 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 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 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 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 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 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 */ 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 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 */ 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 */ 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 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 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 */ 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 */ 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 */ 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 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 */ 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 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 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 */ 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