libfdt/fdt_sw.c

   1 /*
   2  * libfdt - Flat Device Tree manipulation
   3  * Copyright (C) 2006 David Gibson, IBM Corporation.
   4  *
   5  * This library is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU Lesser General Public License
   7  * as published by the Free Software Foundation; either version 2.1 of
   8  * the License, or (at your option) any later version.
   9  *
  10  * This library is distributed in the hope that it will be useful, but
  11  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * Lesser General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU Lesser General Public
  16  * License along with this library; if not, write to the Free Software
  17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  18  */
  19 #include "libfdt_env.h"
  20
  21 #include <fdt.h>
  22 #include <libfdt.h>
  23
  24 #include "libfdt_internal.h"
  25
  26 static int check_header_sw(void *fdt)
  27 {
  28         if (fdt_magic(fdt) != SW_MAGIC)
  29                 return -FDT_ERR_BADMAGIC;
  30         return 0;
  31 }
  32
  33 static void *grab_space(void *fdt, int len)
  34 {
  35         int offset = fdt_size_dt_struct(fdt);
  36         int spaceleft;
  37
  38         spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
  39                 - fdt_size_dt_strings(fdt);
  40
  41         if ((offset + len < offset) || (offset + len > spaceleft))
  42                 return NULL;
  43
  44         fdt_set_header(fdt, size_dt_struct, offset + len);
  45         return fdt_offset_ptr(fdt, offset, len);
  46 }
  47
  48 int fdt_create(void *buf, int bufsize)
  49 {
  50         void *fdt = buf;
  51
  52         if (bufsize < sizeof(struct fdt_header))
  53                 return -FDT_ERR_NOSPACE;
  54
  55         memset(buf, 0, bufsize);
  56
  57         fdt_set_header(fdt, magic, SW_MAGIC);
  58         fdt_set_header(fdt, version, FDT_LAST_SUPPORTED_VERSION);
  59         fdt_set_header(fdt, last_comp_version, FDT_FIRST_SUPPORTED_VERSION);
  60         fdt_set_header(fdt, totalsize, bufsize);
  61
  62         fdt_set_header(fdt, off_mem_rsvmap, ALIGN(sizeof(struct fdt_header),
  63                                               sizeof(struct fdt_reserve_entry)));
  64         fdt_set_header(fdt, off_dt_struct, fdt_off_mem_rsvmap(fdt));
  65         fdt_set_header(fdt, off_dt_strings, bufsize);
  66
  67         return 0;
  68 }
  69
  70 int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
  71 {
  72         struct fdt_reserve_entry *re;
  73         int err = check_header_sw(fdt);
  74         int offset;
  75
  76         if (err)
  77                 return err;
  78         if (fdt_size_dt_struct(fdt))
  79                 return -FDT_ERR_BADSTATE;
  80
  81         offset = fdt_off_dt_struct(fdt);
  82         if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
  83                 return -FDT_ERR_NOSPACE;
  84
  85         re = (struct fdt_reserve_entry *)((void *)fdt + offset);
  86         re->address = cpu_to_fdt64(addr);
  87         re->size = cpu_to_fdt64(size);
  88
  89         fdt_set_header(fdt, off_dt_struct, offset + sizeof(*re));
  90
  91         return 0;
  92 }
  93
  94 int fdt_finish_reservemap(void *fdt)
  95 {
  96         return fdt_add_reservemap_entry(fdt, 0, 0);
  97 }
  98
  99 int fdt_begin_node(void *fdt, const char *name)
 100 {
 101         struct fdt_node_header *nh;
 102         int err = check_header_sw(fdt);
 103         int namelen = strlen(name) + 1;
 104
 105         if (err)
 106                 return err;
 107
 108         nh = grab_space(fdt, sizeof(*nh) + ALIGN(namelen, FDT_TAGSIZE));
 109         if (! nh)
 110                 return -FDT_ERR_NOSPACE;
 111
 112         nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
 113         memcpy(nh->name, name, namelen);
 114         return 0;
 115 }
 116
 117 int fdt_end_node(void *fdt)
 118 {
 119         uint32_t *en;
 120         int err = check_header_sw(fdt);
 121
 122         if (err)
 123                 return err;
 124
 125         en = grab_space(fdt, FDT_TAGSIZE);
 126         if (! en)
 127                 return -FDT_ERR_NOSPACE;
 128
 129         *en = cpu_to_fdt32(FDT_END_NODE);
 130         return 0;
 131 }
 132
 133 static int find_add_string(void *fdt, const char *s)
 134 {
 135         char *strtab = (char *)fdt + fdt_totalsize(fdt);
 136         const char *p;
 137         int strtabsize = fdt_size_dt_strings(fdt);
 138         int len = strlen(s) + 1;
 139         int struct_top, offset;
 140
 141         p = _fdt_find_string(strtab - strtabsize, strtabsize, s);
 142         if (p)
 143                 return p - strtab;
 144
 145         /* Add it */
 146         offset = -strtabsize - len;
 147         struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 148         if (fdt_totalsize(fdt) + offset < struct_top)
 149                 return 0; /* no more room :( */
 150
 151         memcpy(strtab + offset, s, len);
 152         fdt_set_header(fdt, size_dt_strings, strtabsize + len);
 153         return offset;
 154 }
 155
 156 int fdt_property(void *fdt, const char *name, const void *val, int len)
 157 {
 158         struct fdt_property *prop;
 159         int err = check_header_sw(fdt);
 160         int nameoff;
 161
 162         if (err)
 163                 return err;
 164
 165         nameoff = find_add_string(fdt, name);
 166         if (nameoff == 0)
 167                 return -FDT_ERR_NOSPACE;
 168
 169         prop = grab_space(fdt, sizeof(*prop) + ALIGN(len, FDT_TAGSIZE));
 170         if (! prop)
 171                 return -FDT_ERR_NOSPACE;
 172
 173         prop->tag = cpu_to_fdt32(FDT_PROP);
 174         prop->nameoff = cpu_to_fdt32(nameoff);
 175         prop->len = cpu_to_fdt32(len);
 176         memcpy(prop->data, val, len);
 177         return 0;
 178 }
 179
 180 int fdt_finish(void *fdt)
 181 {
 182         int err = check_header_sw(fdt);
 183         char *p = (char *)fdt;
 184         uint32_t *end;
 185         int oldstroffset, newstroffset;
 186         uint32_t tag;
 187         int offset, nextoffset;
 188
 189         if (err)
 190                 return err;
 191
 192         /* Add terminator */
 193         end = grab_space(fdt, sizeof(*end));
 194         if (! end)
 195                 return -FDT_ERR_NOSPACE;
 196         *end = cpu_to_fdt32(FDT_END);
 197
 198         /* Relocate the string table */
 199         oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
 200         newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 201         memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
 202         fdt_set_header(fdt, off_dt_strings, newstroffset);
 203
 204         /* Walk the structure, correcting string offsets */
 205         offset = 0;
 206         while ((tag = fdt_next_tag(fdt, offset, &nextoffset, NULL)) != FDT_END) {
 207                 if (tag == FDT_PROP) {
 208                         struct fdt_property *prop = fdt_offset_ptr(fdt, offset,
 209                                                                    sizeof(*prop));
 210                         int nameoff;
 211
 212                         if (! prop)
 213                                 return -FDT_ERR_BADSTRUCTURE;
 214
 215                         nameoff = fdt32_to_cpu(prop->nameoff);
 216                         nameoff += fdt_size_dt_strings(fdt);
 217                         prop->nameoff = cpu_to_fdt32(nameoff);
 218                 }
 219                 offset = nextoffset;
 220         }
 221
 222         /* Finally, adjust the header */
 223         fdt_set_header(fdt, totalsize, newstroffset + fdt_size_dt_strings(fdt));
 224         fdt_set_header(fdt, magic, FDT_MAGIC);
 225         return 0;
 226 }