sys/boot/fdt/fdt_overlay.c

   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$");
  29
  30 #include <stand.h>
  31 #include <libfdt.h>
  32
  33 #include "fdt_overlay.h"
  34
  35 /*
  36  * Get max phandle
  37  */
  38 static uint32_t
  39 fdt_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  */
  60 static void *
  61 fdt_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
 119 out:
 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  */
 131 static int
 132 fdt_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  */
 154 static void
 155 fdt_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  */
 171 static int
 172 fdt_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  */
 191 static void
 192 fdt_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  */
 210 static void
 211 fdt_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  */
 224 static void
 225 fdt_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  */
 267 static void
 268 fdt_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  */
 314 static int
 315 fdt_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  */
 357 static int
 358 fdt_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  */
 384 static int
 385 fdt_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
 400 int
 401 fdt_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
 434 out:
 435         free(overlay_copy);
 436
 437         return (rv);
 438 }