lib/libfdt/fdt_sw.c

   1 /*
   2  * libfdt - Flat Device Tree manipulation
   3  * Copyright (C) 2006 David Gibson, IBM Corporation.
   4  * SPDX-License-Identifier:     GPL-2.0+ BSD-2-Clause
   5  */
   6 #include <libfdt_env.h>
   7 #include <fdt.h>
   8 #include <libfdt.h>
   9
  10 #include "libfdt_internal.h"
  11
  12 static int _fdt_sw_check_header(void *fdt)
  13 {
  14         if (fdt_magic(fdt) != FDT_SW_MAGIC)
  15                 return -FDT_ERR_BADMAGIC;
  16         /* FIXME: should check more details about the header state */
  17         return 0;
  18 }
  19
  20 #define FDT_SW_CHECK_HEADER(fdt) \
  21         { \
  22                 int err; \
  23                 if ((err = _fdt_sw_check_header(fdt)) != 0) \
  24                         return err; \
  25         }
  26
  27 static void *_fdt_grab_space(void *fdt, size_t len)
  28 {
  29         int offset = fdt_size_dt_struct(fdt);
  30         int spaceleft;
  31
  32         spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
  33                 - fdt_size_dt_strings(fdt);
  34
  35         if ((offset + len < offset) || (offset + len > spaceleft))
  36                 return NULL;
  37
  38         fdt_set_size_dt_struct(fdt, offset + len);
  39         return _fdt_offset_ptr_w(fdt, offset);
  40 }
  41
  42 int fdt_create(void *buf, int bufsize)
  43 {
  44         void *fdt = buf;
  45
  46         if (bufsize < sizeof(struct fdt_header))
  47                 return -FDT_ERR_NOSPACE;
  48
  49         memset(buf, 0, bufsize);
  50
  51         fdt_set_magic(fdt, FDT_SW_MAGIC);
  52         fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
  53         fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
  54         fdt_set_totalsize(fdt,  bufsize);
  55
  56         fdt_set_off_mem_rsvmap(fdt, FDT_ALIGN(sizeof(struct fdt_header),
  57                                               sizeof(struct fdt_reserve_entry)));
  58         fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
  59         fdt_set_off_dt_strings(fdt, bufsize);
  60
  61         return 0;
  62 }
  63
  64 int fdt_resize(void *fdt, void *buf, int bufsize)
  65 {
  66         size_t headsize, tailsize;
  67         char *oldtail, *newtail;
  68
  69         FDT_SW_CHECK_HEADER(fdt);
  70
  71         headsize = fdt_off_dt_struct(fdt);
  72         tailsize = fdt_size_dt_strings(fdt);
  73
  74         if ((headsize + tailsize) > bufsize)
  75                 return -FDT_ERR_NOSPACE;
  76
  77         oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
  78         newtail = (char *)buf + bufsize - tailsize;
  79
  80         /* Two cases to avoid clobbering data if the old and new
  81          * buffers partially overlap */
  82         if (buf <= fdt) {
  83                 memmove(buf, fdt, headsize);
  84                 memmove(newtail, oldtail, tailsize);
  85         } else {
  86                 memmove(newtail, oldtail, tailsize);
  87                 memmove(buf, fdt, headsize);
  88         }
  89
  90         fdt_set_off_dt_strings(buf, bufsize);
  91         fdt_set_totalsize(buf, bufsize);
  92
  93         return 0;
  94 }
  95
  96 int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
  97 {
  98         struct fdt_reserve_entry *re;
  99         int offset;
 100
 101         FDT_SW_CHECK_HEADER(fdt);
 102
 103         if (fdt_size_dt_struct(fdt))
 104                 return -FDT_ERR_BADSTATE;
 105
 106         offset = fdt_off_dt_struct(fdt);
 107         if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
 108                 return -FDT_ERR_NOSPACE;
 109
 110         re = (struct fdt_reserve_entry *)((char *)fdt + offset);
 111         re->address = cpu_to_fdt64(addr);
 112         re->size = cpu_to_fdt64(size);
 113
 114         fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
 115
 116         return 0;
 117 }
 118
 119 int fdt_finish_reservemap(void *fdt)
 120 {
 121         return fdt_add_reservemap_entry(fdt, 0, 0);
 122 }
 123
 124 int fdt_begin_node(void *fdt, const char *name)
 125 {
 126         struct fdt_node_header *nh;
 127         int namelen = strlen(name) + 1;
 128
 129         FDT_SW_CHECK_HEADER(fdt);
 130
 131         nh = _fdt_grab_space(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
 132         if (! nh)
 133                 return -FDT_ERR_NOSPACE;
 134
 135         nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
 136         memcpy(nh->name, name, namelen);
 137         return 0;
 138 }
 139
 140 int fdt_end_node(void *fdt)
 141 {
 142         fdt32_t *en;
 143
 144         FDT_SW_CHECK_HEADER(fdt);
 145
 146         en = _fdt_grab_space(fdt, FDT_TAGSIZE);
 147         if (! en)
 148                 return -FDT_ERR_NOSPACE;
 149
 150         *en = cpu_to_fdt32(FDT_END_NODE);
 151         return 0;
 152 }
 153
 154 static int _fdt_find_add_string(void *fdt, const char *s)
 155 {
 156         char *strtab = (char *)fdt + fdt_totalsize(fdt);
 157         const char *p;
 158         int strtabsize = fdt_size_dt_strings(fdt);
 159         int len = strlen(s) + 1;
 160         int struct_top, offset;
 161
 162         p = _fdt_find_string(strtab - strtabsize, strtabsize, s);
 163         if (p)
 164                 return p - strtab;
 165
 166         /* Add it */
 167         offset = -strtabsize - len;
 168         struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 169         if (fdt_totalsize(fdt) + offset < struct_top)
 170                 return 0; /* no more room :( */
 171
 172         memcpy(strtab + offset, s, len);
 173         fdt_set_size_dt_strings(fdt, strtabsize + len);
 174         return offset;
 175 }
 176
 177 int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp)
 178 {
 179         struct fdt_property *prop;
 180         int nameoff;
 181
 182         FDT_SW_CHECK_HEADER(fdt);
 183
 184         nameoff = _fdt_find_add_string(fdt, name);
 185         if (nameoff == 0)
 186                 return -FDT_ERR_NOSPACE;
 187
 188         prop = _fdt_grab_space(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
 189         if (! prop)
 190                 return -FDT_ERR_NOSPACE;
 191
 192         prop->tag = cpu_to_fdt32(FDT_PROP);
 193         prop->nameoff = cpu_to_fdt32(nameoff);
 194         prop->len = cpu_to_fdt32(len);
 195         *valp = prop->data;
 196         return 0;
 197 }
 198
 199 int fdt_property(void *fdt, const char *name, const void *val, int len)
 200 {
 201         void *ptr;
 202         int ret;
 203
 204         ret = fdt_property_placeholder(fdt, name, len, &ptr);
 205         if (ret)
 206                 return ret;
 207         memcpy(ptr, val, len);
 208         return 0;
 209 }
 210
 211 int fdt_finish(void *fdt)
 212 {
 213         char *p = (char *)fdt;
 214         fdt32_t *end;
 215         int oldstroffset, newstroffset;
 216         uint32_t tag;
 217         int offset, nextoffset;
 218
 219         FDT_SW_CHECK_HEADER(fdt);
 220
 221         /* Add terminator */
 222         end = _fdt_grab_space(fdt, sizeof(*end));
 223         if (! end)
 224                 return -FDT_ERR_NOSPACE;
 225         *end = cpu_to_fdt32(FDT_END);
 226
 227         /* Relocate the string table */
 228         oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
 229         newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 230         memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
 231         fdt_set_off_dt_strings(fdt, newstroffset);
 232
 233         /* Walk the structure, correcting string offsets */
 234         offset = 0;
 235         while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
 236                 if (tag == FDT_PROP) {
 237                         struct fdt_property *prop =
 238                                 _fdt_offset_ptr_w(fdt, offset);
 239                         int nameoff;
 240
 241                         nameoff = fdt32_to_cpu(prop->nameoff);
 242                         nameoff += fdt_size_dt_strings(fdt);
 243                         prop->nameoff = cpu_to_fdt32(nameoff);
 244                 }
 245                 offset = nextoffset;
 246         }
 247         if (nextoffset < 0)
 248                 return nextoffset;
 249
 250         /* Finally, adjust the header */
 251         fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
 252         fdt_set_magic(fdt, FDT_MAGIC);
 253         return 0;
 254 }