scripts/dtc/libfdt/fdt_rw.c

   1 // SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
   2 /*
   3  * libfdt - Flat Device Tree manipulation
   4  * Copyright (C) 2006 David Gibson, IBM Corporation.
   5  */
   6 #include "libfdt_env.h"
   7
   8 #include <fdt.h>
   9 #include <libfdt.h>
  10
  11 #include "libfdt_internal.h"
  12
  13 static int fdt_blocks_misordered_(const void *fdt,
  14                                   int mem_rsv_size, int struct_size)
  15 {
  16         return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8))
  17                 || (fdt_off_dt_struct(fdt) <
  18                     (fdt_off_mem_rsvmap(fdt) + mem_rsv_size))
  19                 || (fdt_off_dt_strings(fdt) <
  20                     (fdt_off_dt_struct(fdt) + struct_size))
  21                 || (fdt_totalsize(fdt) <
  22                     (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
  23 }
  24
  25 static int fdt_rw_probe_(void *fdt)
  26 {
  27         if (can_assume(VALID_DTB))
  28                 return 0;
  29         FDT_RO_PROBE(fdt);
  30
  31         if (!can_assume(LATEST) && fdt_version(fdt) < 17)
  32                 return -FDT_ERR_BADVERSION;
  33         if (fdt_blocks_misordered_(fdt, sizeof(struct fdt_reserve_entry),
  34                                    fdt_size_dt_struct(fdt)))
  35                 return -FDT_ERR_BADLAYOUT;
  36         if (!can_assume(LATEST) && fdt_version(fdt) > 17)
  37                 fdt_set_version(fdt, 17);
  38
  39         return 0;
  40 }
  41
  42 #define FDT_RW_PROBE(fdt) \
  43         { \
  44                 int err_; \
  45                 if ((err_ = fdt_rw_probe_(fdt)) != 0) \
  46                         return err_; \
  47         }
  48
  49 static inline unsigned int fdt_data_size_(void *fdt)
  50 {
  51         return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
  52 }
  53
  54 static int fdt_splice_(void *fdt, void *splicepoint, int oldlen, int newlen)
  55 {
  56         char *p = splicepoint;
  57         unsigned int dsize = fdt_data_size_(fdt);
  58         size_t soff = p - (char *)fdt;
  59
  60         if ((oldlen < 0) || (soff + oldlen < soff) || (soff + oldlen > dsize))
  61                 return -FDT_ERR_BADOFFSET;
  62         if ((p < (char *)fdt) || (dsize + newlen < (unsigned)oldlen))
  63                 return -FDT_ERR_BADOFFSET;
  64         if (dsize - oldlen + newlen > fdt_totalsize(fdt))
  65                 return -FDT_ERR_NOSPACE;
  66         memmove(p + newlen, p + oldlen, ((char *)fdt + dsize) - (p + oldlen));
  67         return 0;
  68 }
  69
  70 static int fdt_splice_mem_rsv_(void *fdt, struct fdt_reserve_entry *p,
  71                                int oldn, int newn)
  72 {
  73         int delta = (newn - oldn) * sizeof(*p);
  74         int err;
  75         err = fdt_splice_(fdt, p, oldn * sizeof(*p), newn * sizeof(*p));
  76         if (err)
  77                 return err;
  78         fdt_set_off_dt_struct(fdt, fdt_off_dt_struct(fdt) + delta);
  79         fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
  80         return 0;
  81 }
  82
  83 static int fdt_splice_struct_(void *fdt, void *p,
  84                               int oldlen, int newlen)
  85 {
  86         int delta = newlen - oldlen;
  87         int err;
  88
  89         if ((err = fdt_splice_(fdt, p, oldlen, newlen)))
  90                 return err;
  91
  92         fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta);
  93         fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
  94         return 0;
  95 }
  96
  97 /* Must only be used to roll back in case of error */
  98 static void fdt_del_last_string_(void *fdt, const char *s)
  99 {
 100         int newlen = strlen(s) + 1;
 101
 102         fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) - newlen);
 103 }
 104
 105 static int fdt_splice_string_(void *fdt, int newlen)
 106 {
 107         void *p = (char *)fdt
 108                 + fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
 109         int err;
 110
 111         if ((err = fdt_splice_(fdt, p, 0, newlen)))
 112                 return err;
 113
 114         fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen);
 115         return 0;
 116 }
 117
 118 /**
 119  * fdt_find_add_string_() - Find or allocate a string
 120  *
 121  * @fdt: pointer to the device tree to check/adjust
 122  * @s: string to find/add
 123  * @allocated: Set to 0 if the string was found, 1 if not found and so
 124  *      allocated. Ignored if can_assume(NO_ROLLBACK)
 125  * @return offset of string in the string table (whether found or added)
 126  */
 127 static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
 128 {
 129         char *strtab = (char *)fdt + fdt_off_dt_strings(fdt);
 130         const char *p;
 131         char *new;
 132         int len = strlen(s) + 1;
 133         int err;
 134
 135         if (!can_assume(NO_ROLLBACK))
 136                 *allocated = 0;
 137
 138         p = fdt_find_string_(strtab, fdt_size_dt_strings(fdt), s);
 139         if (p)
 140                 /* found it */
 141                 return (p - strtab);
 142
 143         new = strtab + fdt_size_dt_strings(fdt);
 144         err = fdt_splice_string_(fdt, len);
 145         if (err)
 146                 return err;
 147
 148         if (!can_assume(NO_ROLLBACK))
 149                 *allocated = 1;
 150
 151         memcpy(new, s, len);
 152         return (new - strtab);
 153 }
 154
 155 int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size)
 156 {
 157         struct fdt_reserve_entry *re;
 158         int err;
 159
 160         FDT_RW_PROBE(fdt);
 161
 162         re = fdt_mem_rsv_w_(fdt, fdt_num_mem_rsv(fdt));
 163         err = fdt_splice_mem_rsv_(fdt, re, 0, 1);
 164         if (err)
 165                 return err;
 166
 167         re->address = cpu_to_fdt64(address);
 168         re->size = cpu_to_fdt64(size);
 169         return 0;
 170 }
 171
 172 int fdt_del_mem_rsv(void *fdt, int n)
 173 {
 174         struct fdt_reserve_entry *re = fdt_mem_rsv_w_(fdt, n);
 175
 176         FDT_RW_PROBE(fdt);
 177
 178         if (n >= fdt_num_mem_rsv(fdt))
 179                 return -FDT_ERR_NOTFOUND;
 180
 181         return fdt_splice_mem_rsv_(fdt, re, 1, 0);
 182 }
 183
 184 static int fdt_resize_property_(void *fdt, int nodeoffset, const char *name,
 185                                 int len, struct fdt_property **prop)
 186 {
 187         int oldlen;
 188         int err;
 189
 190         *prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
 191         if (!*prop)
 192                 return oldlen;
 193
 194         if ((err = fdt_splice_struct_(fdt, (*prop)->data, FDT_TAGALIGN(oldlen),
 195                                       FDT_TAGALIGN(len))))
 196                 return err;
 197
 198         (*prop)->len = cpu_to_fdt32(len);
 199         return 0;
 200 }
 201
 202 static int fdt_add_property_(void *fdt, int nodeoffset, const char *name,
 203                              int len, struct fdt_property **prop)
 204 {
 205         int proplen;
 206         int nextoffset;
 207         int namestroff;
 208         int err;
 209         int allocated;
 210
 211         if ((nextoffset = fdt_check_node_offset_(fdt, nodeoffset)) < 0)
 212                 return nextoffset;
 213
 214         namestroff = fdt_find_add_string_(fdt, name, &allocated);
 215         if (namestroff < 0)
 216                 return namestroff;
 217
 218         *prop = fdt_offset_ptr_w_(fdt, nextoffset);
 219         proplen = sizeof(**prop) + FDT_TAGALIGN(len);
 220
 221         err = fdt_splice_struct_(fdt, *prop, 0, proplen);
 222         if (err) {
 223                 /* Delete the string if we failed to add it */
 224                 if (!can_assume(NO_ROLLBACK) && allocated)
 225                         fdt_del_last_string_(fdt, name);
 226                 return err;
 227         }
 228
 229         (*prop)->tag = cpu_to_fdt32(FDT_PROP);
 230         (*prop)->nameoff = cpu_to_fdt32(namestroff);
 231         (*prop)->len = cpu_to_fdt32(len);
 232         return 0;
 233 }
 234
 235 int fdt_set_name(void *fdt, int nodeoffset, const char *name)
 236 {
 237         char *namep;
 238         int oldlen, newlen;
 239         int err;
 240
 241         FDT_RW_PROBE(fdt);
 242
 243         namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen);
 244         if (!namep)
 245                 return oldlen;
 246
 247         newlen = strlen(name);
 248
 249         err = fdt_splice_struct_(fdt, namep, FDT_TAGALIGN(oldlen+1),
 250                                  FDT_TAGALIGN(newlen+1));
 251         if (err)
 252                 return err;
 253
 254         memcpy(namep, name, newlen+1);
 255         return 0;
 256 }
 257
 258 int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name,
 259                             int len, void **prop_data)
 260 {
 261         struct fdt_property *prop;
 262         int err;
 263
 264         FDT_RW_PROBE(fdt);
 265
 266         err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop);
 267         if (err == -FDT_ERR_NOTFOUND)
 268                 err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
 269         if (err)
 270                 return err;
 271
 272         *prop_data = prop->data;
 273         return 0;
 274 }
 275
 276 int fdt_setprop(void *fdt, int nodeoffset, const char *name,
 277                 const void *val, int len)
 278 {
 279         void *prop_data;
 280         int err;
 281
 282         err = fdt_setprop_placeholder(fdt, nodeoffset, name, len, &prop_data);
 283         if (err)
 284                 return err;
 285
 286         if (len)
 287                 memcpy(prop_data, val, len);
 288         return 0;
 289 }
 290
 291 int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
 292                    const void *val, int len)
 293 {
 294         struct fdt_property *prop;
 295         int err, oldlen, newlen;
 296
 297         FDT_RW_PROBE(fdt);
 298
 299         prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
 300         if (prop) {
 301                 newlen = len + oldlen;
 302                 err = fdt_splice_struct_(fdt, prop->data,
 303                                          FDT_TAGALIGN(oldlen),
 304                                          FDT_TAGALIGN(newlen));
 305                 if (err)
 306                         return err;
 307                 prop->len = cpu_to_fdt32(newlen);
 308                 memcpy(prop->data + oldlen, val, len);
 309         } else {
 310                 err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
 311                 if (err)
 312                         return err;
 313                 memcpy(prop->data, val, len);
 314         }
 315         return 0;
 316 }
 317
 318 int fdt_delprop(void *fdt, int nodeoffset, const char *name)
 319 {
 320         struct fdt_property *prop;
 321         int len, proplen;
 322
 323         FDT_RW_PROBE(fdt);
 324
 325         prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
 326         if (!prop)
 327                 return len;
 328
 329         proplen = sizeof(*prop) + FDT_TAGALIGN(len);
 330         return fdt_splice_struct_(fdt, prop, proplen, 0);
 331 }
 332
 333 int fdt_add_subnode_namelen(void *fdt, int parentoffset,
 334                             const char *name, int namelen)
 335 {
 336         struct fdt_node_header *nh;
 337         int offset, nextoffset;
 338         int nodelen;
 339         int err;
 340         uint32_t tag;
 341         fdt32_t *endtag;
 342
 343         FDT_RW_PROBE(fdt);
 344
 345         offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen);
 346         if (offset >= 0)
 347                 return -FDT_ERR_EXISTS;
 348         else if (offset != -FDT_ERR_NOTFOUND)
 349                 return offset;
 350
 351         /* Try to place the new node after the parent's properties */
 352         fdt_next_tag(fdt, parentoffset, &nextoffset); /* skip the BEGIN_NODE */
 353         do {
 354                 offset = nextoffset;
 355                 tag = fdt_next_tag(fdt, offset, &nextoffset);
 356         } while ((tag == FDT_PROP) || (tag == FDT_NOP));
 357
 358         nh = fdt_offset_ptr_w_(fdt, offset);
 359         nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE;
 360
 361         err = fdt_splice_struct_(fdt, nh, 0, nodelen);
 362         if (err)
 363                 return err;
 364
 365         nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
 366         memset(nh->name, 0, FDT_TAGALIGN(namelen+1));
 367         memcpy(nh->name, name, namelen);
 368         endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE);
 369         *endtag = cpu_to_fdt32(FDT_END_NODE);
 370
 371         return offset;
 372 }
 373
 374 int fdt_add_subnode(void *fdt, int parentoffset, const char *name)
 375 {
 376         return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name));
 377 }
 378
 379 int fdt_del_node(void *fdt, int nodeoffset)
 380 {
 381         int endoffset;
 382
 383         FDT_RW_PROBE(fdt);
 384
 385         endoffset = fdt_node_end_offset_(fdt, nodeoffset);
 386         if (endoffset < 0)
 387                 return endoffset;
 388
 389         return fdt_splice_struct_(fdt, fdt_offset_ptr_w_(fdt, nodeoffset),
 390                                   endoffset - nodeoffset, 0);
 391 }
 392
 393 static void fdt_packblocks_(const char *old, char *new,
 394                             int mem_rsv_size, int struct_size)
 395 {
 396         int mem_rsv_off, struct_off, strings_off;
 397
 398         mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8);
 399         struct_off = mem_rsv_off + mem_rsv_size;
 400         strings_off = struct_off + struct_size;
 401
 402         memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size);
 403         fdt_set_off_mem_rsvmap(new, mem_rsv_off);
 404
 405         memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size);
 406         fdt_set_off_dt_struct(new, struct_off);
 407         fdt_set_size_dt_struct(new, struct_size);
 408
 409         memmove(new + strings_off, old + fdt_off_dt_strings(old),
 410                 fdt_size_dt_strings(old));
 411         fdt_set_off_dt_strings(new, strings_off);
 412         fdt_set_size_dt_strings(new, fdt_size_dt_strings(old));
 413 }
 414
 415 int fdt_open_into(const void *fdt, void *buf, int bufsize)
 416 {
 417         int err;
 418         int mem_rsv_size, struct_size;
 419         int newsize;
 420         const char *fdtstart = fdt;
 421         const char *fdtend = fdtstart + fdt_totalsize(fdt);
 422         char *tmp;
 423
 424         FDT_RO_PROBE(fdt);
 425
 426         mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
 427                 * sizeof(struct fdt_reserve_entry);
 428
 429         if (can_assume(LATEST) || fdt_version(fdt) >= 17) {
 430                 struct_size = fdt_size_dt_struct(fdt);
 431         } else {
 432                 struct_size = 0;
 433                 while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END)
 434                         ;
 435                 if (struct_size < 0)
 436                         return struct_size;
 437         }
 438
 439         if (can_assume(LIBFDT_ORDER) ||
 440             !fdt_blocks_misordered_(fdt, mem_rsv_size, struct_size)) {
 441                 /* no further work necessary */
 442                 err = fdt_move(fdt, buf, bufsize);
 443                 if (err)
 444                         return err;
 445                 fdt_set_version(buf, 17);
 446                 fdt_set_size_dt_struct(buf, struct_size);
 447                 fdt_set_totalsize(buf, bufsize);
 448                 return 0;
 449         }
 450
 451         /* Need to reorder */
 452         newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size
 453                 + struct_size + fdt_size_dt_strings(fdt);
 454
 455         if (bufsize < newsize)
 456                 return -FDT_ERR_NOSPACE;
 457
 458         /* First attempt to build converted tree at beginning of buffer */
 459         tmp = buf;
 460         /* But if that overlaps with the old tree... */
 461         if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) {
 462                 /* Try right after the old tree instead */
 463                 tmp = (char *)(uintptr_t)fdtend;
 464                 if ((tmp + newsize) > ((char *)buf + bufsize))
 465                         return -FDT_ERR_NOSPACE;
 466         }
 467
 468         fdt_packblocks_(fdt, tmp, mem_rsv_size, struct_size);
 469         memmove(buf, tmp, newsize);
 470
 471         fdt_set_magic(buf, FDT_MAGIC);
 472         fdt_set_totalsize(buf, bufsize);
 473         fdt_set_version(buf, 17);
 474         fdt_set_last_comp_version(buf, 16);
 475         fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt));
 476
 477         return 0;
 478 }
 479
 480 int fdt_pack(void *fdt)
 481 {
 482         int mem_rsv_size;
 483
 484         FDT_RW_PROBE(fdt);
 485
 486         mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
 487                 * sizeof(struct fdt_reserve_entry);
 488         fdt_packblocks_(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt));
 489         fdt_set_totalsize(fdt, fdt_data_size_(fdt));
 490
 491         return 0;
 492 }