1 /* $OpenBSD: fdt.c,v 1.4 2023/02/13 16:16:03 kettenis Exp $ */ 2 3 /* 4 * Copyright (c) 2009 Dariusz Swiderski <sfires@sfires.net> 5 * Copyright (c) 2009, 2016 Mark Kettenis <kettenis@openbsd.org> 6 * 7 * Permission to use, copy, modify, and distribute this software for any 8 * purpose with or without fee is hereby granted, provided that the above 9 * copyright notice and this permission notice appear in all copies. 10 * 11 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 12 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 13 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 14 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 15 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 16 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 17 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 18 */ 19 20 #include <sys/param.h> 21 #include <sys/systm.h> 22 23 #include <lib/libkern/libkern.h> 24 25 #include "fdt.h" 26 27 unsigned int fdt_check_head(void *); 28 char *fdt_get_str(uint32_t); 29 void *skip_property(uint32_t *); 30 void *skip_props(uint32_t *); 31 void *skip_node_name(uint32_t *); 32 void *skip_node(void *); 33 void *skip_nops(uint32_t *); 34 void *fdt_parent_node_recurse(void *, void *); 35 36 static int tree_inited = 0; 37 static struct fdt tree; 38 39 unsigned int 40 fdt_check_head(void *fdt) 41 { 42 struct fdt_head *fh; 43 uint32_t *ptr, *tok; 44 45 fh = fdt; 46 ptr = (uint32_t *)fdt; 47 48 if (betoh32(fh->fh_magic) != FDT_MAGIC) 49 return 0; 50 51 if (betoh32(fh->fh_version) > FDT_CODE_VERSION) 52 return 0; 53 54 tok = skip_nops(ptr + (betoh32(fh->fh_struct_off) / 4)); 55 if (betoh32(*tok) != FDT_NODE_BEGIN) 56 return 0; 57 58 /* check for end signature on version 17 blob */ 59 if ((betoh32(fh->fh_version) >= 17) && 60 (betoh32(*(ptr + (betoh32(fh->fh_struct_off) / 4) + 61 (betoh32(fh->fh_struct_size) / 4) - 1)) != FDT_END)) 62 return 0; 63 64 return betoh32(fh->fh_version); 65 } 66 67 /* 68 * Initializes internal structures of module. 69 * Has to be called once. 70 */ 71 int 72 fdt_init(void *fdt) 73 { 74 int version; 75 76 memset(&tree, 0, sizeof(struct fdt)); 77 tree_inited = 0; 78 79 if (!fdt) 80 return 0; 81 82 if (!(version = fdt_check_head(fdt))) 83 return 0; 84 85 tree.header = (struct fdt_head *)fdt; 86 tree.tree = (char *)fdt + betoh32(tree.header->fh_struct_off); 87 tree.strings = (char *)fdt + betoh32(tree.header->fh_strings_off); 88 tree.memory = (char *)fdt + betoh32(tree.header->fh_reserve_off); 89 tree.end = (char *)fdt + betoh32(tree.header->fh_size); 90 tree.version = version; 91 tree.strings_size = betoh32(tree.header->fh_strings_size); 92 if (tree.version >= 17) 93 tree.struct_size = betoh32(tree.header->fh_struct_size); 94 tree_inited = 1; 95 96 return version; 97 } 98 99 void 100 fdt_finalize(void) 101 { 102 char *start = (char *)tree.header; 103 104 tree.header->fh_size = htobe32(tree.end - start); 105 tree.header->fh_struct_off = htobe32(tree.tree - start); 106 tree.header->fh_strings_off = htobe32(tree.strings - start); 107 tree.header->fh_reserve_off = htobe32(tree.memory - start); 108 tree.header->fh_strings_size = htobe32(tree.strings_size); 109 if (tree.version >= 17) 110 tree.header->fh_struct_size = htobe32(tree.struct_size); 111 } 112 113 /* 114 * Return the size of the FDT. 115 */ 116 size_t 117 fdt_get_size(void *fdt) 118 { 119 if (!fdt) 120 return 0; 121 122 if (!fdt_check_head(fdt)) 123 return 0; 124 125 return betoh32(((struct fdt_head *)fdt)->fh_size); 126 } 127 128 /* 129 * Retrieve string pointer from strings table. 130 */ 131 char * 132 fdt_get_str(uint32_t num) 133 { 134 if (num > tree.strings_size) 135 return NULL; 136 return (tree.strings) ? (tree.strings + num) : NULL; 137 } 138 139 int 140 fdt_add_str(char *name) 141 { 142 size_t len = roundup(strlen(name) + 1, sizeof(uint32_t)); 143 char *end = tree.strings + tree.strings_size; 144 145 if (end + len > tree.end) 146 panic("FDT overflow"); 147 148 tree.strings_size += len; 149 memset(end, 0, len); 150 memcpy(end, name, strlen(name)); 151 152 return (end - tree.strings); 153 } 154 155 /* 156 * Utility functions for skipping parts of tree. 157 */ 158 159 void * 160 skip_nops(uint32_t *ptr) 161 { 162 while (betoh32(*ptr) == FDT_NOP) 163 ptr++; 164 165 return ptr; 166 } 167 168 void * 169 skip_property(uint32_t *ptr) 170 { 171 uint32_t size; 172 173 size = betoh32(*(ptr + 1)); 174 /* move forward by magic + size + nameid + rounded up property size */ 175 ptr += 3 + roundup(size, sizeof(uint32_t)) / sizeof(uint32_t); 176 177 return skip_nops(ptr); 178 } 179 180 void * 181 skip_props(uint32_t *ptr) 182 { 183 while (betoh32(*ptr) == FDT_PROPERTY) { 184 ptr = skip_property(ptr); 185 } 186 return ptr; 187 } 188 189 void * 190 skip_node_name(uint32_t *ptr) 191 { 192 /* skip name, aligned to 4 bytes, this is NULL term., so must add 1 */ 193 ptr += roundup(strlen((char *)ptr) + 1, 194 sizeof(uint32_t)) / sizeof(uint32_t); 195 196 return skip_nops(ptr); 197 } 198 199 /* 200 * Retrieves node property, the returned pointer is inside the fdt tree, 201 * so we should not modify content pointed by it directly. 202 */ 203 int 204 fdt_node_property(void *node, char *name, char **out) 205 { 206 uint32_t *ptr; 207 uint32_t nameid; 208 char *tmp; 209 210 if (!tree_inited) 211 return 0; 212 213 ptr = (uint32_t *)node; 214 215 if (betoh32(*ptr) != FDT_NODE_BEGIN) 216 return 0; 217 218 ptr = skip_node_name(ptr + 1); 219 220 while (betoh32(*ptr) == FDT_PROPERTY) { 221 nameid = betoh32(*(ptr + 2)); /* id of name in strings table */ 222 tmp = fdt_get_str(nameid); 223 if (!strcmp(name, tmp)) { 224 *out = (char *)(ptr + 3); /* beginning of the value */ 225 return betoh32(*(ptr + 1)); /* size of value */ 226 } 227 ptr = skip_property(ptr); 228 } 229 return 0; 230 } 231 232 int 233 fdt_node_set_property(void *node, char *name, void *data, int len) 234 { 235 char *end = tree.strings + tree.strings_size; 236 uint32_t *ptr, *next; 237 uint32_t nameid; 238 uint32_t curlen; 239 size_t delta; 240 char *tmp; 241 242 if (!tree_inited) 243 return 0; 244 245 ptr = (uint32_t *)node; 246 247 if (betoh32(*ptr) != FDT_NODE_BEGIN) 248 return 0; 249 250 ptr = skip_node_name(ptr + 1); 251 252 while (betoh32(*ptr) == FDT_PROPERTY) { 253 nameid = betoh32(*(ptr + 2)); /* id of name in strings table */ 254 tmp = fdt_get_str(nameid); 255 next = skip_property(ptr); 256 if (!strcmp(name, tmp)) { 257 curlen = betoh32(*(ptr + 1)); 258 delta = roundup(len, sizeof(uint32_t)) - 259 roundup(curlen, sizeof(uint32_t)); 260 if (end + delta > tree.end) 261 panic("FDT overflow"); 262 263 memmove((char *)next + delta, next, 264 end - (char *)next); 265 tree.struct_size += delta; 266 tree.strings += delta; 267 *(ptr + 1) = htobe32(len); 268 memcpy(ptr + 3, data, len); 269 return 1; 270 } 271 ptr = next; 272 } 273 return 0; 274 } 275 276 int 277 fdt_node_add_property(void *node, char *name, void *data, int len) 278 { 279 char *end = tree.strings + tree.strings_size; 280 char *dummy; 281 282 if (!tree_inited) 283 return 0; 284 285 if (!fdt_node_property(node, name, &dummy)) { 286 uint32_t *ptr = (uint32_t *)node; 287 288 if (betoh32(*ptr) != FDT_NODE_BEGIN) 289 return 0; 290 291 if (end + 3 * sizeof(uint32_t) > tree.end) 292 panic("FDT overflow"); 293 294 ptr = skip_node_name(ptr + 1); 295 296 memmove(ptr + 3, ptr, end - (char *)ptr); 297 tree.struct_size += 3 * sizeof(uint32_t); 298 tree.strings += 3 * sizeof(uint32_t); 299 *ptr++ = htobe32(FDT_PROPERTY); 300 *ptr++ = htobe32(0); 301 *ptr++ = htobe32(fdt_add_str(name)); 302 } 303 304 return fdt_node_set_property(node, name, data, len); 305 } 306 307 int 308 fdt_node_add_node(void *node, char *name, void **child) 309 { 310 size_t len = roundup(strlen(name) + 1, sizeof(uint32_t)) + 8; 311 char *end = tree.strings + tree.strings_size; 312 uint32_t *ptr = (uint32_t *)node; 313 314 if (!tree_inited) 315 return 0; 316 317 if (betoh32(*ptr) != FDT_NODE_BEGIN) 318 return 0; 319 320 if (end + len > tree.end) 321 panic("FDT overflow"); 322 323 ptr = skip_node_name(ptr + 1); 324 ptr = skip_props(ptr); 325 326 /* skip children */ 327 while (betoh32(*ptr) == FDT_NODE_BEGIN) 328 ptr = skip_node(ptr); 329 330 memmove((char *)ptr + len, ptr, end - (char *)ptr); 331 tree.struct_size += len; 332 tree.strings += len; 333 334 *child = ptr; 335 *ptr++ = htobe32(FDT_NODE_BEGIN); 336 memset(ptr, 0, len - 8); 337 memcpy(ptr, name, strlen(name)); 338 ptr += (len - 8) / sizeof(uint32_t); 339 *ptr++ = htobe32(FDT_NODE_END); 340 341 return 1; 342 } 343 344 /* 345 * Retrieves next node, skipping all the children nodes of the pointed node, 346 * returns pointer to next node, no matter if it exists or not. 347 */ 348 void * 349 skip_node(void *node) 350 { 351 uint32_t *ptr = node; 352 353 ptr++; 354 355 ptr = skip_node_name(ptr); 356 ptr = skip_props(ptr); 357 358 /* skip children */ 359 while (betoh32(*ptr) == FDT_NODE_BEGIN) 360 ptr = skip_node(ptr); 361 362 return skip_nops(ptr + 1); 363 } 364 365 /* 366 * Retrieves next node, skipping all the children nodes of the pointed node, 367 * returns pointer to next node if exists, otherwise returns NULL. 368 * If passed 0 will return first node of the tree (root). 369 */ 370 void * 371 fdt_next_node(void *node) 372 { 373 uint32_t *ptr; 374 375 if (!tree_inited) 376 return NULL; 377 378 ptr = node; 379 380 if (node == NULL) { 381 ptr = skip_nops((uint32_t *)tree.tree); 382 return (betoh32(*ptr) == FDT_NODE_BEGIN) ? ptr : NULL; 383 } 384 385 if (betoh32(*ptr) != FDT_NODE_BEGIN) 386 return NULL; 387 388 ptr++; 389 390 ptr = skip_node_name(ptr); 391 ptr = skip_props(ptr); 392 393 /* skip children */ 394 while (betoh32(*ptr) == FDT_NODE_BEGIN) 395 ptr = skip_node(ptr); 396 397 if (betoh32(*ptr) != FDT_NODE_END) 398 return NULL; 399 400 ptr = skip_nops(ptr + 1); 401 402 if (betoh32(*ptr) != FDT_NODE_BEGIN) 403 return NULL; 404 405 return ptr; 406 } 407 408 /* 409 * Retrieves node property as integers and puts them in the given 410 * integer array. 411 */ 412 int 413 fdt_node_property_ints(void *node, char *name, int *out, int outlen) 414 { 415 int *data; 416 int i, inlen; 417 418 inlen = fdt_node_property(node, name, (char **)&data) / sizeof(int); 419 if (inlen <= 0) 420 return -1; 421 422 for (i = 0; i < inlen && i < outlen; i++) 423 out[i] = betoh32(data[i]); 424 425 return i; 426 } 427 428 /* 429 * Retrieves node property as an integer. 430 */ 431 int 432 fdt_node_property_int(void *node, char *name, int *out) 433 { 434 return fdt_node_property_ints(node, name, out, 1); 435 } 436 437 /* 438 * Retrieves next node, skipping all the children nodes of the pointed node 439 */ 440 void * 441 fdt_child_node(void *node) 442 { 443 uint32_t *ptr; 444 445 if (!tree_inited) 446 return NULL; 447 448 ptr = node; 449 450 if (betoh32(*ptr) != FDT_NODE_BEGIN) 451 return NULL; 452 453 ptr++; 454 455 ptr = skip_node_name(ptr); 456 ptr = skip_props(ptr); 457 /* check if there is a child node */ 458 return (betoh32(*ptr) == FDT_NODE_BEGIN) ? (ptr) : NULL; 459 } 460 461 /* 462 * Retrieves node name. 463 */ 464 char * 465 fdt_node_name(void *node) 466 { 467 uint32_t *ptr; 468 469 if (!tree_inited) 470 return NULL; 471 472 ptr = node; 473 474 if (betoh32(*ptr) != FDT_NODE_BEGIN) 475 return NULL; 476 477 return (char *)(ptr + 1); 478 } 479 480 void * 481 fdt_find_node(char *name) 482 { 483 void *node = fdt_next_node(0); 484 const char *p = name; 485 486 if (!tree_inited) 487 return NULL; 488 489 if (*p != '/') 490 return NULL; 491 492 while (*p) { 493 void *child; 494 const char *q; 495 496 while (*p == '/') 497 p++; 498 if (*p == 0) 499 return node; 500 q = strchr(p, '/'); 501 if (q == NULL) 502 q = p + strlen(p); 503 504 for (child = fdt_child_node(node); child; 505 child = fdt_next_node(child)) { 506 if (strncmp(p, fdt_node_name(child), q - p) == 0) { 507 node = child; 508 break; 509 } 510 } 511 512 if (child == NULL) 513 return NULL; /* No match found. */ 514 515 p = q; 516 } 517 518 return node; 519 } 520 521 void * 522 fdt_parent_node_recurse(void *pnode, void *child) 523 { 524 void *node = fdt_child_node(pnode); 525 void *tmp; 526 527 while (node && (node != child)) { 528 if ((tmp = fdt_parent_node_recurse(node, child))) 529 return tmp; 530 node = fdt_next_node(node); 531 } 532 return (node) ? pnode : NULL; 533 } 534 535 void * 536 fdt_parent_node(void *node) 537 { 538 void *pnode = fdt_next_node(0); 539 540 if (!tree_inited) 541 return NULL; 542 543 if (node == pnode) 544 return NULL; 545 546 return fdt_parent_node_recurse(pnode, node); 547 } 548 549 int 550 fdt_node_is_compatible(void *node, const char *name) 551 { 552 char *data; 553 int len; 554 555 len = fdt_node_property(node, "compatible", &data); 556 while (len > 0) { 557 if (strcmp(data, name) == 0) 558 return 1; 559 len -= strlen(data) + 1; 560 data += strlen(data) + 1; 561 } 562 563 return 0; 564 } 565 566 #ifdef DEBUG 567 /* 568 * Debug methods for printing whole tree, particular nodes and properties 569 */ 570 void * 571 fdt_print_property(void *node, int level) 572 { 573 uint32_t *ptr; 574 char *tmp, *value; 575 int cnt; 576 uint32_t nameid, size; 577 578 ptr = (uint32_t *)node; 579 580 if (!tree_inited) 581 return NULL; 582 583 if (betoh32(*ptr) != FDT_PROPERTY) 584 return ptr; /* should never happen */ 585 586 /* extract property name_id and size */ 587 size = betoh32(*++ptr); 588 nameid = betoh32(*++ptr); 589 590 for (cnt = 0; cnt < level; cnt++) 591 printf("\t"); 592 593 tmp = fdt_get_str(nameid); 594 printf("\t%s : ", tmp ? tmp : "NO_NAME"); 595 596 ptr++; 597 value = (char *)ptr; 598 599 if (!strcmp(tmp, "device_type") || !strcmp(tmp, "compatible") || 600 !strcmp(tmp, "model") || !strcmp(tmp, "bootargs") || 601 !strcmp(tmp, "linux,stdout-path")) { 602 printf("%s", value); 603 } else if (!strcmp(tmp, "clock-frequency") || 604 !strcmp(tmp, "timebase-frequency")) { 605 printf("%d", betoh32(*((unsigned int *)value))); 606 } else { 607 for (cnt = 0; cnt < size; cnt++) { 608 if ((cnt % sizeof(uint32_t)) == 0) 609 printf(" "); 610 printf("%x%x", value[cnt] >> 4, value[cnt] & 0xf); 611 } 612 } 613 ptr += roundup(size, sizeof(uint32_t)) / sizeof(uint32_t); 614 printf("\n"); 615 616 return ptr; 617 } 618 619 void 620 fdt_print_node(void *node, int level) 621 { 622 uint32_t *ptr; 623 int cnt; 624 625 ptr = (uint32_t *)node; 626 627 if (betoh32(*ptr) != FDT_NODE_BEGIN) 628 return; 629 630 ptr++; 631 632 for (cnt = 0; cnt < level; cnt++) 633 printf("\t"); 634 printf("%s :\n", fdt_node_name(node)); 635 ptr = skip_node_name(ptr); 636 637 while (betoh32(*ptr) == FDT_PROPERTY) 638 ptr = fdt_print_property(ptr, level); 639 } 640 641 void 642 fdt_print_node_recurse(void *node, int level) 643 { 644 void *child; 645 646 fdt_print_node(node, level); 647 for (child = fdt_child_node(node); child; child = fdt_next_node(child)) 648 fdt_print_node_recurse(child, level + 1); 649 } 650 651 void 652 fdt_print_tree(void) 653 { 654 fdt_print_node_recurse(fdt_next_node(0), 0); 655 } 656 #endif 657