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_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
  66 {
  67         struct fdt_reserve_entry *re;
  68         int offset;
  69
  70         FDT_SW_CHECK_HEADER(fdt);
  71
  72         if (fdt_size_dt_struct(fdt))
  73                 return -FDT_ERR_BADSTATE;
  74
  75         offset = fdt_off_dt_struct(fdt);
  76         if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
  77                 return -FDT_ERR_NOSPACE;
  78
  79         re = (struct fdt_reserve_entry *)((char *)fdt + offset);
  80         re->address = cpu_to_fdt64(addr);
  81         re->size = cpu_to_fdt64(size);
  82
  83         fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
  84
  85         return 0;
  86 }
  87
  88 int fdt_finish_reservemap(void *fdt)
  89 {
  90         return fdt_add_reservemap_entry(fdt, 0, 0);
  91 }
  92
  93 int fdt_begin_node(void *fdt, const char *name)
  94 {
  95         struct fdt_node_header *nh;
  96         int namelen = strlen(name) + 1;
  97
  98         FDT_SW_CHECK_HEADER(fdt);
  99
 100         nh = _fdt_grab_space(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
 101         if (! nh)
 102                 return -FDT_ERR_NOSPACE;
 103
 104         nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
 105         memcpy(nh->name, name, namelen);
 106         return 0;
 107 }
 108
 109 int fdt_end_node(void *fdt)
 110 {
 111         fdt32_t *en;
 112
 113         FDT_SW_CHECK_HEADER(fdt);
 114
 115         en = _fdt_grab_space(fdt, FDT_TAGSIZE);
 116         if (! en)
 117                 return -FDT_ERR_NOSPACE;
 118
 119         *en = cpu_to_fdt32(FDT_END_NODE);
 120         return 0;
 121 }
 122
 123 static int _fdt_find_add_string(void *fdt, const char *s)
 124 {
 125         char *strtab = (char *)fdt + fdt_totalsize(fdt);
 126         const char *p;
 127         int strtabsize = fdt_size_dt_strings(fdt);
 128         int len = strlen(s) + 1;
 129         int struct_top, offset;
 130
 131         p = _fdt_find_string(strtab - strtabsize, strtabsize, s);
 132         if (p)
 133                 return p - strtab;
 134
 135         /* Add it */
 136         offset = -strtabsize - len;
 137         struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 138         if (fdt_totalsize(fdt) + offset < struct_top)
 139                 return 0; /* no more room :( */
 140
 141         memcpy(strtab + offset, s, len);
 142         fdt_set_size_dt_strings(fdt, strtabsize + len);
 143         return offset;
 144 }
 145
 146 int fdt_property(void *fdt, const char *name, const void *val, int len)
 147 {
 148         struct fdt_property *prop;
 149         int nameoff;
 150
 151         FDT_SW_CHECK_HEADER(fdt);
 152
 153         nameoff = _fdt_find_add_string(fdt, name);
 154         if (nameoff == 0)
 155                 return -FDT_ERR_NOSPACE;
 156
 157         prop = _fdt_grab_space(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
 158         if (! prop)
 159                 return -FDT_ERR_NOSPACE;
 160
 161         prop->tag = cpu_to_fdt32(FDT_PROP);
 162         prop->nameoff = cpu_to_fdt32(nameoff);
 163         prop->len = cpu_to_fdt32(len);
 164         memcpy(prop->data, val, len);
 165         return 0;
 166 }
 167
 168 int fdt_finish(void *fdt)
 169 {
 170         char *p = (char *)fdt;
 171         fdt32_t *end;
 172         int oldstroffset, newstroffset;
 173         uint32_t tag;
 174         int offset, nextoffset;
 175
 176         FDT_SW_CHECK_HEADER(fdt);
 177
 178         /* Add terminator */
 179         end = _fdt_grab_space(fdt, sizeof(*end));
 180         if (! end)
 181                 return -FDT_ERR_NOSPACE;
 182         *end = cpu_to_fdt32(FDT_END);
 183
 184         /* Relocate the string table */
 185         oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
 186         newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
 187         memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
 188         fdt_set_off_dt_strings(fdt, newstroffset);
 189
 190         /* Walk the structure, correcting string offsets */
 191         offset = 0;
 192         while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
 193                 if (tag == FDT_PROP) {
 194                         struct fdt_property *prop =
 195                                 _fdt_offset_ptr_w(fdt, offset);
 196                         int nameoff;
 197
 198                         nameoff = fdt32_to_cpu(prop->nameoff);
 199                         nameoff += fdt_size_dt_strings(fdt);
 200                         prop->nameoff = cpu_to_fdt32(nameoff);
 201                 }
 202                 offset = nextoffset;
 203         }
 204         if (nextoffset < 0)
 205                 return nextoffset;
 206
 207         /* Finally, adjust the header */
 208         fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
 209         fdt_set_magic(fdt, FDT_MAGIC);
 210         return 0;
 211 }