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 "config.h"
  20 #if CONFIG_OF_LIBFDT
  21
  22 #include "libfdt_env.h"
  23
  24 #include <fdt.h>
  25 #include <libfdt.h>
  26
  27 #include "libfdt_internal.h"
  28
  29 static int check_header_sw(void *fdt)
  30 {
  31         if (fdt_magic(fdt) != SW_MAGIC)
  32                 return -FDT_ERR_BADMAGIC;
  33         return 0;
  34 }
  35
  36 static void *grab_space(void *fdt, int len)
  37 {
  38         int offset = fdt_size_dt_struct(fdt);
  39         int spaceleft;
  40
  41         spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
  42                 - fdt_size_dt_strings(fdt);
  43
  44         if ((offset + len < offset) || (offset + len > spaceleft))
  45                 return NULL;
  46
  47         fdt_set_header(fdt, size_dt_struct, offset + len);
  48         return fdt_offset_ptr(fdt, offset, len);
  49 }
  50
  51 int fdt_create(void *buf, int bufsize)
  52 {
  53         void *fdt = buf;
  54
  55         if (bufsize < sizeof(struct fdt_header))
  56                 return -FDT_ERR_NOSPACE;
  57
  58         memset(buf, 0, bufsize);
  59
  60         fdt_set_header(fdt, magic, SW_MAGIC);
  61         fdt_set_header(fdt, version, FDT_LAST_SUPPORTED_VERSION);
  62         fdt_set_header(fdt, last_comp_version, FDT_FIRST_SUPPORTED_VERSION);
  63         fdt_set_header(fdt, totalsize, bufsize);
  64
  65         fdt_set_header(fdt, off_mem_rsvmap, ALIGN(sizeof(struct fdt_header),
  66                                               sizeof(struct fdt_reserve_entry)));
  67         fdt_set_header(fdt, off_dt_struct, fdt_off_mem_rsvmap(fdt));
  68         fdt_set_header(fdt, off_dt_strings, bufsize);
  69
  70         return 0;
  71 }
  72
  73 int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
  74 {
  75         struct fdt_reserve_entry *re;
  76         int err = check_header_sw(fdt);
  77         int offset;
  78
  79         if (err)
  80                 return err;
  81         if (fdt_size_dt_struct(fdt))
  82                 return -FDT_ERR_BADSTATE;
  83
  84         offset = fdt_off_dt_struct(fdt);
  85         if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
  86                 return -FDT_ERR_NOSPACE;
  87
  88         re = (struct fdt_reserve_entry *)((void *)fdt + offset);
  89         re->address = cpu_to_fdt64(addr);
  90         re->size = cpu_to_fdt64(size);
  91
  92         fdt_set_header(fdt, off_dt_struct, offset + sizeof(*re));
  93
  94         return 0;
  95 }
  96
  97 int fdt_finish_reservemap(void *fdt)
  98 {
  99         return fdt_add_reservemap_entry(fdt, 0, 0);
 100 }
 101
 102 int fdt_begin_node(void *fdt, const char *name)
 103 {
 104         struct fdt_node_header *nh;
 105         int err = check_header_sw(fdt);
 106         int namelen = strlen(name) + 1;
 107
 108         if (err)
 109                 return err;
 110
 111         nh = grab_space(fdt, sizeof(*nh) + ALIGN(namelen, FDT_TAGSIZE));
 112         if (! nh)
 113                 return -FDT_ERR_NOSPACE;
 114
 115         nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
 116         memcpy(nh->name, name, namelen);
 117         return 0;
 118 }
 119
 120 int fdt_end_node(void *fdt)
 121 {
 122         uint32_t *en;
 123         int err = check_header_sw(fdt);
 124
 125         if (err)
 126                 return err;
 127
 128         en = grab_space(fdt, FDT_TAGSIZE);
 129         if (! en)
 130                 return -FDT_ERR_NOSPACE;
 131
 132         *en = cpu_to_fdt32(FDT_END_NODE);
 133         return 0;
 134 }
 135
 136 static int find_add_string(void *fdt, const char *s)
 137 {
 138         char *strtab = (char *)fdt + fdt_totalsize(fdt);
 139         const char *p;
 140         int strtabsize = fdt_size_dt_strings(fdt);
 141         int len = strlen(s) + 1;
 142         int struct_top, offset;
 143
 144         p = _fdt_find_string(strtab - strtabsize, strtabsize, s);
 145         if (p)
 146                 return p - strtab;
 147
 148         /* Add it */
 149         offset = -strtabsize - len;
 150         struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 151         if (fdt_totalsize(fdt) + offset < struct_top)
 152                 return 0; /* no more room :( */
 153
 154         memcpy(strtab + offset, s, len);
 155         fdt_set_header(fdt, size_dt_strings, strtabsize + len);
 156         return offset;
 157 }
 158
 159 int fdt_property(void *fdt, const char *name, const void *val, int len)
 160 {
 161         struct fdt_property *prop;
 162         int err = check_header_sw(fdt);
 163         int nameoff;
 164
 165         if (err)
 166                 return err;
 167
 168         nameoff = find_add_string(fdt, name);
 169         if (nameoff == 0)
 170                 return -FDT_ERR_NOSPACE;
 171
 172         prop = grab_space(fdt, sizeof(*prop) + ALIGN(len, FDT_TAGSIZE));
 173         if (! prop)
 174                 return -FDT_ERR_NOSPACE;
 175
 176         prop->tag = cpu_to_fdt32(FDT_PROP);
 177         prop->nameoff = cpu_to_fdt32(nameoff);
 178         prop->len = cpu_to_fdt32(len);
 179         memcpy(prop->data, val, len);
 180         return 0;
 181 }
 182
 183 int fdt_finish(void *fdt)
 184 {
 185         int err = check_header_sw(fdt);
 186         char *p = (char *)fdt;
 187         uint32_t *end;
 188         int oldstroffset, newstroffset;
 189         uint32_t tag;
 190         int offset, nextoffset;
 191
 192         if (err)
 193                 return err;
 194
 195         /* Add terminator */
 196         end = grab_space(fdt, sizeof(*end));
 197         if (! end)
 198                 return -FDT_ERR_NOSPACE;
 199         *end = cpu_to_fdt32(FDT_END);
 200
 201         /* Relocate the string table */
 202         oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
 203         newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 204         memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
 205         fdt_set_header(fdt, off_dt_strings, newstroffset);
 206
 207         /* Walk the structure, correcting string offsets */
 208         offset = 0;
 209         while ((tag = fdt_next_tag(fdt, offset, &nextoffset, NULL)) != FDT_END) {
 210                 if (tag == FDT_PROP) {
 211                         struct fdt_property *prop = fdt_offset_ptr(fdt, offset,
 212                                                                    sizeof(*prop));
 213                         int nameoff;
 214
 215                         if (! prop)
 216                                 return -FDT_ERR_BADSTRUCTURE;
 217
 218                         nameoff = fdt32_to_cpu(prop->nameoff);
 219                         nameoff += fdt_size_dt_strings(fdt);
 220                         prop->nameoff = cpu_to_fdt32(nameoff);
 221                 }
 222                 offset = nextoffset;
 223         }
 224
 225         /* Finally, adjust the header */
 226         fdt_set_header(fdt, totalsize, newstroffset + fdt_size_dt_strings(fdt));
 227         fdt_set_header(fdt, magic, FDT_MAGIC);
 228         return 0;
 229 }
 230
 231 #endif /* CONFIG_OF_LIBFDT */