ufdt_overlay.c revision 3084ce7cbdff84093286459758f99c15082e6556
1/*- 2 * Copyright (c) 2015 Oleksandr Tymoshenko <gonzo@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 */ 59static 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 */ 70static 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 */ 82static 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 */ 99static 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 */ 130static 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 188fail: 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 */ 200static 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 226static int ufdt_overlay_do_fixups(struct ufdt *main_tree, 227 struct ufdt *overlay_tree) { 228 int len = 0; 229 struct ufdt_node *main_symbols_node, *overlay_fixups_node; 230 231 main_symbols_node = ufdt_get_node_by_path(main_tree, "/__symbols__"); 232 overlay_fixups_node = ufdt_get_node_by_path(overlay_tree, "/__fixups__"); 233 234 if (!main_symbols_node) { 235 dto_error("Bad main_symbols in ufdt_overlay_do_fixups\n"); 236 return -1; 237 } 238 239 if (!overlay_fixups_node) { 240 dto_error("Bad overlay_fixups in ufdt_overlay_do_fixups\n"); 241 return -1; 242 } 243 244 struct ufdt_node **it; 245 for_each_prop(it, overlay_fixups_node) { 246 /* 247 * A property in __fixups__ looks like: 248 * symbol_name = 249 * "/path/to/node:prop:offset0\x00/path/to/node:prop:offset1..." 250 * So we firstly find the node "symbol_name" and obtain its phandle in 251 * __symbols__ of the main_tree. 252 */ 253 254 struct ufdt_node *fixups = *it; 255 char *symbol_path = ufdt_node_get_fdt_prop_data_by_name( 256 main_symbols_node, name_of(fixups), &len); 257 258 if (!symbol_path) { 259 dto_error("Couldn't find '%s' symbol in main dtb\n", name_of(fixups)); 260 return -1; 261 } 262 263 struct ufdt_node *symbol_node; 264 symbol_node = ufdt_get_node_by_path(main_tree, symbol_path); 265 266 if (!symbol_node) { 267 dto_error("Couldn't find '%s' path in main dtb\n", symbol_path); 268 return -1; 269 } 270 271 uint32_t phandle = ufdt_node_get_phandle(symbol_node); 272 273 const char *fixups_paths = ufdt_node_get_fdt_prop_data(fixups, &len); 274 275 if (ufdt_do_one_fixup(overlay_tree, fixups_paths, len, phandle) < 0) { 276 dto_error("Failed one fixup in ufdt_do_one_fixup\n"); 277 return -1; 278 } 279 } 280 281 return 0; 282} 283 284/* END of doing fixup in the overlay ufdt. */ 285 286/* 287 * Here is to overlay all fragments in the overlay_tree to the main_tree. 288 * What is "overlay fragment"? The main purpose is to add some subtrees to the 289 * main_tree in order to complete the entire device tree. 290 * 291 * A frgament consists of two parts: 1. the subtree to be added 2. where it 292 * should be added. 293 * 294 * Overlaying a fragment requires: 1. find the node in the main_tree 2. merge 295 * the subtree into that node in the main_tree. 296 */ 297 298/* BEGIN of applying fragments. */ 299 300/* 301 * Overlay the overlay_node over target_node. 302 */ 303static int ufdt_overlay_node(struct ufdt_node *target_node, 304 struct ufdt_node *overlay_node) { 305 return merge_ufdt_into(target_node, overlay_node); 306} 307 308/* 309 * Return value of ufdt_apply_fragment(). 310 */ 311 312enum overlay_result { 313 OVERLAY_RESULT_OK, 314 OVERLAY_RESULT_MISSING_TARGET, 315 OVERLAY_RESULT_MISSING_OVERLAY, 316 OVERLAY_RESULT_TARGET_PATH_INVALID, 317 OVERLAY_RESULT_TARGET_INVALID, 318 OVERLAY_RESULT_MERGE_FAIL, 319}; 320 321/* 322 * Apply one overlay fragment (subtree). 323 */ 324static enum overlay_result ufdt_apply_fragment(struct ufdt *tree, 325 struct ufdt_node *frag_node) { 326 uint32_t target; 327 const char *target_path; 328 const void *val; 329 struct ufdt_node *target_node = NULL; 330 struct ufdt_node *overlay_node = NULL; 331 332 val = ufdt_node_get_fdt_prop_data_by_name(frag_node, "target", NULL); 333 if (val) { 334 dto_memcpy(&target, val, sizeof(target)); 335 target = fdt32_to_cpu(target); 336 target_node = ufdt_get_node_by_phandle(tree, target); 337 if (target_node == NULL) { 338 dto_error("failed to find target %04x\n", target); 339 return OVERLAY_RESULT_TARGET_INVALID; 340 } 341 } 342 343 if (target_node == NULL) { 344 target_path = 345 ufdt_node_get_fdt_prop_data_by_name(frag_node, "target-path", NULL); 346 if (target_path == NULL) { 347 return OVERLAY_RESULT_MISSING_TARGET; 348 } 349 350 target_node = ufdt_get_node_by_path(tree, target_path); 351 if (target_node == NULL) { 352 dto_error("failed to find target-path %s\n", target_path); 353 return OVERLAY_RESULT_TARGET_PATH_INVALID; 354 } 355 } 356 357 overlay_node = ufdt_node_get_node_by_path(frag_node, "__overlay__"); 358 if (overlay_node == NULL) { 359 dto_error("missing __overlay__ sub-node\n"); 360 return OVERLAY_RESULT_MISSING_OVERLAY; 361 } 362 363 int err = ufdt_overlay_node(target_node, overlay_node); 364 365 if (err < 0) { 366 dto_error("failed to overlay node %s to target %s\n", name_of(overlay_node), 367 name_of(target_node)); 368 return OVERLAY_RESULT_MERGE_FAIL; 369 } 370 371 return OVERLAY_RESULT_OK; 372} 373 374/* 375 * Applies all fragments to the main_tree. 376 */ 377static int ufdt_overlay_apply_fragments(struct ufdt *main_tree, 378 struct ufdt *overlay_tree) { 379 enum overlay_result err; 380 struct ufdt_node **it; 381 /* 382 * This loop may iterate to subnodes that's not a fragment node. 383 * In such case, ufdt_apply_fragment would fail with return value = -1. 384 */ 385 for_each_node(it, overlay_tree->root) { 386 err = ufdt_apply_fragment(main_tree, *it); 387 if (err == OVERLAY_RESULT_MERGE_FAIL) { 388 return -1; 389 } 390 } 391 return 0; 392} 393 394/* END of applying fragments. */ 395 396/* 397 * Since the overlay_tree will be "merged" into the main_tree, some 398 * references (e.g., phandle values that acts as an unique ID) need to be 399 * updated so it won't lead to collision that different nodes have the same 400 * phandle value. 401 * 402 * Two things need to be done: 403 * 404 * 1. ufdt_try_increase_phandle() 405 * Update phandle (an unique integer ID of a node in the device tree) of each 406 * node in the overlay_tree. To achieve this, we simply increase each phandle 407 * values in the overlay_tree by the max phandle value of the main_tree. 408 * 409 * 2. ufdt_overlay_do_local_fixups() 410 * If there are some reference in the overlay_tree that references nodes 411 * inside the overlay_tree, we have to modify the reference value (address of 412 * the referenced node: phandle) so that it corresponds to the right node inside 413 * the overlay_tree. Where the reference exists is kept in __local_fixups__ node 414 * in the overlay_tree. 415 */ 416 417/* BEGIN of updating local references (phandle values) in the overlay ufdt. */ 418 419/* 420 * local fixups 421 */ 422static int ufdt_local_fixup_prop(struct ufdt_node *target_prop_node, 423 struct ufdt_node *local_fixup_prop_node, 424 uint32_t phandle_offset) { 425 /* 426 * prop_offsets_ptr should be a list of fdt32_t. 427 * <offset0 offset1 offset2 ...> 428 */ 429 char *prop_offsets_ptr; 430 int len = 0; 431 prop_offsets_ptr = ufdt_node_get_fdt_prop_data(local_fixup_prop_node, &len); 432 433 char *prop_data; 434 int target_length = 0; 435 436 prop_data = ufdt_node_get_fdt_prop_data(target_prop_node, &target_length); 437 438 if (prop_offsets_ptr == NULL || prop_data == NULL) return -1; 439 440 int i; 441 for (i = 0; i < len; i += sizeof(fdt32_t)) { 442 int offset = fdt32_to_cpu(*(fdt32_t *)(prop_offsets_ptr + i)); 443 if (offset + sizeof(fdt32_t) > (size_t)target_length) return -1; 444 fdt_increase_u32((prop_data + offset), phandle_offset); 445 } 446 return 0; 447} 448 449static int ufdt_local_fixup_node(struct ufdt_node *target_node, 450 struct ufdt_node *local_fixups_node, 451 uint32_t phandle_offset) { 452 if (local_fixups_node == NULL) return 0; 453 454 struct ufdt_node **it_local_fixups; 455 struct ufdt_node *sub_target_node; 456 457 for_each_prop(it_local_fixups, local_fixups_node) { 458 sub_target_node = 459 ufdt_node_get_property_by_name(target_node, name_of(*it_local_fixups)); 460 461 if (sub_target_node != NULL) { 462 int err = ufdt_local_fixup_prop(sub_target_node, *it_local_fixups, 463 phandle_offset); 464 if (err < 0) return -1; 465 } else { 466 return -1; 467 } 468 } 469 470 for_each_node(it_local_fixups, local_fixups_node) { 471 sub_target_node = 472 ufdt_node_get_node_by_path(target_node, name_of(*it_local_fixups)); 473 if (sub_target_node != NULL) { 474 int err = ufdt_local_fixup_node(sub_target_node, *it_local_fixups, 475 phandle_offset); 476 if (err < 0) return -1; 477 } else { 478 return -1; 479 } 480 } 481 482 return 0; 483} 484 485/* 486 * Handle __local_fixups__ node in overlay DTB 487 * The __local_fixups__ format we expect is 488 * __local_fixups__ { 489 * path { 490 * to { 491 * local_ref1 = <offset>; 492 * }; 493 * }; 494 * path2 { 495 * to2 { 496 * local_ref2 = <offset1 offset2 ...>; 497 * }; 498 * }; 499 * }; 500 * 501 * which follows the dtc patch from: 502 * https://marc.info/?l=devicetree&m=144061468601974&w=4 503 */ 504static int ufdt_overlay_do_local_fixups(struct ufdt *tree, 505 uint32_t phandle_offset) { 506 struct ufdt_node *overlay_node = ufdt_get_node_by_path(tree, "/"); 507 struct ufdt_node *local_fixups_node = 508 ufdt_get_node_by_path(tree, "/__local_fixups__"); 509 510 int err = 511 ufdt_local_fixup_node(overlay_node, local_fixups_node, phandle_offset); 512 513 if (err < 0) return -1; 514 515 return 0; 516} 517 518static int ufdt_overlay_local_ref_update(struct ufdt *main_tree, 519 struct ufdt *overlay_tree) { 520 uint32_t phandle_offset = 0; 521 522 phandle_offset = ufdt_get_max_phandle(main_tree); 523 if (phandle_offset > 0) { 524 ufdt_try_increase_phandle(overlay_tree, phandle_offset); 525 } 526 527 int err = ufdt_overlay_do_local_fixups(overlay_tree, phandle_offset); 528 if (err < 0) { 529 dto_error("failed to perform local fixups in overlay\n"); 530 return -1; 531 } 532 return 0; 533} 534 535/* END of updating local references (phandle values) in the overlay ufdt. */ 536 537static int ufdt_overlay_apply(struct ufdt *main_tree, struct ufdt *overlay_tree, 538 size_t overlay_length) { 539 if (overlay_length < sizeof(struct fdt_header)) { 540 dto_error("Overlay_length %zu smaller than header size %zu\n", 541 overlay_length, sizeof(struct fdt_header)); 542 return -1; 543 } 544 545 if (ufdt_overlay_local_ref_update(main_tree, overlay_tree) < 0) { 546 dto_error("failed to perform local fixups in overlay\n"); 547 return -1; 548 } 549 550 if (ufdt_overlay_do_fixups(main_tree, overlay_tree) < 0) { 551 dto_error("failed to perform fixups in overlay\n"); 552 return -1; 553 } 554 if (ufdt_overlay_apply_fragments(main_tree, overlay_tree) < 0) { 555 dto_error("failed to apply fragments\n"); 556 return -1; 557 } 558 559 return 0; 560} 561 562struct fdt_header *ufdt_install_blob(void *blob, size_t blob_size) { 563 struct fdt_header *pHeader; 564 int err; 565 566 dto_debug("ufdt_install_blob (0x%08jx)\n", (uintmax_t)blob); 567 568 if (blob_size < sizeof(struct fdt_header)) { 569 dto_error("Blob_size %zu smaller than the header size %zu\n", blob_size, 570 sizeof(struct fdt_header)); 571 return NULL; 572 } 573 574 pHeader = (struct fdt_header *)blob; 575 err = fdt_check_header(pHeader); 576 if (err < 0) { 577 if (err == -FDT_ERR_BADVERSION) { 578 dto_error("incompatible blob version: %d, should be: %d", 579 fdt_version(pHeader), FDT_LAST_SUPPORTED_VERSION); 580 581 } else { 582 dto_error("error validating blob: %s", fdt_strerror(err)); 583 } 584 return NULL; 585 } 586 587 return pHeader; 588} 589 590/* 591* From Google, based on dt_overlay_apply() logic 592* Will dto_malloc a new fdt blob and return it. Will not dto_free parameters. 593*/ 594struct fdt_header *ufdt_apply_overlay(struct fdt_header *main_fdt_header, 595 size_t main_fdt_size, 596 void *overlay_fdtp, 597 size_t overlay_size) { 598 size_t out_fdt_size; 599 600 if (main_fdt_header == NULL) { 601 return NULL; 602 } 603 604 if (overlay_size < 8 || overlay_size != fdt_totalsize(overlay_fdtp)) { 605 dto_error("Bad overlay size!\n"); 606 return NULL; 607 } 608 if (main_fdt_size < 8 || main_fdt_size != fdt_totalsize(main_fdt_header)) { 609 dto_error("Bad fdt size!\n"); 610 return NULL; 611 } 612 613 out_fdt_size = fdt_totalsize(main_fdt_header) + overlay_size; 614 /* It's actually more than enough */ 615 struct fdt_header *out_fdt_header = dto_malloc(out_fdt_size); 616 617 if (out_fdt_header == NULL) { 618 dto_error("failed to allocate memory for DTB blob with overlays\n"); 619 return NULL; 620 } 621 622 struct ufdt *main_tree, *overlay_tree; 623 624 main_tree = fdt_to_ufdt(main_fdt_header, main_fdt_size); 625 626 overlay_tree = fdt_to_ufdt(overlay_fdtp, overlay_size); 627 628 int err = ufdt_overlay_apply(main_tree, overlay_tree, overlay_size); 629 if (err < 0) { 630 goto fail; 631 } 632 633 err = ufdt_to_fdt(main_tree, out_fdt_header, out_fdt_size); 634 if (err < 0) { 635 dto_error("Failed to dump the device tree to out_fdt_header\n"); 636 goto fail; 637 } 638 639 ufdt_destruct(main_tree); 640 ufdt_destruct(overlay_tree); 641 return out_fdt_header; 642 643fail: 644 ufdt_destruct(main_tree); 645 ufdt_destruct(overlay_tree); 646 dto_free(out_fdt_header); 647 return NULL; 648} 649