dtc/libfdt/fdt_rw.c

0001 // SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
0002 /*
0003  * libfdt - Flat Device Tree manipulation
0004  * Copyright (C) 2006 David Gibson, IBM Corporation.
0005  */
0006 #include "libfdt_env.h"
0007
0008 #include <fdt.h>
0009 #include <libfdt.h>
0010
0011 #include "libfdt_internal.h"
0012
0013 static int fdt_blocks_misordered_(const void *fdt,
0014                   int mem_rsv_size, int struct_size)
0015 {
0016     return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8))
0017         || (fdt_off_dt_struct(fdt) <
0018             (fdt_off_mem_rsvmap(fdt) + mem_rsv_size))
0019         || (fdt_off_dt_strings(fdt) <
0020             (fdt_off_dt_struct(fdt) + struct_size))
0021         || (fdt_totalsize(fdt) <
0022             (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
0023 }
0024
0025 static int fdt_rw_probe_(void *fdt)
0026 {
0027     if (can_assume(VALID_DTB))
0028         return 0;
0029     FDT_RO_PROBE(fdt);
0030
0031     if (!can_assume(LATEST) && fdt_version(fdt) < 17)
0032         return -FDT_ERR_BADVERSION;
0033     if (fdt_blocks_misordered_(fdt, sizeof(struct fdt_reserve_entry),
0034                    fdt_size_dt_struct(fdt)))
0035         return -FDT_ERR_BADLAYOUT;
0036     if (!can_assume(LATEST) && fdt_version(fdt) > 17)
0037         fdt_set_version(fdt, 17);
0038
0039     return 0;
0040 }
0041
0042 #define FDT_RW_PROBE(fdt) \
0043     { \
0044         int err_; \
0045         if ((err_ = fdt_rw_probe_(fdt)) != 0) \
0046             return err_; \
0047     }
0048
0049 static inline unsigned int fdt_data_size_(void *fdt)
0050 {
0051     return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
0052 }
0053
0054 static int fdt_splice_(void *fdt, void *splicepoint, int oldlen, int newlen)
0055 {
0056     char *p = splicepoint;
0057     unsigned int dsize = fdt_data_size_(fdt);
0058     size_t soff = p - (char *)fdt;
0059
0060     if ((oldlen < 0) || (soff + oldlen < soff) || (soff + oldlen > dsize))
0061         return -FDT_ERR_BADOFFSET;
0062     if ((p < (char *)fdt) || (dsize + newlen < (unsigned)oldlen))
0063         return -FDT_ERR_BADOFFSET;
0064     if (dsize - oldlen + newlen > fdt_totalsize(fdt))
0065         return -FDT_ERR_NOSPACE;
0066     memmove(p + newlen, p + oldlen, ((char *)fdt + dsize) - (p + oldlen));
0067     return 0;
0068 }
0069
0070 static int fdt_splice_mem_rsv_(void *fdt, struct fdt_reserve_entry *p,
0071                    int oldn, int newn)
0072 {
0073     int delta = (newn - oldn) * sizeof(*p);
0074     int err;
0075     err = fdt_splice_(fdt, p, oldn * sizeof(*p), newn * sizeof(*p));
0076     if (err)
0077         return err;
0078     fdt_set_off_dt_struct(fdt, fdt_off_dt_struct(fdt) + delta);
0079     fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
0080     return 0;
0081 }
0082
0083 static int fdt_splice_struct_(void *fdt, void *p,
0084                   int oldlen, int newlen)
0085 {
0086     int delta = newlen - oldlen;
0087     int err;
0088
0089     if ((err = fdt_splice_(fdt, p, oldlen, newlen)))
0090         return err;
0091
0092     fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta);
0093     fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
0094     return 0;
0095 }
0096
0097 /* Must only be used to roll back in case of error */
0098 static void fdt_del_last_string_(void *fdt, const char *s)
0099 {
0100     int newlen = strlen(s) + 1;
0101
0102     fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) - newlen);
0103 }
0104
0105 static int fdt_splice_string_(void *fdt, int newlen)
0106 {
0107     void *p = (char *)fdt
0108         + fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
0109     int err;
0110
0111     if ((err = fdt_splice_(fdt, p, 0, newlen)))
0112         return err;
0113
0114     fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen);
0115     return 0;
0116 }
0117
0118 /**
0119  * fdt_find_add_string_() - Find or allocate a string
0120  *
0121  * @fdt: pointer to the device tree to check/adjust
0122  * @s: string to find/add
0123  * @allocated: Set to 0 if the string was found, 1 if not found and so
0124  *  allocated. Ignored if can_assume(NO_ROLLBACK)
0125  * @return offset of string in the string table (whether found or added)
0126  */
0127 static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
0128 {
0129     char *strtab = (char *)fdt + fdt_off_dt_strings(fdt);
0130     const char *p;
0131     char *new;
0132     int len = strlen(s) + 1;
0133     int err;
0134
0135     if (!can_assume(NO_ROLLBACK))
0136         *allocated = 0;
0137
0138     p = fdt_find_string_(strtab, fdt_size_dt_strings(fdt), s);
0139     if (p)
0140         /* found it */
0141         return (p - strtab);
0142
0143     new = strtab + fdt_size_dt_strings(fdt);
0144     err = fdt_splice_string_(fdt, len);
0145     if (err)
0146         return err;
0147
0148     if (!can_assume(NO_ROLLBACK))
0149         *allocated = 1;
0150
0151     memcpy(new, s, len);
0152     return (new - strtab);
0153 }
0154
0155 int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size)
0156 {
0157     struct fdt_reserve_entry *re;
0158     int err;
0159
0160     FDT_RW_PROBE(fdt);
0161
0162     re = fdt_mem_rsv_w_(fdt, fdt_num_mem_rsv(fdt));
0163     err = fdt_splice_mem_rsv_(fdt, re, 0, 1);
0164     if (err)
0165         return err;
0166
0167     re->address = cpu_to_fdt64(address);
0168     re->size = cpu_to_fdt64(size);
0169     return 0;
0170 }
0171
0172 int fdt_del_mem_rsv(void *fdt, int n)
0173 {
0174     struct fdt_reserve_entry *re = fdt_mem_rsv_w_(fdt, n);
0175
0176     FDT_RW_PROBE(fdt);
0177
0178     if (n >= fdt_num_mem_rsv(fdt))
0179         return -FDT_ERR_NOTFOUND;
0180
0181     return fdt_splice_mem_rsv_(fdt, re, 1, 0);
0182 }
0183
0184 static int fdt_resize_property_(void *fdt, int nodeoffset, const char *name,
0185                 int len, struct fdt_property **prop)
0186 {
0187     int oldlen;
0188     int err;
0189
0190     *prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
0191     if (!*prop)
0192         return oldlen;
0193
0194     if ((err = fdt_splice_struct_(fdt, (*prop)->data, FDT_TAGALIGN(oldlen),
0195                       FDT_TAGALIGN(len))))
0196         return err;
0197
0198     (*prop)->len = cpu_to_fdt32(len);
0199     return 0;
0200 }
0201
0202 static int fdt_add_property_(void *fdt, int nodeoffset, const char *name,
0203                  int len, struct fdt_property **prop)
0204 {
0205     int proplen;
0206     int nextoffset;
0207     int namestroff;
0208     int err;
0209     int allocated;
0210
0211     if ((nextoffset = fdt_check_node_offset_(fdt, nodeoffset)) < 0)
0212         return nextoffset;
0213
0214     namestroff = fdt_find_add_string_(fdt, name, &allocated);
0215     if (namestroff < 0)
0216         return namestroff;
0217
0218     *prop = fdt_offset_ptr_w_(fdt, nextoffset);
0219     proplen = sizeof(**prop) + FDT_TAGALIGN(len);
0220
0221     err = fdt_splice_struct_(fdt, *prop, 0, proplen);
0222     if (err) {
0223         /* Delete the string if we failed to add it */
0224         if (!can_assume(NO_ROLLBACK) && allocated)
0225             fdt_del_last_string_(fdt, name);
0226         return err;
0227     }
0228
0229     (*prop)->tag = cpu_to_fdt32(FDT_PROP);
0230     (*prop)->nameoff = cpu_to_fdt32(namestroff);
0231     (*prop)->len = cpu_to_fdt32(len);
0232     return 0;
0233 }
0234
0235 int fdt_set_name(void *fdt, int nodeoffset, const char *name)
0236 {
0237     char *namep;
0238     int oldlen, newlen;
0239     int err;
0240
0241     FDT_RW_PROBE(fdt);
0242
0243     namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen);
0244     if (!namep)
0245         return oldlen;
0246
0247     newlen = strlen(name);
0248
0249     err = fdt_splice_struct_(fdt, namep, FDT_TAGALIGN(oldlen+1),
0250                  FDT_TAGALIGN(newlen+1));
0251     if (err)
0252         return err;
0253
0254     memcpy(namep, name, newlen+1);
0255     return 0;
0256 }
0257
0258 int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name,
0259                 int len, void **prop_data)
0260 {
0261     struct fdt_property *prop;
0262     int err;
0263
0264     FDT_RW_PROBE(fdt);
0265
0266     err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop);
0267     if (err == -FDT_ERR_NOTFOUND)
0268         err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
0269     if (err)
0270         return err;
0271
0272     *prop_data = prop->data;
0273     return 0;
0274 }
0275
0276 int fdt_setprop(void *fdt, int nodeoffset, const char *name,
0277         const void *val, int len)
0278 {
0279     void *prop_data;
0280     int err;
0281
0282     err = fdt_setprop_placeholder(fdt, nodeoffset, name, len, &prop_data);
0283     if (err)
0284         return err;
0285
0286     if (len)
0287         memcpy(prop_data, val, len);
0288     return 0;
0289 }
0290
0291 int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
0292            const void *val, int len)
0293 {
0294     struct fdt_property *prop;
0295     int err, oldlen, newlen;
0296
0297     FDT_RW_PROBE(fdt);
0298
0299     prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
0300     if (prop) {
0301         newlen = len + oldlen;
0302         err = fdt_splice_struct_(fdt, prop->data,
0303                      FDT_TAGALIGN(oldlen),
0304                      FDT_TAGALIGN(newlen));
0305         if (err)
0306             return err;
0307         prop->len = cpu_to_fdt32(newlen);
0308         memcpy(prop->data + oldlen, val, len);
0309     } else {
0310         err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
0311         if (err)
0312             return err;
0313         memcpy(prop->data, val, len);
0314     }
0315     return 0;
0316 }
0317
0318 int fdt_delprop(void *fdt, int nodeoffset, const char *name)
0319 {
0320     struct fdt_property *prop;
0321     int len, proplen;
0322
0323     FDT_RW_PROBE(fdt);
0324
0325     prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
0326     if (!prop)
0327         return len;
0328
0329     proplen = sizeof(*prop) + FDT_TAGALIGN(len);
0330     return fdt_splice_struct_(fdt, prop, proplen, 0);
0331 }
0332
0333 int fdt_add_subnode_namelen(void *fdt, int parentoffset,
0334                 const char *name, int namelen)
0335 {
0336     struct fdt_node_header *nh;
0337     int offset, nextoffset;
0338     int nodelen;
0339     int err;
0340     uint32_t tag;
0341     fdt32_t *endtag;
0342
0343     FDT_RW_PROBE(fdt);
0344
0345     offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen);
0346     if (offset >= 0)
0347         return -FDT_ERR_EXISTS;
0348     else if (offset != -FDT_ERR_NOTFOUND)
0349         return offset;
0350
0351     /* Try to place the new node after the parent's properties */
0352     tag = fdt_next_tag(fdt, parentoffset, &nextoffset);
0353     /* the fdt_subnode_offset_namelen() should ensure this never hits */
0354     if (!can_assume(LIBFDT_FLAWLESS) && (tag != FDT_BEGIN_NODE))
0355         return -FDT_ERR_INTERNAL;
0356     do {
0357         offset = nextoffset;
0358         tag = fdt_next_tag(fdt, offset, &nextoffset);
0359     } while ((tag == FDT_PROP) || (tag == FDT_NOP));
0360
0361     nh = fdt_offset_ptr_w_(fdt, offset);
0362     nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE;
0363
0364     err = fdt_splice_struct_(fdt, nh, 0, nodelen);
0365     if (err)
0366         return err;
0367
0368     nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
0369     memset(nh->name, 0, FDT_TAGALIGN(namelen+1));
0370     memcpy(nh->name, name, namelen);
0371     endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE);
0372     *endtag = cpu_to_fdt32(FDT_END_NODE);
0373
0374     return offset;
0375 }
0376
0377 int fdt_add_subnode(void *fdt, int parentoffset, const char *name)
0378 {
0379     return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name));
0380 }
0381
0382 int fdt_del_node(void *fdt, int nodeoffset)
0383 {
0384     int endoffset;
0385
0386     FDT_RW_PROBE(fdt);
0387
0388     endoffset = fdt_node_end_offset_(fdt, nodeoffset);
0389     if (endoffset < 0)
0390         return endoffset;
0391
0392     return fdt_splice_struct_(fdt, fdt_offset_ptr_w_(fdt, nodeoffset),
0393                   endoffset - nodeoffset, 0);
0394 }
0395
0396 static void fdt_packblocks_(const char *old, char *new,
0397                 int mem_rsv_size,
0398                 int struct_size,
0399                 int strings_size)
0400 {
0401     int mem_rsv_off, struct_off, strings_off;
0402
0403     mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8);
0404     struct_off = mem_rsv_off + mem_rsv_size;
0405     strings_off = struct_off + struct_size;
0406
0407     memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size);
0408     fdt_set_off_mem_rsvmap(new, mem_rsv_off);
0409
0410     memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size);
0411     fdt_set_off_dt_struct(new, struct_off);
0412     fdt_set_size_dt_struct(new, struct_size);
0413
0414     memmove(new + strings_off, old + fdt_off_dt_strings(old), strings_size);
0415     fdt_set_off_dt_strings(new, strings_off);
0416     fdt_set_size_dt_strings(new, fdt_size_dt_strings(old));
0417 }
0418
0419 int fdt_open_into(const void *fdt, void *buf, int bufsize)
0420 {
0421     int err;
0422     int mem_rsv_size, struct_size;
0423     int newsize;
0424     const char *fdtstart = fdt;
0425     const char *fdtend = fdtstart + fdt_totalsize(fdt);
0426     char *tmp;
0427
0428     FDT_RO_PROBE(fdt);
0429
0430     mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
0431         * sizeof(struct fdt_reserve_entry);
0432
0433     if (can_assume(LATEST) || fdt_version(fdt) >= 17) {
0434         struct_size = fdt_size_dt_struct(fdt);
0435     } else if (fdt_version(fdt) == 16) {
0436         struct_size = 0;
0437         while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END)
0438             ;
0439         if (struct_size < 0)
0440             return struct_size;
0441     } else {
0442         return -FDT_ERR_BADVERSION;
0443     }
0444
0445     if (can_assume(LIBFDT_ORDER) ||
0446         !fdt_blocks_misordered_(fdt, mem_rsv_size, struct_size)) {
0447         /* no further work necessary */
0448         err = fdt_move(fdt, buf, bufsize);
0449         if (err)
0450             return err;
0451         fdt_set_version(buf, 17);
0452         fdt_set_size_dt_struct(buf, struct_size);
0453         fdt_set_totalsize(buf, bufsize);
0454         return 0;
0455     }
0456
0457     /* Need to reorder */
0458     newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size
0459         + struct_size + fdt_size_dt_strings(fdt);
0460
0461     if (bufsize < newsize)
0462         return -FDT_ERR_NOSPACE;
0463
0464     /* First attempt to build converted tree at beginning of buffer */
0465     tmp = buf;
0466     /* But if that overlaps with the old tree... */
0467     if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) {
0468         /* Try right after the old tree instead */
0469         tmp = (char *)(uintptr_t)fdtend;
0470         if ((tmp + newsize) > ((char *)buf + bufsize))
0471             return -FDT_ERR_NOSPACE;
0472     }
0473
0474     fdt_packblocks_(fdt, tmp, mem_rsv_size, struct_size,
0475             fdt_size_dt_strings(fdt));
0476     memmove(buf, tmp, newsize);
0477
0478     fdt_set_magic(buf, FDT_MAGIC);
0479     fdt_set_totalsize(buf, bufsize);
0480     fdt_set_version(buf, 17);
0481     fdt_set_last_comp_version(buf, 16);
0482     fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt));
0483
0484     return 0;
0485 }
0486
0487 int fdt_pack(void *fdt)
0488 {
0489     int mem_rsv_size;
0490
0491     FDT_RW_PROBE(fdt);
0492
0493     mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
0494         * sizeof(struct fdt_reserve_entry);
0495     fdt_packblocks_(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt),
0496             fdt_size_dt_strings(fdt));
0497     fdt_set_totalsize(fdt, fdt_data_size_(fdt));
0498
0499     return 0;
0500 }