fdt_overlay.c revision 303975
1/*- 2 * Copyright (c) 2015 Oleksandr Tymoshenko <gonzo@FreeBSD.org> 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions 7 * are met: 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24 * SUCH DAMAGE. 25 */ 26 27#include <sys/cdefs.h> 28__FBSDID("$FreeBSD: releng/11.0/sys/boot/fdt/fdt_overlay.c 298821 2016-04-29 22:42:59Z gonzo $"); 29 30#include <stand.h> 31#include <libfdt.h> 32 33#include "fdt_overlay.h" 34 35/* 36 * Get max phandle 37 */ 38static uint32_t 39fdt_max_phandle(void *fdtp) 40{ 41 int o, depth; 42 uint32_t max_phandle, phandle; 43 44 depth = 1; 45 o = fdt_path_offset(fdtp, "/"); 46 max_phandle = fdt_get_phandle(fdtp, o); 47 for (depth = 0; (o >= 0) && (depth >= 0); o = fdt_next_node(fdtp, o, &depth)) { 48 phandle = fdt_get_phandle(fdtp, o); 49 if (max_phandle < phandle) 50 max_phandle = phandle; 51 } 52 53 return max_phandle; 54} 55 56/* 57 * Returns exact memory location specified by fixup in format 58 * /path/to/node:property:offset 59 */ 60static void * 61fdt_get_fixup_location(void *fdtp, const char *fixup) 62{ 63 char *path, *prop, *offsetp, *endp; 64 int prop_offset, o, proplen; 65 void *result; 66 67 result = 0; 68 69 path = strdup(fixup); 70 prop = strchr(path, ':'); 71 if (prop == NULL) { 72 printf("missing property part in \"%s\"\n", fixup); 73 result = NULL; 74 goto out; 75 } 76 77 *prop = 0; 78 prop++; 79 80 offsetp = strchr(prop, ':'); 81 if (offsetp == NULL) { 82 printf("missing offset part in \"%s\"\n", fixup); 83 result = NULL; 84 goto out; 85 } 86 87 *offsetp = 0; 88 offsetp++; 89 90 prop_offset = strtoul(offsetp, &endp, 10); 91 if (*endp != '\0') { 92 printf("\"%s\" is not valid number\n", offsetp); 93 result = NULL; 94 goto out; 95 } 96 97 o = fdt_path_offset(fdtp, path); 98 if (o < 0) { 99 printf("path \"%s\" not found\n", path); 100 result = NULL; 101 goto out; 102 } 103 104 result = fdt_getprop_w(fdtp, o, prop, &proplen); 105 if (result == NULL){ 106 printf("property \"%s\" not found in \"%s\" node\n", prop, path); 107 result = NULL; 108 goto out; 109 } 110 111 if (proplen < prop_offset + sizeof(uint32_t)) { 112 printf("%s: property length is too small for fixup\n", fixup); 113 result = NULL; 114 goto out; 115 } 116 117 result = (char*)result + prop_offset; 118 119out: 120 free(path); 121 return (result); 122} 123 124/* 125 * Process one entry in __fixups__ { } node 126 * @fixups is property value, array of NUL-terminated strings 127 * with fixup locations 128 * @fixups_len length of the fixups array in bytes 129 * @phandle is value for these locations 130 */ 131static int 132fdt_do_one_fixup(void *fdtp, const char *fixups, int fixups_len, int phandle) 133{ 134 void *fixup_pos; 135 uint32_t val; 136 137 val = cpu_to_fdt32(phandle); 138 139 while (fixups_len > 0) { 140 fixup_pos = fdt_get_fixup_location(fdtp, fixups); 141 if (fixup_pos != NULL) 142 memcpy(fixup_pos, &val, sizeof(val)); 143 144 fixups_len -= strlen(fixups) + 1; 145 fixups += strlen(fixups) + 1; 146 } 147 148 return (0); 149} 150 151/* 152 * Increase u32 value at pos by offset 153 */ 154static void 155fdt_increase_u32(void *pos, uint32_t offset) 156{ 157 uint32_t val; 158 159 memcpy(&val, pos, sizeof(val)); 160 val = cpu_to_fdt32(fdt32_to_cpu(val) + offset); 161 memcpy(pos, &val, sizeof(val)); 162} 163 164/* 165 * Process local fixups 166 * @fixups is property value, array of NUL-terminated strings 167 * with fixup locations 168 * @fixups_len length of the fixups array in bytes 169 * @offset value these locations should be increased by 170 */ 171static int 172fdt_do_local_fixup(void *fdtp, const char *fixups, int fixups_len, int offset) 173{ 174 void *fixup_pos; 175 176 while (fixups_len > 0) { 177 fixup_pos = fdt_get_fixup_location(fdtp, fixups); 178 if (fixup_pos != NULL) 179 fdt_increase_u32(fixup_pos, offset); 180 181 fixups_len -= strlen(fixups) + 1; 182 fixups += strlen(fixups) + 1; 183 } 184 185 return (0); 186} 187 188/* 189 * Increase node phandle by phandle_offset 190 */ 191static void 192fdt_increase_phandle(void *fdtp, int node_offset, uint32_t phandle_offset) 193{ 194 int proplen; 195 void *phandle_pos, *node_pos; 196 197 node_pos = (char*)fdtp + node_offset; 198 199 phandle_pos = fdt_getprop_w(fdtp, node_offset, "phandle", &proplen); 200 if (phandle_pos) 201 fdt_increase_u32(phandle_pos, phandle_offset); 202 phandle_pos = fdt_getprop_w(fdtp, node_offset, "linux,phandle", &proplen); 203 if (phandle_pos) 204 fdt_increase_u32(phandle_pos, phandle_offset); 205} 206 207/* 208 * Increase all phandles by offset 209 */ 210static void 211fdt_increase_phandles(void *fdtp, uint32_t offset) 212{ 213 int o, depth; 214 215 o = fdt_path_offset(fdtp, "/"); 216 for (depth = 0; (o >= 0) && (depth >= 0); o = fdt_next_node(fdtp, o, &depth)) { 217 fdt_increase_phandle(fdtp, o, offset); 218 } 219} 220 221/* 222 * Overlay one node defined by <overlay_fdtp, overlay_o> over <main_fdtp, target_o> 223 */ 224static void 225fdt_overlay_node(void *main_fdtp, int target_o, void *overlay_fdtp, int overlay_o) 226{ 227 int len, o, depth; 228 const char *name; 229 const void *val; 230 int target_subnode_o; 231 232 /* Overlay properties */ 233 for (o = fdt_first_property_offset(overlay_fdtp, overlay_o); 234 o >= 0; o = fdt_next_property_offset(overlay_fdtp, o)) { 235 val = fdt_getprop_by_offset(overlay_fdtp, o, &name, &len); 236 if (val) 237 fdt_setprop(main_fdtp, target_o, name, val, len); 238 } 239 240 /* Now overlay nodes */ 241 o = overlay_o; 242 for (depth = 0; (o >= 0) && (depth >= 0); 243 o = fdt_next_node(overlay_fdtp, o, &depth)) { 244 if (depth != 1) 245 continue; 246 /* Check if there is node with the same name */ 247 name = fdt_get_name(overlay_fdtp, o, NULL); 248 target_subnode_o = fdt_subnode_offset(main_fdtp, target_o, name); 249 if (target_subnode_o < 0) { 250 /* create new subnode and run merge recursively */ 251 target_subnode_o = fdt_add_subnode(main_fdtp, target_o, name); 252 if (target_subnode_o < 0) { 253 printf("failed to create subnode \"%s\": %d\n", 254 name, target_subnode_o); 255 return; 256 } 257 } 258 259 fdt_overlay_node(main_fdtp, target_subnode_o, 260 overlay_fdtp, o); 261 } 262} 263 264/* 265 * Apply one overlay fragment 266 */ 267static void 268fdt_apply_fragment(void *main_fdtp, void *overlay_fdtp, int fragment_o) 269{ 270 uint32_t target; 271 const char *target_path; 272 const void *val; 273 int target_node_o, overlay_node_o; 274 275 target_node_o = -1; 276 val = fdt_getprop(overlay_fdtp, fragment_o, "target", NULL); 277 if (val) { 278 memcpy(&target, val, sizeof(target)); 279 target = fdt32_to_cpu(target); 280 target_node_o = fdt_node_offset_by_phandle(main_fdtp, target); 281 if (target_node_o < 0) { 282 printf("failed to find target %04x\n", target); 283 return; 284 } 285 } 286 287 if (target_node_o < 0) { 288 target_path = fdt_getprop(overlay_fdtp, fragment_o, "target-path", NULL); 289 if (target_path == NULL) 290 return; 291 292 target_node_o = fdt_path_offset(main_fdtp, target_path); 293 if (target_node_o < 0) { 294 printf("failed to find target-path %s\n", target_path); 295 return; 296 } 297 } 298 299 if (target_node_o < 0) 300 return; 301 302 overlay_node_o = fdt_subnode_offset(overlay_fdtp, fragment_o, "__overlay__"); 303 if (overlay_node_o < 0) { 304 printf("missing __overlay__ sub-node\n"); 305 return; 306 } 307 308 fdt_overlay_node(main_fdtp, target_node_o, overlay_fdtp, overlay_node_o); 309} 310 311/* 312 * Handle __fixups__ node in overlay DTB 313 */ 314static int 315fdt_overlay_do_fixups(void *main_fdtp, void *overlay_fdtp) 316{ 317 int main_symbols_o, symbol_o, overlay_fixups_o; 318 int fixup_prop_o; 319 int len; 320 const char *fixups, *name; 321 const char *symbol_path; 322 uint32_t phandle; 323 324 main_symbols_o = fdt_path_offset(main_fdtp, "/__symbols__"); 325 overlay_fixups_o = fdt_path_offset(overlay_fdtp, "/__fixups__"); 326 327 if (main_symbols_o < 0) 328 return (-1); 329 if (overlay_fixups_o < 0) 330 return (-1); 331 332 for (fixup_prop_o = fdt_first_property_offset(overlay_fdtp, overlay_fixups_o); 333 fixup_prop_o >= 0; 334 fixup_prop_o = fdt_next_property_offset(overlay_fdtp, fixup_prop_o)) { 335 fixups = fdt_getprop_by_offset(overlay_fdtp, fixup_prop_o, &name, &len); 336 symbol_path = fdt_getprop(main_fdtp, main_symbols_o, name, NULL); 337 if (symbol_path == NULL) { 338 printf("couldn't find \"%s\" symbol in main dtb\n", name); 339 return (-1); 340 } 341 symbol_o = fdt_path_offset(main_fdtp, symbol_path); 342 if (symbol_o < 0) { 343 printf("couldn't find \"%s\" path in main dtb\n", symbol_path); 344 return (-1); 345 } 346 phandle = fdt_get_phandle(main_fdtp, symbol_o); 347 if (fdt_do_one_fixup(overlay_fdtp, fixups, len, phandle) < 0) 348 return (-1); 349 } 350 351 return (0); 352} 353 354/* 355 * Handle __local_fixups__ node in overlay DTB 356 */ 357static int 358fdt_overlay_do_local_fixups(void *main_fdtp, void *overlay_fdtp) 359{ 360 int overlay_local_fixups_o; 361 int len; 362 const char *fixups; 363 uint32_t phandle_offset; 364 365 overlay_local_fixups_o = fdt_path_offset(overlay_fdtp, "/__local_fixups__"); 366 367 if (overlay_local_fixups_o < 0) 368 return (-1); 369 370 phandle_offset = fdt_max_phandle(main_fdtp); 371 fdt_increase_phandles(overlay_fdtp, phandle_offset); 372 fixups = fdt_getprop_w(overlay_fdtp, overlay_local_fixups_o, "fixup", &len); 373 if (fixups) { 374 if (fdt_do_local_fixup(overlay_fdtp, fixups, len, phandle_offset) < 0) 375 return (-1); 376 } 377 378 return (0); 379} 380 381/* 382 * Apply all fragments to main DTB 383 */ 384static int 385fdt_overlay_apply_fragments(void *main_fdtp, void *overlay_fdtp) 386{ 387 int o, depth; 388 389 o = fdt_path_offset(overlay_fdtp, "/"); 390 for (depth = 0; (o >= 0) && (depth >= 0); o = fdt_next_node(overlay_fdtp, o, &depth)) { 391 if (depth != 1) 392 continue; 393 394 fdt_apply_fragment(main_fdtp, overlay_fdtp, o); 395 } 396 397 return (0); 398} 399 400int 401fdt_overlay_apply(void *main_fdtp, void *overlay_fdtp, size_t overlay_length) 402{ 403 void *overlay_copy; 404 int rv; 405 406 rv = 0; 407 408 /* We modify overlay in-place, so we need writable copy */ 409 overlay_copy = malloc(overlay_length); 410 if (overlay_copy == NULL) { 411 printf("failed to allocate memory for overlay copy\n"); 412 return (-1); 413 } 414 415 memcpy(overlay_copy, overlay_fdtp, overlay_length); 416 417 if (fdt_overlay_do_fixups(main_fdtp, overlay_copy) < 0) { 418 printf("failed to perform fixups in overlay\n"); 419 rv = -1; 420 goto out; 421 } 422 423 if (fdt_overlay_do_local_fixups(main_fdtp, overlay_copy) < 0) { 424 printf("failed to perform local fixups in overlay\n"); 425 rv = -1; 426 goto out; 427 } 428 429 if (fdt_overlay_apply_fragments(main_fdtp, overlay_copy) < 0) { 430 printf("failed to apply fragments\n"); 431 rv = -1; 432 } 433 434out: 435 free(overlay_copy); 436 437 return (rv); 438} 439