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