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 + 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 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 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 */ 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 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 */ 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 */ 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 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 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 */ 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 */ 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 */ 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 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 */ 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 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 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 */ 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