EmbeddedPkg/Library/FdtLib/fdt_sw.c

   1 /*
   2  * libfdt - Flat Device Tree manipulation
   3  * Copyright (C) 2006 David Gibson, IBM Corporation.
   4  *
   5  * libfdt is dual licensed: you can use it either under the terms of
   6  * the GPL, or the BSD license, at your option.
   7  *
   8  *  a) This library is free software; you can redistribute it and/or
   9  *     modify it under the terms of the GNU General Public License as
  10  *     published by the Free Software Foundation; either version 2 of the
  11  *     License, or (at your option) any later version.
  12  *
  13  *     This library is distributed in the hope that it will be useful,
  14  *     but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  *     GNU General Public License for more details.
  17  *
  18  *     You should have received a copy of the GNU General Public
  19  *     License along with this library; if not, write to the Free
  20  *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
  21  *     MA 02110-1301 USA
  22  *
  23  * Alternatively,
  24  *
  25  *  b) Redistribution and use in source and binary forms, with or
  26  *     without modification, are permitted provided that the following
  27  *     conditions are met:
  28  *
  29  *     1. Redistributions of source code must retain the above
  30  *        copyright notice, this list of conditions and the following
  31  *        disclaimer.
  32  *     2. Redistributions in binary form must reproduce the above
  33  *        copyright notice, this list of conditions and the following
  34  *        disclaimer in the documentation and/or other materials
  35  *        provided with the distribution.
  36  *
  37  *     THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
  38  *     CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
  39  *     INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  40  *     MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  41  *     DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
  42  *     CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  43  *     SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
  44  *     NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  45  *     LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  46  *     HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  47  *     CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
  48  *     OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  49  *     EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  50  */
  51 #include "libfdt_env.h"
  52
  53 #include <fdt.h>
  54 #include <libfdt.h>
  55
  56 #include "libfdt_internal.h"
  57
  58 static int _fdt_sw_check_header(void *fdt)
  59 {
  60         if (fdt_magic(fdt) != FDT_SW_MAGIC)
  61                 return -FDT_ERR_BADMAGIC;
  62         /* FIXME: should check more details about the header state */
  63         return 0;
  64 }
  65
  66 #define FDT_SW_CHECK_HEADER(fdt) \
  67         { \
  68                 int err; \
  69                 if ((err = _fdt_sw_check_header(fdt)) != 0) \
  70                         return err; \
  71         }
  72
  73 static void *_fdt_grab_space(void *fdt, size_t len)
  74 {
  75         int offset = fdt_size_dt_struct(fdt);
  76         int spaceleft;
  77
  78         spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
  79                 - fdt_size_dt_strings(fdt);
  80
  81         if ((offset + len < offset) || (offset + len > spaceleft))
  82                 return NULL;
  83
  84         fdt_set_size_dt_struct(fdt, offset + len);
  85         return _fdt_offset_ptr_w(fdt, offset);
  86 }
  87
  88 int fdt_create(void *buf, int bufsize)
  89 {
  90         void *fdt = buf;
  91
  92         if (bufsize < sizeof(struct fdt_header))
  93                 return -FDT_ERR_NOSPACE;
  94
  95         memset(buf, 0, bufsize);
  96
  97         fdt_set_magic(fdt, FDT_SW_MAGIC);
  98         fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
  99         fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
 100         fdt_set_totalsize(fdt,  bufsize);
 101
 102         fdt_set_off_mem_rsvmap(fdt, FDT_ALIGN(sizeof(struct fdt_header),
 103                                               sizeof(struct fdt_reserve_entry)));
 104         fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
 105         fdt_set_off_dt_strings(fdt, bufsize);
 106
 107         return 0;
 108 }
 109
 110 int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
 111 {
 112         struct fdt_reserve_entry *re;
 113         int offset;
 114
 115         FDT_SW_CHECK_HEADER(fdt);
 116
 117         if (fdt_size_dt_struct(fdt))
 118                 return -FDT_ERR_BADSTATE;
 119
 120         offset = fdt_off_dt_struct(fdt);
 121         if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
 122                 return -FDT_ERR_NOSPACE;
 123
 124         re = (struct fdt_reserve_entry *)((char *)fdt + offset);
 125         re->address = cpu_to_fdt64(addr);
 126         re->size = cpu_to_fdt64(size);
 127
 128         fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
 129
 130         return 0;
 131 }
 132
 133 int fdt_finish_reservemap(void *fdt)
 134 {
 135         return fdt_add_reservemap_entry(fdt, 0, 0);
 136 }
 137
 138 int fdt_begin_node(void *fdt, const char *name)
 139 {
 140         struct fdt_node_header *nh;
 141         int namelen = strlen(name) + 1;
 142
 143         FDT_SW_CHECK_HEADER(fdt);
 144
 145         nh = _fdt_grab_space(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
 146         if (! nh)
 147                 return -FDT_ERR_NOSPACE;
 148
 149         nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
 150         memcpy(nh->name, name, namelen);
 151         return 0;
 152 }
 153
 154 int fdt_end_node(void *fdt)
 155 {
 156         uint32_t *en;
 157
 158         FDT_SW_CHECK_HEADER(fdt);
 159
 160         en = _fdt_grab_space(fdt, FDT_TAGSIZE);
 161         if (! en)
 162                 return -FDT_ERR_NOSPACE;
 163
 164         *en = cpu_to_fdt32(FDT_END_NODE);
 165         return 0;
 166 }
 167
 168 static int _fdt_find_add_string(void *fdt, const char *s)
 169 {
 170         char *strtab = (char *)fdt + fdt_totalsize(fdt);
 171         const char *p;
 172         int strtabsize = fdt_size_dt_strings(fdt);
 173         int len = strlen(s) + 1;
 174         int struct_top, offset;
 175
 176         p = _fdt_find_string(strtab - strtabsize, strtabsize, s);
 177         if (p)
 178                 return p - strtab;
 179
 180         /* Add it */
 181         offset = -strtabsize - len;
 182         struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 183         if (fdt_totalsize(fdt) + offset < struct_top)
 184                 return 0; /* no more room :( */
 185
 186         memcpy(strtab + offset, s, len);
 187         fdt_set_size_dt_strings(fdt, strtabsize + len);
 188         return offset;
 189 }
 190
 191 int fdt_property(void *fdt, const char *name, const void *val, int len)
 192 {
 193         struct fdt_property *prop;
 194         int nameoff;
 195
 196         FDT_SW_CHECK_HEADER(fdt);
 197
 198         nameoff = _fdt_find_add_string(fdt, name);
 199         if (nameoff == 0)
 200                 return -FDT_ERR_NOSPACE;
 201
 202         prop = _fdt_grab_space(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
 203         if (! prop)
 204                 return -FDT_ERR_NOSPACE;
 205
 206         prop->tag = cpu_to_fdt32(FDT_PROP);
 207         prop->nameoff = cpu_to_fdt32(nameoff);
 208         prop->len = cpu_to_fdt32(len);
 209         memcpy(prop->data, val, len);
 210         return 0;
 211 }
 212
 213 int fdt_finish(void *fdt)
 214 {
 215         char *p = (char *)fdt;
 216         uint32_t *end;
 217         int oldstroffset, newstroffset;
 218         uint32_t tag;
 219         int offset, nextoffset;
 220
 221         FDT_SW_CHECK_HEADER(fdt);
 222
 223         /* Add terminator */
 224         end = _fdt_grab_space(fdt, sizeof(*end));
 225         if (! end)
 226                 return -FDT_ERR_NOSPACE;
 227         *end = cpu_to_fdt32(FDT_END);
 228
 229         /* Relocate the string table */
 230         oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
 231         newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 232         memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
 233         fdt_set_off_dt_strings(fdt, newstroffset);
 234
 235         /* Walk the structure, correcting string offsets */
 236         offset = 0;
 237         while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
 238                 if (tag == FDT_PROP) {
 239                         struct fdt_property *prop =
 240                                 _fdt_offset_ptr_w(fdt, offset);
 241                         int nameoff;
 242
 243                         nameoff = fdt32_to_cpu(prop->nameoff);
 244                         nameoff += fdt_size_dt_strings(fdt);
 245                         prop->nameoff = cpu_to_fdt32(nameoff);
 246                 }
 247                 offset = nextoffset;
 248         }
 249         if (nextoffset < 0)
 250                 return nextoffset;
 251
 252         /* Finally, adjust the header */
 253         fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
 254         fdt_set_magic(fdt, FDT_MAGIC);
 255         return 0;
 256 }