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