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
  59 _fdt_sw_check_header (
  60   void  *fdt
  61   )
  62 {
  63   if (fdt_magic (fdt) != FDT_SW_MAGIC) {
  64     return -FDT_ERR_BADMAGIC;
  65   }
  66
  67   /* FIXME: should check more details about the header state */
  68   return 0;
  69 }
  70
  71 #define FDT_SW_CHECK_HEADER(fdt) \
  72         { \
  73                 int err; \
  74                 if ((err = _fdt_sw_check_header(fdt)) != 0) \
  75                         return err; \
  76         }
  77
  78 static void *
  79 _fdt_grab_space (
  80   void    *fdt,
  81   size_t  len
  82   )
  83 {
  84   int  offset = fdt_size_dt_struct (fdt);
  85   int  spaceleft;
  86
  87   spaceleft = fdt_totalsize (fdt) - fdt_off_dt_struct (fdt)
  88               - fdt_size_dt_strings (fdt);
  89
  90   if ((offset + len < offset) || (offset + len > spaceleft)) {
  91     return NULL;
  92   }
  93
  94   fdt_set_size_dt_struct (fdt, offset + len);
  95   return _fdt_offset_ptr_w (fdt, offset);
  96 }
  97
  98 int
  99 fdt_create (
 100   void  *buf,
 101   int   bufsize
 102   )
 103 {
 104   void  *fdt = buf;
 105
 106   if (bufsize < sizeof (struct fdt_header)) {
 107     return -FDT_ERR_NOSPACE;
 108   }
 109
 110   memset (buf, 0, bufsize);
 111
 112   fdt_set_magic (fdt, FDT_SW_MAGIC);
 113   fdt_set_version (fdt, FDT_LAST_SUPPORTED_VERSION);
 114   fdt_set_last_comp_version (fdt, FDT_FIRST_SUPPORTED_VERSION);
 115   fdt_set_totalsize (fdt, bufsize);
 116
 117   fdt_set_off_mem_rsvmap (
 118     fdt,
 119     FDT_ALIGN (
 120       sizeof (struct fdt_header),
 121       sizeof (struct fdt_reserve_entry)
 122       )
 123     );
 124   fdt_set_off_dt_struct (fdt, fdt_off_mem_rsvmap (fdt));
 125   fdt_set_off_dt_strings (fdt, bufsize);
 126
 127   return 0;
 128 }
 129
 130 int
 131 fdt_resize (
 132   void  *fdt,
 133   void  *buf,
 134   int   bufsize
 135   )
 136 {
 137   size_t  headsize, tailsize;
 138   char    *oldtail, *newtail;
 139
 140   FDT_SW_CHECK_HEADER (fdt);
 141
 142   headsize = fdt_off_dt_struct (fdt);
 143   tailsize = fdt_size_dt_strings (fdt);
 144
 145   if ((headsize + tailsize) > bufsize) {
 146     return -FDT_ERR_NOSPACE;
 147   }
 148
 149   oldtail = (char *)fdt + fdt_totalsize (fdt) - tailsize;
 150   newtail = (char *)buf + bufsize - tailsize;
 151
 152   /* Two cases to avoid clobbering data if the old and new
 153    * buffers partially overlap */
 154   if (buf <= fdt) {
 155     memmove (buf, fdt, headsize);
 156     memmove (newtail, oldtail, tailsize);
 157   } else {
 158     memmove (newtail, oldtail, tailsize);
 159     memmove (buf, fdt, headsize);
 160   }
 161
 162   fdt_set_off_dt_strings (buf, bufsize);
 163   fdt_set_totalsize (buf, bufsize);
 164
 165   return 0;
 166 }
 167
 168 int
 169 fdt_add_reservemap_entry (
 170   void      *fdt,
 171   uint64_t  addr,
 172   uint64_t  size
 173   )
 174 {
 175   struct fdt_reserve_entry  *re;
 176   int                       offset;
 177
 178   FDT_SW_CHECK_HEADER (fdt);
 179
 180   if (fdt_size_dt_struct (fdt)) {
 181     return -FDT_ERR_BADSTATE;
 182   }
 183
 184   offset = fdt_off_dt_struct (fdt);
 185   if ((offset + sizeof (*re)) > fdt_totalsize (fdt)) {
 186     return -FDT_ERR_NOSPACE;
 187   }
 188
 189   re          = (struct fdt_reserve_entry *)((char *)fdt + offset);
 190   re->address = cpu_to_fdt64 (addr);
 191   re->size    = cpu_to_fdt64 (size);
 192
 193   fdt_set_off_dt_struct (fdt, offset + sizeof (*re));
 194
 195   return 0;
 196 }
 197
 198 int
 199 fdt_finish_reservemap (
 200   void  *fdt
 201   )
 202 {
 203   return fdt_add_reservemap_entry (fdt, 0, 0);
 204 }
 205
 206 int
 207 fdt_begin_node (
 208   void        *fdt,
 209   const char  *name
 210   )
 211 {
 212   struct fdt_node_header  *nh;
 213   int                     namelen = strlen (name) + 1;
 214
 215   FDT_SW_CHECK_HEADER (fdt);
 216
 217   nh = _fdt_grab_space (fdt, sizeof (*nh) + FDT_TAGALIGN (namelen));
 218   if (!nh) {
 219     return -FDT_ERR_NOSPACE;
 220   }
 221
 222   nh->tag = cpu_to_fdt32 (FDT_BEGIN_NODE);
 223   memcpy (nh->name, name, namelen);
 224   return 0;
 225 }
 226
 227 int
 228 fdt_end_node (
 229   void  *fdt
 230   )
 231 {
 232   fdt32_t  *en;
 233
 234   FDT_SW_CHECK_HEADER (fdt);
 235
 236   en = _fdt_grab_space (fdt, FDT_TAGSIZE);
 237   if (!en) {
 238     return -FDT_ERR_NOSPACE;
 239   }
 240
 241   *en = cpu_to_fdt32 (FDT_END_NODE);
 242   return 0;
 243 }
 244
 245 static int
 246 _fdt_find_add_string (
 247   void        *fdt,
 248   const char  *s
 249   )
 250 {
 251   char        *strtab = (char *)fdt + fdt_totalsize (fdt);
 252   const char  *p;
 253   int         strtabsize = fdt_size_dt_strings (fdt);
 254   int         len = strlen (s) + 1;
 255   int         struct_top, offset;
 256
 257   p = _fdt_find_string (strtab - strtabsize, strtabsize, s);
 258   if (p) {
 259     return p - strtab;
 260   }
 261
 262   /* Add it */
 263   offset     = -strtabsize - len;
 264   struct_top = fdt_off_dt_struct (fdt) + fdt_size_dt_struct (fdt);
 265   if (fdt_totalsize (fdt) + offset < struct_top) {
 266     return 0;             /* no more room :( */
 267   }
 268
 269   memcpy (strtab + offset, s, len);
 270   fdt_set_size_dt_strings (fdt, strtabsize + len);
 271   return offset;
 272 }
 273
 274 int
 275 fdt_property_placeholder (
 276   void        *fdt,
 277   const char  *name,
 278   int         len,
 279   void        **valp
 280   )
 281 {
 282   struct fdt_property  *prop;
 283   int                  nameoff;
 284
 285   FDT_SW_CHECK_HEADER (fdt);
 286
 287   nameoff = _fdt_find_add_string (fdt, name);
 288   if (nameoff == 0) {
 289     return -FDT_ERR_NOSPACE;
 290   }
 291
 292   prop = _fdt_grab_space (fdt, sizeof (*prop) + FDT_TAGALIGN (len));
 293   if (!prop) {
 294     return -FDT_ERR_NOSPACE;
 295   }
 296
 297   prop->tag     = cpu_to_fdt32 (FDT_PROP);
 298   prop->nameoff = cpu_to_fdt32 (nameoff);
 299   prop->len     = cpu_to_fdt32 (len);
 300   *valp         = prop->data;
 301   return 0;
 302 }
 303
 304 int
 305 fdt_property (
 306   void        *fdt,
 307   const char  *name,
 308   const void  *val,
 309   int         len
 310   )
 311 {
 312   void  *ptr;
 313   int   ret;
 314
 315   ret = fdt_property_placeholder (fdt, name, len, &ptr);
 316   if (ret) {
 317     return ret;
 318   }
 319
 320   memcpy (ptr, val, len);
 321   return 0;
 322 }
 323
 324 int
 325 fdt_finish (
 326   void  *fdt
 327   )
 328 {
 329   char      *p = (char *)fdt;
 330   fdt32_t   *end;
 331   int       oldstroffset, newstroffset;
 332   uint32_t  tag;
 333   int       offset, nextoffset;
 334
 335   FDT_SW_CHECK_HEADER (fdt);
 336
 337   /* Add terminator */
 338   end = _fdt_grab_space (fdt, sizeof (*end));
 339   if (!end) {
 340     return -FDT_ERR_NOSPACE;
 341   }
 342
 343   *end = cpu_to_fdt32 (FDT_END);
 344
 345   /* Relocate the string table */
 346   oldstroffset = fdt_totalsize (fdt) - fdt_size_dt_strings (fdt);
 347   newstroffset = fdt_off_dt_struct (fdt) + fdt_size_dt_struct (fdt);
 348   memmove (p + newstroffset, p + oldstroffset, fdt_size_dt_strings (fdt));
 349   fdt_set_off_dt_strings (fdt, newstroffset);
 350
 351   /* Walk the structure, correcting string offsets */
 352   offset = 0;
 353   while ((tag = fdt_next_tag (fdt, offset, &nextoffset)) != FDT_END) {
 354     if (tag == FDT_PROP) {
 355       struct fdt_property  *prop =
 356         _fdt_offset_ptr_w (fdt, offset);
 357       int  nameoff;
 358
 359       nameoff       = fdt32_to_cpu (prop->nameoff);
 360       nameoff      += fdt_size_dt_strings (fdt);
 361       prop->nameoff = cpu_to_fdt32 (nameoff);
 362     }
 363
 364     offset = nextoffset;
 365   }
 366
 367   if (nextoffset < 0) {
 368     return nextoffset;
 369   }
 370
 371   /* Finally, adjust the header */
 372   fdt_set_totalsize (fdt, newstroffset + fdt_size_dt_strings (fdt));
 373   fdt_set_magic (fdt, FDT_MAGIC);
 374   return 0;
 375 }