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 static void assert_type_is_valid(enum dt_driver_type type) 86 { 87 switch (type) { 88 case DT_DRIVER_NOTYPE: 89 case DT_DRIVER_UART: 90 case DT_DRIVER_CLK: 91 return; 92 default: 93 assert(0); 94 } 95 } 96 97 /* 98 * Driver provider registering API functions 99 */ 100 101 TEE_Result dt_driver_register_provider(const void *fdt, int nodeoffset, 102 get_of_device_func get_of_device, 103 void *priv, enum dt_driver_type type) 104 { 105 struct dt_driver_provider *prv = NULL; 106 int provider_cells = 0; 107 uint32_t phandle = 0; 108 109 assert_type_is_valid(type); 110 111 provider_cells = fdt_get_dt_driver_cells(fdt, nodeoffset, type); 112 if (provider_cells < 0) { 113 DMSG("Failed to find provider cells: %d", provider_cells); 114 return TEE_ERROR_GENERIC; 115 } 116 117 phandle = fdt_get_phandle(fdt, nodeoffset); 118 if (!phandle || phandle == (uint32_t)-1) { 119 DMSG("Failed to find provide phandle"); 120 return TEE_ERROR_GENERIC; 121 } 122 123 prv = calloc(1, sizeof(*prv)); 124 if (!prv) 125 return TEE_ERROR_OUT_OF_MEMORY; 126 127 prv->nodeoffset = nodeoffset; 128 prv->type = type; 129 prv->provider_cells = provider_cells; 130 prv->phandle = phandle; 131 prv->get_of_device = get_of_device; 132 prv->priv_data = priv; 133 134 SLIST_INSERT_HEAD(&dt_driver_provider_list, prv, link); 135 136 return TEE_SUCCESS; 137 } 138 139 /* 140 * Helper functions for dt_drivers querying driver provider information 141 */ 142 143 int fdt_get_dt_driver_cells(const void *fdt, int nodeoffset, 144 enum dt_driver_type type) 145 { 146 const char *cells_name = NULL; 147 const fdt32_t *c = NULL; 148 int len = 0; 149 150 switch (type) { 151 case DT_DRIVER_CLK: 152 cells_name = "#clock-cells"; 153 break; 154 default: 155 panic(); 156 } 157 158 c = fdt_getprop(fdt, nodeoffset, cells_name, &len); 159 if (!c) 160 return len; 161 162 if (len != sizeof(*c)) 163 return -FDT_ERR_BADNCELLS; 164 165 return fdt32_to_cpu(*c); 166 } 167 168 unsigned int dt_driver_provider_cells(struct dt_driver_provider *prv) 169 { 170 return prv->provider_cells; 171 } 172 173 struct dt_driver_provider * 174 dt_driver_get_provider_by_node(int nodeoffset, enum dt_driver_type type) 175 { 176 struct dt_driver_provider *prv = NULL; 177 178 SLIST_FOREACH(prv, &dt_driver_provider_list, link) 179 if (prv->nodeoffset == nodeoffset && prv->type == type) 180 return prv; 181 182 return NULL; 183 } 184 185 struct dt_driver_provider * 186 dt_driver_get_provider_by_phandle(uint32_t phandle, enum dt_driver_type type) 187 { 188 struct dt_driver_provider *prv = NULL; 189 190 SLIST_FOREACH(prv, &dt_driver_provider_list, link) 191 if (prv->phandle == phandle && prv->type == type) 192 return prv; 193 194 return NULL; 195 } 196 197 static void *device_from_provider_prop(struct dt_driver_provider *prv, 198 const uint32_t *prop, 199 TEE_Result *res) 200 { 201 struct dt_driver_phandle_args *pargs = NULL; 202 unsigned int n = 0; 203 void *device = NULL; 204 205 pargs = calloc(1, prv->provider_cells * sizeof(uint32_t *) + 206 sizeof(*pargs)); 207 if (!pargs) { 208 *res = TEE_ERROR_OUT_OF_MEMORY; 209 return NULL; 210 } 211 212 pargs->args_count = prv->provider_cells; 213 for (n = 0; n < prv->provider_cells; n++) 214 pargs->args[n] = fdt32_to_cpu(prop[n + 1]); 215 216 device = prv->get_of_device(pargs, prv->priv_data, res); 217 218 free(pargs); 219 220 return device; 221 } 222 223 void *dt_driver_device_from_node_idx_prop(const char *prop_name, 224 const void *fdt, int nodeoffset, 225 unsigned int prop_idx, 226 enum dt_driver_type type, 227 TEE_Result *res) 228 { 229 int len = 0; 230 int idx = 0; 231 int idx32 = 0; 232 int prv_cells = 0; 233 uint32_t phandle = 0; 234 const uint32_t *prop = NULL; 235 struct dt_driver_provider *prv = NULL; 236 237 prop = fdt_getprop(fdt, nodeoffset, prop_name, &len); 238 if (!prop) { 239 DMSG("Property %s missing in node %s", prop_name, 240 fdt_get_name(fdt, nodeoffset, NULL)); 241 *res = TEE_ERROR_GENERIC; 242 return NULL; 243 } 244 245 while (idx < len) { 246 idx32 = idx / sizeof(uint32_t); 247 phandle = fdt32_to_cpu(prop[idx32]); 248 249 prv = dt_driver_get_provider_by_phandle(phandle, type); 250 if (!prv) { 251 /* No provider registered yet */ 252 *res = TEE_ERROR_DEFER_DRIVER_INIT; 253 return NULL; 254 } 255 256 prv_cells = dt_driver_provider_cells(prv); 257 if (prop_idx) { 258 prop_idx--; 259 idx += sizeof(phandle) + prv_cells * sizeof(uint32_t); 260 continue; 261 } 262 263 return device_from_provider_prop(prv, prop + idx32, res); 264 } 265 266 *res = TEE_ERROR_GENERIC; 267 return NULL; 268 } 269 270 static unsigned int __maybe_unused probe_list_count(void) 271 { 272 struct dt_driver_probe *elt = NULL; 273 unsigned int count = 0; 274 275 TAILQ_FOREACH(elt, &dt_driver_probe_list, link) 276 count++; 277 278 return count; 279 } 280 281 static void __maybe_unused print_probe_list(const void *fdt __maybe_unused) 282 { 283 struct dt_driver_probe *elt = NULL; 284 285 DMSG("Probe list: %u elements", probe_list_count()); 286 287 TAILQ_FOREACH(elt, &dt_driver_probe_list, link) 288 DMSG("- Driver %s probes on node %s", 289 elt->dt_drv->name, 290 fdt_get_name(fdt, elt->nodeoffset, NULL)); 291 292 DMSG("Probe list end"); 293 } 294 295 /* 296 * Probe element: push to ready list if succeeds, push to probe list if probe 297 * if deferred, panic with an error trace otherwise. 298 */ 299 static TEE_Result probe_driver_node(const void *fdt, 300 struct dt_driver_probe *elt) 301 { 302 TEE_Result res = TEE_ERROR_GENERIC; 303 const char __maybe_unused *drv_name = NULL; 304 const char __maybe_unused *node_name = NULL; 305 306 node_name = fdt_get_name(fdt, elt->nodeoffset, NULL); 307 drv_name = elt->dt_drv->name; 308 309 if (!elt->dt_drv->probe) { 310 DMSG("No probe operator for driver %s, skipped", drv_name); 311 return TEE_SUCCESS; 312 } 313 314 FMSG("Probing %s on node %s", drv_name, node_name); 315 316 res = elt->dt_drv->probe(fdt, elt->nodeoffset, elt->dm->compat_data); 317 switch (res) { 318 case TEE_SUCCESS: 319 TAILQ_INSERT_HEAD(&dt_driver_ready_list, elt, link); 320 321 DMSG("element: %s on node %s initialized", drv_name, node_name); 322 break; 323 case TEE_ERROR_DEFER_DRIVER_INIT: 324 elt->deferrals++; 325 TAILQ_INSERT_TAIL(&dt_driver_probe_list, elt, link); 326 327 DMSG("element: %s on node %s deferred %u time(s)", drv_name, 328 node_name, elt->deferrals); 329 break; 330 default: 331 TAILQ_INSERT_HEAD(&dt_driver_failed_list, elt, link); 332 333 EMSG("Failed to probe %s on node %s: %#"PRIx32, 334 drv_name, node_name, res); 335 break; 336 } 337 338 return res; 339 } 340 341 static TEE_Result alloc_elt_and_probe(const void *fdt, int node, 342 const struct dt_driver *dt_drv, 343 const struct dt_device_match *dm) 344 { 345 struct dt_driver_probe *elt = NULL; 346 347 /* Will be freed when lists are released */ 348 elt = calloc(1, sizeof(*elt)); 349 if (!elt) 350 return TEE_ERROR_OUT_OF_MEMORY; 351 352 elt->nodeoffset = node; 353 elt->dt_drv = dt_drv; 354 elt->dm = dm; 355 elt->type = dt_drv->type; 356 357 return probe_driver_node(fdt, elt); 358 } 359 360 /* Lookup a compatible driver, possibly of a specific @type, for the FDT node */ 361 static TEE_Result probe_device_by_compat(const void *fdt, int node, 362 const char *compat, 363 enum dt_driver_type type) 364 { 365 const struct dt_driver *drv = NULL; 366 const struct dt_device_match *dm = NULL; 367 368 for_each_dt_driver(drv) { 369 if (drv->type != type) 370 continue; 371 372 for (dm = drv->match_table; dm && dm->compatible; dm++) 373 if (strcmp(dm->compatible, compat) == 0) 374 return alloc_elt_and_probe(fdt, node, drv, dm); 375 } 376 377 return TEE_ERROR_ITEM_NOT_FOUND; 378 } 379 380 /* 381 * Lookup the best matching compatible driver, possibly of a specific @type, 382 * for the FDT node. 383 */ 384 TEE_Result dt_driver_probe_device_by_node(const void *fdt, int nodeoffset, 385 enum dt_driver_type type) 386 { 387 int idx = 0; 388 int len = 0; 389 int count = 0; 390 const char *compat = NULL; 391 TEE_Result res = TEE_ERROR_GENERIC; 392 393 assert_type_is_valid(type); 394 395 count = fdt_stringlist_count(fdt, nodeoffset, "compatible"); 396 if (count < 0) 397 return TEE_ERROR_ITEM_NOT_FOUND; 398 399 for (idx = 0; idx < count; idx++) { 400 compat = fdt_stringlist_get(fdt, nodeoffset, "compatible", 401 idx, &len); 402 if (!compat) 403 return TEE_ERROR_GENERIC; 404 405 res = probe_device_by_compat(fdt, nodeoffset, compat, type); 406 407 if (res != TEE_ERROR_ITEM_NOT_FOUND) 408 return res; 409 } 410 411 return TEE_ERROR_ITEM_NOT_FOUND; 412 } 413 414 static TEE_Result process_probe_list(const void *fdt) 415 { 416 struct dt_driver_probe *elt = NULL; 417 struct dt_driver_probe *prev = NULL; 418 unsigned int __maybe_unused loop_count = 0; 419 unsigned int __maybe_unused deferral_loop_count = 0; 420 bool __maybe_unused one_deferred = false; 421 bool one_probed_ok = false; 422 423 do { 424 loop_count++; 425 FMSG("Probe loop %u after %u for deferral(s)", loop_count, 426 deferral_loop_count); 427 428 /* Hack here for TRACE_DEBUG messages on probe list elements */ 429 if (TRACE_LEVEL >= TRACE_FLOW) 430 print_probe_list(fdt); 431 432 if (TAILQ_EMPTY(&dt_driver_probe_list)) 433 return TEE_SUCCESS; 434 435 /* 436 * Probe from current end to top. Deferred probed node are 437 * pushed back after current tail for the next probe round. 438 * Reset probe result flags and see status after probe round. 439 */ 440 one_deferred = false; 441 one_probed_ok = false; 442 added_node = false; 443 444 TAILQ_FOREACH_REVERSE_SAFE(elt, &dt_driver_probe_list, 445 dt_driver_probe_head, link, prev) { 446 TAILQ_REMOVE(&dt_driver_probe_list, elt, link); 447 448 switch (probe_driver_node(fdt, elt)) { 449 case TEE_SUCCESS: 450 one_probed_ok = true; 451 break; 452 case TEE_ERROR_DEFER_DRIVER_INIT: 453 one_deferred = true; 454 break; 455 default: 456 break; 457 } 458 } 459 460 if (one_deferred) 461 deferral_loop_count++; 462 463 } while (added_node || one_probed_ok); 464 465 EMSG("Panic on unresolved dependencies after %u rounds, %u deferred:", 466 loop_count, deferral_loop_count); 467 468 TAILQ_FOREACH(elt, &dt_driver_probe_list, link) 469 EMSG("- Pending: driver %s on node %s", elt->dt_drv->name, 470 fdt_get_name(fdt, elt->nodeoffset, NULL)); 471 472 TAILQ_FOREACH(elt, &dt_driver_failed_list, link) 473 EMSG("- Failed: driver %s on node %s", elt->dt_drv->name, 474 fdt_get_name(fdt, elt->nodeoffset, NULL)); 475 476 panic(); 477 } 478 479 static int driver_probe_compare(struct dt_driver_probe *candidate, 480 struct dt_driver_probe *elt) 481 { 482 if (candidate->nodeoffset != elt->nodeoffset || 483 candidate->type != elt->type) 484 return 1; 485 486 assert(elt->dt_drv == candidate->dt_drv); 487 return 0; 488 } 489 490 /* 491 * Return TEE_SUCCESS if compatible found 492 * TEE_ERROR_OUT_OF_MEMORY if heap is exhausted 493 */ 494 static TEE_Result add_node_to_probe(const void *fdt, int node, 495 const struct dt_driver *dt_drv, 496 const struct dt_device_match *dm) 497 { 498 const char __maybe_unused *node_name = fdt_get_name(fdt, node, NULL); 499 const char __maybe_unused *drv_name = dt_drv->name; 500 struct dt_driver_probe *elt = NULL; 501 struct dt_driver_probe elt_new = { 502 .dm = dm, 503 .dt_drv = dt_drv, 504 .nodeoffset = node, 505 .type = dt_drv->type, 506 }; 507 508 /* If node/type found in probe list or ready list, nothing to do */ 509 TAILQ_FOREACH(elt, &dt_driver_probe_list, link) 510 if (!driver_probe_compare(&elt_new, elt)) 511 return TEE_SUCCESS; 512 513 TAILQ_FOREACH(elt, &dt_driver_ready_list, link) 514 if (!driver_probe_compare(&elt_new, elt)) 515 return TEE_SUCCESS; 516 517 elt = malloc(sizeof(*elt)); 518 if (!elt) 519 return TEE_ERROR_OUT_OF_MEMORY; 520 521 DMSG("element: %s on node %s", node_name, drv_name); 522 523 memcpy(elt, &elt_new, sizeof(*elt)); 524 525 added_node = true; 526 527 TAILQ_INSERT_TAIL(&dt_driver_probe_list, elt, link); 528 529 /* Hack here for TRACE_DEBUG messages on current probe list elements */ 530 if (TRACE_LEVEL >= TRACE_FLOW) 531 print_probe_list(fdt); 532 533 return TEE_SUCCESS; 534 } 535 536 /* 537 * Add a node to the probe list if a dt_driver matches target compatible. 538 * 539 * If @type is DT_DRIVER_ANY, probe list can hold only 1 driver to probe for 540 * the node. A node may probe several drivers if have a unique driver type. 541 * 542 * Return TEE_SUCCESS if compatible found 543 * TEE_ERROR_ITEM_NOT_FOUND if no matching driver 544 * TEE_ERROR_OUT_OF_MEMORY if heap is exhausted 545 */ 546 static TEE_Result add_probe_node_by_compat(const void *fdt, int node, 547 const char *compat) 548 { 549 TEE_Result res = TEE_ERROR_ITEM_NOT_FOUND; 550 const struct dt_driver *dt_drv = NULL; 551 const struct dt_device_match *dm = NULL; 552 uint32_t found_types = 0; 553 554 for_each_dt_driver(dt_drv) { 555 for (dm = dt_drv->match_table; dm && dm->compatible; dm++) { 556 if (strcmp(dm->compatible, compat) == 0) { 557 assert(dt_drv->type < 32); 558 559 res = add_node_to_probe(fdt, node, dt_drv, dm); 560 if (res) 561 return res; 562 563 if (found_types & BIT(dt_drv->type)) { 564 EMSG("Driver %s multi hit on type %u", 565 dt_drv->name, dt_drv->type); 566 panic(); 567 } 568 found_types |= BIT(dt_drv->type); 569 570 /* Matching found for this driver, try next */ 571 break; 572 } 573 } 574 } 575 576 return res; 577 } 578 579 /* 580 * Add the node to the probe list if matching compatible drivers are found. 581 * Follow node's compatible property list ordering to find matching driver. 582 */ 583 TEE_Result dt_driver_maybe_add_probe_node(const void *fdt, int node) 584 { 585 int idx = 0; 586 int len = 0; 587 int count = 0; 588 const char *compat = NULL; 589 TEE_Result res = TEE_ERROR_GENERIC; 590 591 if (_fdt_get_status(fdt, node) == DT_STATUS_DISABLED) 592 return TEE_SUCCESS; 593 594 count = fdt_stringlist_count(fdt, node, "compatible"); 595 if (count < 0) 596 return TEE_SUCCESS; 597 598 for (idx = 0; idx < count; idx++) { 599 compat = fdt_stringlist_get(fdt, node, "compatible", idx, &len); 600 assert(compat && len > 0); 601 602 res = add_probe_node_by_compat(fdt, node, compat); 603 604 /* Stop lookup if something was found */ 605 if (res != TEE_ERROR_ITEM_NOT_FOUND) 606 return res; 607 } 608 609 return TEE_SUCCESS; 610 } 611 612 static void parse_node(const void *fdt, int node) 613 { 614 TEE_Result __maybe_unused res = TEE_ERROR_GENERIC; 615 int subnode = 0; 616 617 fdt_for_each_subnode(subnode, fdt, node) { 618 res = dt_driver_maybe_add_probe_node(fdt, subnode); 619 if (res) { 620 EMSG("Failed on node %s with %#"PRIx32, 621 fdt_get_name(fdt, subnode, NULL), res); 622 panic(); 623 } 624 625 /* 626 * Rescursively parse the FDT, skipping disabled nodes. 627 * FDT is expected reliable and core shall have sufficient 628 * stack depth to possibly parse all DT nodes. 629 */ 630 if (IS_ENABLED(CFG_DRIVERS_DT_RECURSIVE_PROBE)) { 631 if (_fdt_get_status(fdt, subnode) == DT_STATUS_DISABLED) 632 continue; 633 634 parse_node(fdt, subnode); 635 } 636 } 637 } 638 639 /* 640 * Parse FDT for nodes and save in probe list the node for which a dt_driver 641 * matches node's compatible property. 642 */ 643 static TEE_Result probe_dt_drivers(void) 644 { 645 const void *fdt = NULL; 646 647 if (!IS_ENABLED(CFG_EMBED_DTB)) 648 return TEE_SUCCESS; 649 650 fdt = get_embedded_dt(); 651 assert(fdt); 652 653 parse_node(fdt, fdt_path_offset(fdt, "/")); 654 655 return process_probe_list(fdt); 656 } 657 658 driver_init(probe_dt_drivers); 659 660 static TEE_Result release_probe_lists(void) 661 { 662 struct dt_driver_probe *elt = NULL; 663 struct dt_driver_probe *next = NULL; 664 struct dt_driver_provider *prov = NULL; 665 struct dt_driver_provider *next_prov = NULL; 666 const void * __maybe_unused fdt = NULL; 667 668 if (!IS_ENABLED(CFG_EMBED_DTB)) 669 return TEE_SUCCESS; 670 671 fdt = get_embedded_dt(); 672 673 assert(fdt && TAILQ_EMPTY(&dt_driver_probe_list)); 674 675 TAILQ_FOREACH_SAFE(elt, &dt_driver_ready_list, link, next) 676 free(elt); 677 678 TAILQ_FOREACH_SAFE(elt, &dt_driver_failed_list, link, next) 679 free(elt); 680 681 SLIST_FOREACH_SAFE(prov, &dt_driver_provider_list, link, next_prov) 682 free(prov); 683 684 return TEE_SUCCESS; 685 } 686 687 release_init_resource(release_probe_lists); 688 689 /* 690 * Simple bus support: handy to parse subnodes 691 */ 692 static TEE_Result simple_bus_probe(const void *fdt, int node, 693 const void *compat_data __unused) 694 { 695 TEE_Result res = TEE_ERROR_GENERIC; 696 int subnode = 0; 697 698 fdt_for_each_subnode(subnode, fdt, node) { 699 res = dt_driver_maybe_add_probe_node(fdt, subnode); 700 if (res) { 701 EMSG("Failed on node %s with %#"PRIx32, 702 fdt_get_name(fdt, subnode, NULL), res); 703 panic(); 704 } 705 } 706 707 return TEE_SUCCESS; 708 } 709 710 static const struct dt_device_match simple_bus_match_table[] = { 711 { .compatible = "simple-bus" }, 712 { } 713 }; 714 715 const struct dt_driver simple_bus_dt_driver __dt_driver = { 716 .name = "simple-bus", 717 .match_table = simple_bus_match_table, 718 .probe = simple_bus_probe, 719 }; 720