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