1 /*- 2 * Copyright (c) 2015 Oleksandr Tymoshenko <gonzo (at) FreeBSD.org> 3 * All rights reserved. 4 * 5 * This software was developed by Semihalf under sponsorship from 6 * the FreeBSD Foundation. 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions 10 * are met: 11 * 1. Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 16 * 17 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 18 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 21 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 23 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 24 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 26 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 27 * SUCH DAMAGE. 28 */ 29 30 #include "ufdt_overlay.h" 31 32 #include "libufdt.h" 33 34 35 /* 36 * The original version of fdt_overlay.c is slow in searching for particular 37 * nodes and adding subnodes/properties due to the operations on flattened 38 * device tree (FDT). 39 * 40 * Here we introduce `libufdt` which builds a real tree structure (named 41 * ufdt -- unflattned device tree) from FDT. In the real tree, we can perform 42 * certain operations (e.g., merge 2 subtrees, search for a node by path) in 43 * almost optimal time complexity with acceptable additional memory usage. 44 * 45 * This file is the improved version of fdt_overlay.c by using the real tree 46 * structure defined in libufdt. 47 * 48 * How the device tree overlay works and some 49 * special terms (e.g., fixups, local fixups, fragment, etc) 50 * are described in the document 51 * external/dtc/Documentation/dt-object-internal.txt. 52 */ 53 54 /* BEGIN of operations about phandles in ufdt. */ 55 56 /* 57 * Increases u32 value at pos by offset. 58 */ 59 static void fdt_increase_u32(void *pos, uint32_t offset) { 60 uint32_t val; 61 62 dto_memcpy(&val, pos, sizeof(val)); 63 val = cpu_to_fdt32(fdt32_to_cpu(val) + offset); 64 dto_memcpy(pos, &val, sizeof(val)); 65 } 66 67 /* 68 * Gets the max phandle of a given ufdt. 69 */ 70 static uint32_t ufdt_get_max_phandle(struct ufdt *tree) { 71 struct static_phandle_table sorted_table = tree->phandle_table; 72 if (sorted_table.len > 0) 73 return sorted_table.data[sorted_table.len - 1].phandle; 74 else 75 return 0; 76 } 77 78 /* 79 * Tries to increase the phandle value of a node 80 * if the phandle exists. 81 */ 82 static void ufdt_node_try_increase_phandle(struct ufdt_node *node, 83 uint32_t offset) { 84 int len = 0; 85 char *prop_data = ufdt_node_get_fdt_prop_data_by_name(node, "phandle", &len); 86 if (prop_data != NULL && len == sizeof(fdt32_t)) { 87 fdt_increase_u32(prop_data, offset); 88 } 89 prop_data = ufdt_node_get_fdt_prop_data_by_name(node, "linux,phandle", &len); 90 if (prop_data != NULL && len == sizeof(fdt32_t)) { 91 fdt_increase_u32(prop_data, offset); 92 } 93 } 94 95 /* 96 * Increases all phandles by offset in a ufdt 97 * in O(n) time. 98 */ 99 static void ufdt_try_increase_phandle(struct ufdt *tree, uint32_t offset) { 100 struct static_phandle_table sorted_table = tree->phandle_table; 101 int i; 102 103 for (i = 0; i < sorted_table.len; i++) { 104 struct ufdt_node *target_node = sorted_table.data[i].node; 105 106 ufdt_node_try_increase_phandle(target_node, offset); 107 } 108 } 109 110 /* END of operations about phandles in ufdt. */ 111 112 /* 113 * In the overlay_tree, there are some references (phandle) 114 * pointing to somewhere in the main_tree. 115 * Fix-up operations is to resolve the right address 116 * in the overlay_tree. 117 */ 118 119 /* BEGIN of doing fixup in the overlay ufdt. */ 120 121 /* 122 * Returns exact memory location specified by fixup in format 123 * /path/to/node:property:offset. 124 * A property might contain multiple values and the offset is used to locate a 125 * reference inside the property. 126 * e.g., 127 * "property"=<1, 2, &ref, 4>, we can use /path/to/node:property:8 to get ref, 128 * where 8 is sizeof(uint32) + sizeof(unit32). 129 */ 130 static void *ufdt_get_fixup_location(struct ufdt *tree, const char *fixup) { 131 char *path, *prop_ptr, *offset_ptr, *end_ptr; 132 int prop_offset, prop_len; 133 const char *prop_data; 134 135 /* 136 * TODO(akaineko): Keep track of substring lengths so we don't have to 137 * dto_malloc a copy and split it up. 138 */ 139 path = dto_strdup(fixup); 140 prop_ptr = dto_strchr(path, ':'); 141 if (prop_ptr == NULL) { 142 dto_error("Missing property part in '%s'\n", path); 143 goto fail; 144 } 145 146 *prop_ptr = '\0'; 147 prop_ptr++; 148 149 offset_ptr = dto_strchr(prop_ptr, ':'); 150 if (offset_ptr == NULL) { 151 dto_error("Missing offset part in '%s'\n", path); 152 goto fail; 153 } 154 155 *offset_ptr = '\0'; 156 offset_ptr++; 157 158 prop_offset = dto_strtoul(offset_ptr, &end_ptr, 10 /* base */); 159 if (*end_ptr != '\0') { 160 dto_error("'%s' is not valid number\n", offset_ptr); 161 goto fail; 162 } 163 164 struct ufdt_node *target_node; 165 target_node = ufdt_get_node_by_path(tree, path); 166 if (target_node == NULL) { 167 dto_error("Path '%s' not found\n", path); 168 goto fail; 169 } 170 171 prop_data = 172 ufdt_node_get_fdt_prop_data_by_name(target_node, prop_ptr, &prop_len); 173 if (prop_data == NULL) { 174 dto_error("Property '%s' not found in '%s' node\n", prop_ptr, path); 175 goto fail; 176 } 177 /* 178 * Note that prop_offset is the offset inside the property data. 179 */ 180 if (prop_len < prop_offset + (int)sizeof(uint32_t)) { 181 dto_error("%s: property length is too small for fixup\n", path); 182 goto fail; 183 } 184 185 dto_free(path); 186 return (char *)prop_data + prop_offset; 187 188 fail: 189 dto_free(path); 190 return NULL; 191 } 192 193 /* 194 * Process one entry in __fixups__ { } node. 195 * @fixups is property value, array of NUL-terminated strings 196 * with fixup locations. 197 * @fixups_len length of the fixups array in bytes. 198 * @phandle is value for these locations. 199 */ 200 static int ufdt_do_one_fixup(struct ufdt *tree, const char *fixups, 201 int fixups_len, int phandle) { 202 void *fixup_pos; 203 uint32_t val; 204 205 val = cpu_to_fdt32(phandle); 206 207 while (fixups_len > 0) { 208 fixup_pos = ufdt_get_fixup_location(tree, fixups); 209 if (fixup_pos != NULL) { 210 dto_memcpy(fixup_pos, &val, sizeof(val)); 211 } else { 212 return -1; 213 } 214 215 fixups_len -= dto_strlen(fixups) + 1; 216 fixups += dto_strlen(fixups) + 1; 217 } 218 219 return 0; 220 } 221 222 /* 223 * Handle __fixups__ node in overlay tree. 224 */ 225 226 static int ufdt_overlay_do_fixups(struct ufdt *main_tree, 227 struct ufdt *overlay_tree) { 228 int len = 0; 229 struct ufdt_node *overlay_fixups_node = 230 ufdt_get_node_by_path(overlay_tree, "/__fixups__"); 231 if (!overlay_fixups_node) { 232 /* There is no __fixups__. Do nothing. */ 233 return 0; 234 } 235 236 struct ufdt_node *main_symbols_node = 237 ufdt_get_node_by_path(main_tree, "/__symbols__"); 238 239 struct ufdt_node **it; 240 for_each_prop(it, overlay_fixups_node) { 241 /* Find the first property */ 242 243 /* Check __symbols__ is exist when we have any property in __fixups__ */ 244 if (!main_symbols_node) { 245 dto_error("No node __symbols__ in main dtb.\n"); 246 return -1; 247 } 248 break; 249 } 250 251 for_each_prop(it, overlay_fixups_node) { 252 /* 253 * A property in __fixups__ looks like: 254 * symbol_name = 255 * "/path/to/node:prop:offset0\x00/path/to/node:prop:offset1..." 256 * So we firstly find the node "symbol_name" and obtain its phandle in 257 * __symbols__ of the main_tree. 258 */ 259 260 struct ufdt_node *fixups = *it; 261 char *symbol_path = ufdt_node_get_fdt_prop_data_by_name( 262 main_symbols_node, name_of(fixups), &len); 263 264 if (!symbol_path) { 265 dto_error("Couldn't find '%s' symbol in main dtb\n", name_of(fixups)); 266 return -1; 267 } 268 269 struct ufdt_node *symbol_node; 270 symbol_node = ufdt_get_node_by_path(main_tree, symbol_path); 271 272 if (!symbol_node) { 273 dto_error("Couldn't find '%s' path in main dtb\n", symbol_path); 274 return -1; 275 } 276 277 uint32_t phandle = ufdt_node_get_phandle(symbol_node); 278 279 const char *fixups_paths = ufdt_node_get_fdt_prop_data(fixups, &len); 280 281 if (ufdt_do_one_fixup(overlay_tree, fixups_paths, len, phandle) < 0) { 282 dto_error("Failed one fixup in ufdt_do_one_fixup\n"); 283 return -1; 284 } 285 } 286 287 return 0; 288 } 289 290 /* END of doing fixup in the overlay ufdt. */ 291 292 /* 293 * Here is to overlay all fragments in the overlay_tree to the main_tree. 294 * What is "overlay fragment"? The main purpose is to add some subtrees to the 295 * main_tree in order to complete the entire device tree. 296 * 297 * A frgament consists of two parts: 1. the subtree to be added 2. where it 298 * should be added. 299 * 300 * Overlaying a fragment requires: 1. find the node in the main_tree 2. merge 301 * the subtree into that node in the main_tree. 302 */ 303 304 /* BEGIN of applying fragments. */ 305 306 /* 307 * Overlay the overlay_node over target_node. 308 */ 309 static int ufdt_overlay_node(struct ufdt_node *target_node, 310 struct ufdt_node *overlay_node) { 311 return merge_ufdt_into(target_node, overlay_node); 312 } 313 314 /* 315 * Return value of ufdt_apply_fragment(). 316 */ 317 318 enum overlay_result { 319 OVERLAY_RESULT_OK, 320 OVERLAY_RESULT_MISSING_TARGET, 321 OVERLAY_RESULT_MISSING_OVERLAY, 322 OVERLAY_RESULT_TARGET_PATH_INVALID, 323 OVERLAY_RESULT_TARGET_INVALID, 324 OVERLAY_RESULT_MERGE_FAIL, 325 }; 326 327 /* 328 * Apply one overlay fragment (subtree). 329 */ 330 static enum overlay_result ufdt_apply_fragment(struct ufdt *tree, 331 struct ufdt_node *frag_node) { 332 uint32_t target; 333 const char *target_path; 334 const void *val; 335 struct ufdt_node *target_node = NULL; 336 struct ufdt_node *overlay_node = NULL; 337 338 val = ufdt_node_get_fdt_prop_data_by_name(frag_node, "target", NULL); 339 if (val) { 340 dto_memcpy(&target, val, sizeof(target)); 341 target = fdt32_to_cpu(target); 342 target_node = ufdt_get_node_by_phandle(tree, target); 343 if (target_node == NULL) { 344 dto_error("failed to find target %04x\n", target); 345 return OVERLAY_RESULT_TARGET_INVALID; 346 } 347 } 348 349 if (target_node == NULL) { 350 target_path = 351 ufdt_node_get_fdt_prop_data_by_name(frag_node, "target-path", NULL); 352 if (target_path == NULL) { 353 return OVERLAY_RESULT_MISSING_TARGET; 354 } 355 356 target_node = ufdt_get_node_by_path(tree, target_path); 357 if (target_node == NULL) { 358 dto_error("failed to find target-path %s\n", target_path); 359 return OVERLAY_RESULT_TARGET_PATH_INVALID; 360 } 361 } 362 363 overlay_node = ufdt_node_get_node_by_path(frag_node, "__overlay__"); 364 if (overlay_node == NULL) { 365 dto_error("missing __overlay__ sub-node\n"); 366 return OVERLAY_RESULT_MISSING_OVERLAY; 367 } 368 369 int err = ufdt_overlay_node(target_node, overlay_node); 370 371 if (err < 0) { 372 dto_error("failed to overlay node %s to target %s\n", name_of(overlay_node), 373 name_of(target_node)); 374 return OVERLAY_RESULT_MERGE_FAIL; 375 } 376 377 return OVERLAY_RESULT_OK; 378 } 379 380 /* 381 * Applies all fragments to the main_tree. 382 */ 383 static int ufdt_overlay_apply_fragments(struct ufdt *main_tree, 384 struct ufdt *overlay_tree) { 385 enum overlay_result err; 386 struct ufdt_node **it; 387 /* 388 * This loop may iterate to subnodes that's not a fragment node. 389 * In such case, ufdt_apply_fragment would fail with return value = -1. 390 */ 391 for_each_node(it, overlay_tree->root) { 392 err = ufdt_apply_fragment(main_tree, *it); 393 if (err == OVERLAY_RESULT_MERGE_FAIL) { 394 return -1; 395 } 396 } 397 return 0; 398 } 399 400 /* END of applying fragments. */ 401 402 /* 403 * Since the overlay_tree will be "merged" into the main_tree, some 404 * references (e.g., phandle values that acts as an unique ID) need to be 405 * updated so it won't lead to collision that different nodes have the same 406 * phandle value. 407 * 408 * Two things need to be done: 409 * 410 * 1. ufdt_try_increase_phandle() 411 * Update phandle (an unique integer ID of a node in the device tree) of each 412 * node in the overlay_tree. To achieve this, we simply increase each phandle 413 * values in the overlay_tree by the max phandle value of the main_tree. 414 * 415 * 2. ufdt_overlay_do_local_fixups() 416 * If there are some reference in the overlay_tree that references nodes 417 * inside the overlay_tree, we have to modify the reference value (address of 418 * the referenced node: phandle) so that it corresponds to the right node inside 419 * the overlay_tree. Where the reference exists is kept in __local_fixups__ node 420 * in the overlay_tree. 421 */ 422 423 /* BEGIN of updating local references (phandle values) in the overlay ufdt. */ 424 425 /* 426 * local fixups 427 */ 428 static int ufdt_local_fixup_prop(struct ufdt_node *target_prop_node, 429 struct ufdt_node *local_fixup_prop_node, 430 uint32_t phandle_offset) { 431 /* 432 * prop_offsets_ptr should be a list of fdt32_t. 433 * <offset0 offset1 offset2 ...> 434 */ 435 char *prop_offsets_ptr; 436 int len = 0; 437 prop_offsets_ptr = ufdt_node_get_fdt_prop_data(local_fixup_prop_node, &len); 438 439 char *prop_data; 440 int target_length = 0; 441 442 prop_data = ufdt_node_get_fdt_prop_data(target_prop_node, &target_length); 443 444 if (prop_offsets_ptr == NULL || prop_data == NULL) return -1; 445 446 int i; 447 for (i = 0; i < len; i += sizeof(fdt32_t)) { 448 int offset = fdt32_to_cpu(*(fdt32_t *)(prop_offsets_ptr + i)); 449 if (offset + sizeof(fdt32_t) > (size_t)target_length) return -1; 450 fdt_increase_u32((prop_data + offset), phandle_offset); 451 } 452 return 0; 453 } 454 455 static int ufdt_local_fixup_node(struct ufdt_node *target_node, 456 struct ufdt_node *local_fixups_node, 457 uint32_t phandle_offset) { 458 if (local_fixups_node == NULL) return 0; 459 460 struct ufdt_node **it_local_fixups; 461 struct ufdt_node *sub_target_node; 462 463 for_each_prop(it_local_fixups, local_fixups_node) { 464 sub_target_node = 465 ufdt_node_get_property_by_name(target_node, name_of(*it_local_fixups)); 466 467 if (sub_target_node != NULL) { 468 int err = ufdt_local_fixup_prop(sub_target_node, *it_local_fixups, 469 phandle_offset); 470 if (err < 0) return -1; 471 } else { 472 return -1; 473 } 474 } 475 476 for_each_node(it_local_fixups, local_fixups_node) { 477 sub_target_node = 478 ufdt_node_get_node_by_path(target_node, name_of(*it_local_fixups)); 479 if (sub_target_node != NULL) { 480 int err = ufdt_local_fixup_node(sub_target_node, *it_local_fixups, 481 phandle_offset); 482 if (err < 0) return -1; 483 } else { 484 return -1; 485 } 486 } 487 488 return 0; 489 } 490 491 /* 492 * Handle __local_fixups__ node in overlay DTB 493 * The __local_fixups__ format we expect is 494 * __local_fixups__ { 495 * path { 496 * to { 497 * local_ref1 = <offset>; 498 * }; 499 * }; 500 * path2 { 501 * to2 { 502 * local_ref2 = <offset1 offset2 ...>; 503 * }; 504 * }; 505 * }; 506 * 507 * which follows the dtc patch from: 508 * https://marc.info/?l=devicetree&m=144061468601974&w=4 509 */ 510 static int ufdt_overlay_do_local_fixups(struct ufdt *tree, 511 uint32_t phandle_offset) { 512 struct ufdt_node *overlay_node = ufdt_get_node_by_path(tree, "/"); 513 struct ufdt_node *local_fixups_node = 514 ufdt_get_node_by_path(tree, "/__local_fixups__"); 515 516 int err = 517 ufdt_local_fixup_node(overlay_node, local_fixups_node, phandle_offset); 518 519 if (err < 0) return -1; 520 521 return 0; 522 } 523 524 static int ufdt_overlay_local_ref_update(struct ufdt *main_tree, 525 struct ufdt *overlay_tree) { 526 uint32_t phandle_offset = 0; 527 528 phandle_offset = ufdt_get_max_phandle(main_tree); 529 if (phandle_offset > 0) { 530 ufdt_try_increase_phandle(overlay_tree, phandle_offset); 531 } 532 533 int err = ufdt_overlay_do_local_fixups(overlay_tree, phandle_offset); 534 if (err < 0) { 535 dto_error("failed to perform local fixups in overlay\n"); 536 return -1; 537 } 538 return 0; 539 } 540 541 /* END of updating local references (phandle values) in the overlay ufdt. */ 542 543 static int _ufdt_overlay_fdtps(struct ufdt *main_tree, 544 const struct ufdt *overlay_tree) { 545 for (int i = 0; i < overlay_tree->num_used_fdtps; i++) { 546 void *fdt = overlay_tree->fdtps[i]; 547 if (ufdt_add_fdt(main_tree, fdt) < 0) { 548 return -1; 549 } 550 } 551 return 0; 552 } 553 554 static int ufdt_overlay_apply(struct ufdt *main_tree, struct ufdt *overlay_tree, 555 size_t overlay_length) { 556 if (_ufdt_overlay_fdtps(main_tree, overlay_tree) < 0) { 557 dto_error("failed to add more fdt into main ufdt tree.\n"); 558 return -1; 559 } 560 561 if (overlay_length < sizeof(struct fdt_header)) { 562 dto_error("Overlay_length %zu smaller than header size %zu\n", 563 overlay_length, sizeof(struct fdt_header)); 564 return -1; 565 } 566 567 if (ufdt_overlay_local_ref_update(main_tree, overlay_tree) < 0) { 568 dto_error("failed to perform local fixups in overlay\n"); 569 return -1; 570 } 571 572 if (ufdt_overlay_do_fixups(main_tree, overlay_tree) < 0) { 573 dto_error("failed to perform fixups in overlay\n"); 574 return -1; 575 } 576 if (ufdt_overlay_apply_fragments(main_tree, overlay_tree) < 0) { 577 dto_error("failed to apply fragments\n"); 578 return -1; 579 } 580 581 return 0; 582 } 583 584 struct fdt_header *ufdt_install_blob(void *blob, size_t blob_size) { 585 struct fdt_header *pHeader; 586 int err; 587 588 dto_debug("ufdt_install_blob (0x%08jx)\n", (uintmax_t)blob); 589 590 if (blob_size < sizeof(struct fdt_header)) { 591 dto_error("Blob_size %zu smaller than the header size %zu\n", blob_size, 592 sizeof(struct fdt_header)); 593 return NULL; 594 } 595 596 pHeader = (struct fdt_header *)blob; 597 err = fdt_check_header(pHeader); 598 if (err < 0) { 599 if (err == -FDT_ERR_BADVERSION) { 600 dto_error("incompatible blob version: %d, should be: %d", 601 fdt_version(pHeader), FDT_LAST_SUPPORTED_VERSION); 602 603 } else { 604 dto_error("error validating blob: %s", fdt_strerror(err)); 605 } 606 return NULL; 607 } 608 609 return pHeader; 610 } 611 612 /* 613 * From Google, based on dt_overlay_apply() logic 614 * Will dto_malloc a new fdt blob and return it. Will not dto_free parameters. 615 */ 616 struct fdt_header *ufdt_apply_overlay(struct fdt_header *main_fdt_header, 617 size_t main_fdt_size, 618 void *overlay_fdtp, 619 size_t overlay_size) { 620 size_t out_fdt_size; 621 622 if (main_fdt_header == NULL) { 623 return NULL; 624 } 625 626 if (overlay_size < 8 || overlay_size != fdt_totalsize(overlay_fdtp)) { 627 dto_error("Bad overlay size!\n"); 628 return NULL; 629 } 630 if (main_fdt_size < 8 || main_fdt_size != fdt_totalsize(main_fdt_header)) { 631 dto_error("Bad fdt size!\n"); 632 return NULL; 633 } 634 635 out_fdt_size = fdt_totalsize(main_fdt_header) + overlay_size; 636 /* It's actually more than enough */ 637 struct fdt_header *out_fdt_header = dto_malloc(out_fdt_size); 638 639 if (out_fdt_header == NULL) { 640 dto_error("failed to allocate memory for DTB blob with overlays\n"); 641 return NULL; 642 } 643 644 struct ufdt *main_tree = NULL; 645 struct ufdt *overlay_tree = NULL; 646 647 main_tree = fdt_to_ufdt(main_fdt_header, main_fdt_size); 648 overlay_tree = fdt_to_ufdt(overlay_fdtp, overlay_size); 649 650 int err = ufdt_overlay_apply(main_tree, overlay_tree, overlay_size); 651 if (err < 0) { 652 goto fail; 653 } 654 655 err = ufdt_to_fdt(main_tree, out_fdt_header, out_fdt_size); 656 if (err < 0) { 657 dto_error("Failed to dump the device tree to out_fdt_header\n"); 658 goto fail; 659 } 660 661 ufdt_destruct(overlay_tree); 662 ufdt_destruct(main_tree); 663 664 return out_fdt_header; 665 666 fail: 667 ufdt_destruct(overlay_tree); 668 ufdt_destruct(main_tree); 669 dto_free(out_fdt_header); 670 671 return NULL; 672 } 673
