lib/of_live.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright 2009 Benjamin Herrenschmidt, IBM Corp
   4  * [email protected]
   5  *
   6  * Based on parts of drivers/of/fdt.c from Linux v4.9
   7  * Modifications for U-Boot
   8  * Copyright (c) 2017 Google, Inc
   9  */
  10
  11 #define LOG_CATEGORY    LOGC_DT
  12
  13 #include <abuf.h>
  14 #include <log.h>
  15 #include <linux/libfdt.h>
  16 #include <of_live.h>
  17 #include <malloc.h>
  18 #include <dm/of_access.h>
  19 #include <linux/err.h>
  20 #include <linux/sizes.h>
  21
  22 enum {
  23         BUF_STEP        = SZ_64K,
  24 };
  25
  26 static void *unflatten_dt_alloc(void **mem, unsigned long size,
  27                                 unsigned long align)
  28 {
  29         void *res;
  30
  31         *mem = PTR_ALIGN(*mem, align);
  32         res = *mem;
  33         *mem += size;
  34
  35         return res;
  36 }
  37
  38 /**
  39  * unflatten_dt_node() - Alloc and populate a device_node from the flat tree
  40  * @blob: The parent device tree blob
  41  * @mem: Memory chunk to use for allocating device nodes and properties
  42  * @poffset: pointer to node in flat tree
  43  * @dad: Parent struct device_node
  44  * @nodepp: The device_node tree created by the call
  45  * @fpsize: Size of the node path up at t05he current depth.
  46  * @dryrun: If true, do not allocate device nodes but still calculate needed
  47  * memory size
  48  */
  49 static void *unflatten_dt_node(const void *blob, void *mem, int *poffset,
  50                                struct device_node *dad,
  51                                struct device_node **nodepp,
  52                                unsigned long fpsize, bool dryrun)
  53 {
  54         const __be32 *p;
  55         struct device_node *np;
  56         struct property *pp, **prev_pp = NULL;
  57         const char *pathp;
  58         int l;
  59         unsigned int allocl;
  60         static int depth;
  61         int old_depth;
  62         int offset;
  63         int has_name = 0;
  64         int new_format = 0;
  65
  66         pathp = fdt_get_name(blob, *poffset, &l);
  67         if (!pathp)
  68                 return mem;
  69
  70         allocl = ++l;
  71
  72         /*
  73          * version 0x10 has a more compact unit name here instead of the full
  74          * path. we accumulate the full path size using "fpsize", we'll rebuild
  75          * it later. We detect this because the first character of the name is
  76          * not '/'.
  77          */
  78         if ((*pathp) != '/') {
  79                 new_format = 1;
  80                 if (fpsize == 0) {
  81                         /*
  82                          * root node: special case. fpsize accounts for path
  83                          * plus terminating zero. root node only has '/', so
  84                          * fpsize should be 2, but we want to avoid the first
  85                          * level nodes to have two '/' so we use fpsize 1 here
  86                          */
  87                         fpsize = 1;
  88                         allocl = 2;
  89                         l = 1;
  90                         pathp = "";
  91                 } else {
  92                         /*
  93                          * account for '/' and path size minus terminal 0
  94                          * already in 'l'
  95                          */
  96                         fpsize += l;
  97                         allocl = fpsize;
  98                 }
  99         }
 100
 101         np = unflatten_dt_alloc(&mem, sizeof(struct device_node) + allocl,
 102                                 __alignof__(struct device_node));
 103         if (!dryrun) {
 104                 char *fn;
 105
 106                 fn = (char *)np + sizeof(*np);
 107                 if (new_format) {
 108                         np->name = pathp;
 109                         has_name = 1;
 110                 }
 111                 np->full_name = fn;
 112                 if (new_format) {
 113                         /* rebuild full path for new format */
 114                         if (dad && dad->parent) {
 115                                 strcpy(fn, dad->full_name);
 116 #ifdef DEBUG
 117                                 if ((strlen(fn) + l + 1) != allocl) {
 118                                         debug("%s: p: %d, l: %d, a: %d\n",
 119                                               pathp, (int)strlen(fn), l,
 120                                               allocl);
 121                                 }
 122 #endif
 123                                 fn += strlen(fn);
 124                         }
 125                         *(fn++) = '/';
 126                 }
 127                 memcpy(fn, pathp, l);
 128
 129                 prev_pp = &np->properties;
 130                 if (dad != NULL) {
 131                         np->parent = dad;
 132                         np->sibling = dad->child;
 133                         dad->child = np;
 134                 }
 135         }
 136         /* process properties */
 137         for (offset = fdt_first_property_offset(blob, *poffset);
 138              (offset >= 0);
 139              (offset = fdt_next_property_offset(blob, offset))) {
 140                 const char *pname;
 141                 int sz;
 142
 143                 p = fdt_getprop_by_offset(blob, offset, &pname, &sz);
 144                 if (!p) {
 145                         offset = -FDT_ERR_INTERNAL;
 146                         break;
 147                 }
 148
 149                 if (pname == NULL) {
 150                         debug("Can't find property name in list !\n");
 151                         break;
 152                 }
 153                 if (strcmp(pname, "name") == 0)
 154                         has_name = 1;
 155                 pp = unflatten_dt_alloc(&mem, sizeof(struct property),
 156                                         __alignof__(struct property));
 157                 if (!dryrun) {
 158                         /*
 159                          * We accept flattened tree phandles either in
 160                          * ePAPR-style "phandle" properties, or the
 161                          * legacy "linux,phandle" properties.  If both
 162                          * appear and have different values, things
 163                          * will get weird.  Don't do that. */
 164                         if ((strcmp(pname, "phandle") == 0) ||
 165                             (strcmp(pname, "linux,phandle") == 0)) {
 166                                 if (np->phandle == 0)
 167                                         np->phandle = be32_to_cpup(p);
 168                         }
 169                         /*
 170                          * And we process the "ibm,phandle" property
 171                          * used in pSeries dynamic device tree
 172                          * stuff */
 173                         if (strcmp(pname, "ibm,phandle") == 0)
 174                                 np->phandle = be32_to_cpup(p);
 175                         pp->name = (char *)pname;
 176                         pp->length = sz;
 177                         pp->value = (__be32 *)p;
 178                         *prev_pp = pp;
 179                         prev_pp = &pp->next;
 180                 }
 181         }
 182         /*
 183          * with version 0x10 we may not have the name property, recreate
 184          * it here from the unit name if absent
 185          */
 186         if (!has_name) {
 187                 const char *p1 = pathp, *ps = pathp, *pa = NULL;
 188                 int sz;
 189
 190                 while (*p1) {
 191                         if ((*p1) == '@')
 192                                 pa = p1;
 193                         if ((*p1) == '/')
 194                                 ps = p1 + 1;
 195                         p1++;
 196                 }
 197                 if (pa < ps)
 198                         pa = p1;
 199                 sz = (pa - ps) + 1;
 200                 pp = unflatten_dt_alloc(&mem, sizeof(struct property) + sz,
 201                                         __alignof__(struct property));
 202                 if (!dryrun) {
 203                         pp->name = "name";
 204                         pp->length = sz;
 205                         pp->value = pp + 1;
 206                         *prev_pp = pp;
 207                         prev_pp = &pp->next;
 208                         memcpy(pp->value, ps, sz - 1);
 209                         ((char *)pp->value)[sz - 1] = 0;
 210                         debug("fixed up name for %s -> %s\n", pathp,
 211                               (char *)pp->value);
 212                 }
 213         }
 214         if (!dryrun) {
 215                 *prev_pp = NULL;
 216                 if (!has_name)
 217                         np->name = of_get_property(np, "name", NULL);
 218                 np->type = of_get_property(np, "device_type", NULL);
 219
 220                 if (!np->name)
 221                         np->name = "<NULL>";
 222                 if (!np->type)
 223                         np->type = "<NULL>";    }
 224
 225         old_depth = depth;
 226         *poffset = fdt_next_node(blob, *poffset, &depth);
 227         if (depth < 0)
 228                 depth = 0;
 229         while (*poffset > 0 && depth > old_depth) {
 230                 mem = unflatten_dt_node(blob, mem, poffset, np, NULL,
 231                                         fpsize, dryrun);
 232                 if (!mem)
 233                         return NULL;
 234         }
 235
 236         if (*poffset < 0 && *poffset != -FDT_ERR_NOTFOUND) {
 237                 debug("unflatten: error %d processing FDT\n", *poffset);
 238                 return NULL;
 239         }
 240
 241         /*
 242          * Reverse the child list. Some drivers assumes node order matches .dts
 243          * node order
 244          */
 245         if (!dryrun && np->child) {
 246                 struct device_node *child = np->child;
 247                 np->child = NULL;
 248                 while (child) {
 249                         struct device_node *next = child->sibling;
 250
 251                         child->sibling = np->child;
 252                         np->child = child;
 253                         child = next;
 254                 }
 255         }
 256
 257         if (nodepp)
 258                 *nodepp = np;
 259
 260         return mem;
 261 }
 262
 263 int unflatten_device_tree(const void *blob, struct device_node **mynodes)
 264 {
 265         unsigned long size;
 266         int start;
 267         void *mem;
 268
 269         debug(" -> unflatten_device_tree()\n");
 270
 271         if (!blob) {
 272                 debug("No device tree pointer\n");
 273                 return -EINVAL;
 274         }
 275
 276         debug("Unflattening device tree:\n");
 277         debug("magic: %08x\n", fdt_magic(blob));
 278         debug("size: %08x\n", fdt_totalsize(blob));
 279         debug("version: %08x\n", fdt_version(blob));
 280
 281         if (fdt_check_header(blob)) {
 282                 debug("Invalid device tree blob header\n");
 283                 return -EINVAL;
 284         }
 285
 286         /* First pass, scan for size */
 287         start = 0;
 288         size = (unsigned long)unflatten_dt_node(blob, NULL, &start, NULL, NULL,
 289                                                 0, true);
 290         if (!size)
 291                 return -EFAULT;
 292         size = ALIGN(size, 4);
 293
 294         debug("  size is %lx, allocating...\n", size);
 295
 296         /* Allocate memory for the expanded device tree */
 297         mem = memalign(__alignof__(struct device_node), size + 4);
 298         memset(mem, '\0', size);
 299
 300         /* Set up value for dm_test_livetree_align() */
 301         *(u32 *)mem = BAD_OF_ROOT;
 302
 303         *(__be32 *)(mem + size) = cpu_to_be32(0xdeadbeef);
 304
 305         debug("  unflattening %p...\n", mem);
 306
 307         /* Second pass, do actual unflattening */
 308         start = 0;
 309         unflatten_dt_node(blob, mem, &start, NULL, mynodes, 0, false);
 310         if (be32_to_cpup(mem + size) != 0xdeadbeef) {
 311                 debug("End of tree marker overwritten: %08x\n",
 312                       be32_to_cpup(mem + size));
 313                 return -ENOSPC;
 314         }
 315
 316         debug(" <- unflatten_device_tree()\n");
 317
 318         return 0;
 319 }
 320
 321 int of_live_build(const void *fdt_blob, struct device_node **rootp)
 322 {
 323         int ret;
 324
 325         debug("%s: start\n", __func__);
 326         ret = unflatten_device_tree(fdt_blob, rootp);
 327         if (ret) {
 328                 debug("Failed to create live tree: err=%d\n", ret);
 329                 return ret;
 330         }
 331         ret = of_alias_scan();
 332         if (ret) {
 333                 debug("Failed to scan live tree aliases: err=%d\n", ret);
 334                 return ret;
 335         }
 336         debug("%s: stop\n", __func__);
 337
 338         return ret;
 339 }
 340
 341 void of_live_free(struct device_node *root)
 342 {
 343         /* the tree is stored as a contiguous block of memory */
 344         free(root);
 345 }
 346
 347 int of_live_create_empty(struct device_node **rootp)
 348 {
 349         struct device_node *root;
 350
 351         root = calloc(1, sizeof(struct device_node));
 352         if (!root)
 353                 return -ENOMEM;
 354         root->name = strdup("");
 355         if (!root->name) {
 356                 free(root);
 357                 return -ENOMEM;
 358         }
 359         root->type = "<NULL>";
 360         root->full_name = "";
 361         *rootp = root;
 362
 363         return 0;
 364 }
 365
 366 static int check_space(int ret, struct abuf *buf)
 367 {
 368         if (ret == -FDT_ERR_NOSPACE) {
 369                 if (!abuf_realloc_inc(buf, BUF_STEP))
 370                         return log_msg_ret("spc", -ENOMEM);
 371                 ret = fdt_resize(abuf_data(buf), abuf_data(buf),
 372                                  abuf_size(buf));
 373                 if (ret)
 374                         return log_msg_ret("res", -EFAULT);
 375
 376                 return -EAGAIN;
 377         }
 378
 379         return 0;
 380 }
 381
 382 /**
 383  * flatten_node() - Write out the node and its properties into a flat tree
 384  */
 385 static int flatten_node(struct abuf *buf, const struct device_node *node)
 386 {
 387         const struct device_node *np;
 388         const struct property *pp;
 389         int ret;
 390
 391         ret = fdt_begin_node(abuf_data(buf), node->name);
 392         ret = check_space(ret, buf);
 393         if (ret == -EAGAIN) {
 394                 ret = fdt_begin_node(abuf_data(buf), node->name);
 395                 if (ret) {
 396                         log_debug("Internal error a %d\n", ret);
 397                         return -EFAULT;
 398                 }
 399         }
 400         if (ret)
 401                 return log_msg_ret("beg", ret);
 402
 403         /* First write out the properties */
 404         for (pp = node->properties; !ret && pp; pp = pp->next) {
 405                 ret = fdt_property(abuf_data(buf), pp->name, pp->value,
 406                                    pp->length);
 407                 ret = check_space(ret, buf);
 408                 if (ret == -EAGAIN) {
 409                         ret = fdt_property(abuf_data(buf), pp->name, pp->value,
 410                                            pp->length);
 411                 }
 412         }
 413
 414         /* Next write out the subnodes */
 415         for (np = node->child; np; np = np->sibling) {
 416                 ret = flatten_node(buf, np);
 417                 if (ret)
 418                         return log_msg_ret("sub", ret);
 419         }
 420
 421         ret = fdt_end_node(abuf_data(buf));
 422         ret = check_space(ret, buf);
 423         if (ret == -EAGAIN) {
 424                 ret = fdt_end_node(abuf_data(buf));
 425                 if (ret) {
 426                         log_debug("Internal error b %d\n", ret);
 427                         return -EFAULT;
 428                 }
 429         }
 430         if (ret)
 431                 return log_msg_ret("end", ret);
 432
 433         return 0;
 434 }
 435
 436 int of_live_flatten(const struct device_node *root, struct abuf *buf)
 437 {
 438         int ret;
 439
 440         abuf_init(buf);
 441         if (!abuf_realloc(buf, BUF_STEP))
 442                 return log_msg_ret("ini", -ENOMEM);
 443
 444         ret = fdt_create(abuf_data(buf), abuf_size(buf));
 445         if (!ret)
 446                 ret = fdt_finish_reservemap(abuf_data(buf));
 447         if (ret) {
 448                 log_debug("Failed to start FDT (err=%d)\n", ret);
 449                 return log_msg_ret("sta", -EINVAL);
 450         }
 451
 452         ret = flatten_node(buf, root);
 453         if (ret)
 454                 return log_msg_ret("flt", ret);
 455
 456         ret = fdt_finish(abuf_data(buf));
 457         ret = check_space(ret, buf);
 458         if (ret == -EAGAIN) {
 459                 ret = fdt_finish(abuf_data(buf));
 460                 if (ret) {
 461                         log_debug("Internal error c %d\n", ret);
 462                         return -EFAULT;
 463                 }
 464         }
 465         if (ret)
 466                 return log_msg_ret("fin", ret);
 467
 468         ret = fdt_pack(abuf_data(buf));
 469         if (ret) {
 470                 log_debug("Failed to pack (err=%d)\n", ret);
 471                 return log_msg_ret("pac", -EFAULT);
 472         }
 473
 474         if (!abuf_realloc(buf, fdt_totalsize(abuf_data(buf))))
 475                 return log_msg_ret("abu", -EFAULT);
 476
 477         return 0;
 478 }