1 /* $NetBSD: fdt_sw.c,v 1.1.1.3 2019/12/22 12:30:38 skrll Exp $ */ 2 3 // SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) 4 /* 5 * libfdt - Flat Device Tree manipulation 6 * Copyright (C) 2006 David Gibson, IBM Corporation. 7 */ 8 #include "libfdt_env.h" 9 10 #include <fdt.h> 11 #include <libfdt.h> 12 13 #include "libfdt_internal.h" 14 15 static int fdt_sw_probe_(void *fdt) 16 { 17 if (fdt_magic(fdt) == FDT_MAGIC) 18 return -FDT_ERR_BADSTATE; 19 else if (fdt_magic(fdt) != FDT_SW_MAGIC) 20 return -FDT_ERR_BADMAGIC; 21 return 0; 22 } 23 24 #define FDT_SW_PROBE(fdt) \ 25 { \ 26 int err; \ 27 if ((err = fdt_sw_probe_(fdt)) != 0) \ 28 return err; \ 29 } 30 31 /* 'memrsv' state: Initial state after fdt_create() 32 * 33 * Allowed functions: 34 * fdt_add_reservmap_entry() 35 * fdt_finish_reservemap() [moves to 'struct' state] 36 */ 37 static int fdt_sw_probe_memrsv_(void *fdt) 38 { 39 int err = fdt_sw_probe_(fdt); 40 if (err) 41 return err; 42 43 if (fdt_off_dt_strings(fdt) != 0) 44 return -FDT_ERR_BADSTATE; 45 return 0; 46 } 47 48 #define FDT_SW_PROBE_MEMRSV(fdt) \ 49 { \ 50 int err; \ 51 if ((err = fdt_sw_probe_memrsv_(fdt)) != 0) \ 52 return err; \ 53 } 54 55 /* 'struct' state: Enter this state after fdt_finish_reservemap() 56 * 57 * Allowed functions: 58 * fdt_begin_node() 59 * fdt_end_node() 60 * fdt_property*() 61 * fdt_finish() [moves to 'complete' state] 62 */ 63 static int fdt_sw_probe_struct_(void *fdt) 64 { 65 int err = fdt_sw_probe_(fdt); 66 if (err) 67 return err; 68 69 if (fdt_off_dt_strings(fdt) != fdt_totalsize(fdt)) 70 return -FDT_ERR_BADSTATE; 71 return 0; 72 } 73 74 #define FDT_SW_PROBE_STRUCT(fdt) \ 75 { \ 76 int err; \ 77 if ((err = fdt_sw_probe_struct_(fdt)) != 0) \ 78 return err; \ 79 } 80 81 static inline uint32_t sw_flags(void *fdt) 82 { 83 /* assert: (fdt_magic(fdt) == FDT_SW_MAGIC) */ 84 return fdt_last_comp_version(fdt); 85 } 86 87 /* 'complete' state: Enter this state after fdt_finish() 88 * 89 * Allowed functions: none 90 */ 91 92 static void *fdt_grab_space_(void *fdt, size_t len) 93 { 94 int offset = fdt_size_dt_struct(fdt); 95 int spaceleft; 96 97 spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt) 98 - fdt_size_dt_strings(fdt); 99 100 if ((offset + len < offset) || (offset + len > spaceleft)) 101 return NULL; 102 103 fdt_set_size_dt_struct(fdt, offset + len); 104 return fdt_offset_ptr_w_(fdt, offset); 105 } 106 107 int fdt_create_with_flags(void *buf, int bufsize, uint32_t flags) 108 { 109 const size_t hdrsize = FDT_ALIGN(sizeof(struct fdt_header), 110 sizeof(struct fdt_reserve_entry)); 111 void *fdt = buf; 112 113 if (bufsize < hdrsize) 114 return -FDT_ERR_NOSPACE; 115 116 if (flags & ~FDT_CREATE_FLAGS_ALL) 117 return -FDT_ERR_BADFLAGS; 118 119 memset(buf, 0, bufsize); 120 121 /* 122 * magic and last_comp_version keep intermediate state during the fdt 123 * creation process, which is replaced with the proper FDT format by 124 * fdt_finish(). 125 * 126 * flags should be accessed with sw_flags(). 127 */ 128 fdt_set_magic(fdt, FDT_SW_MAGIC); 129 fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION); 130 fdt_set_last_comp_version(fdt, flags); 131 132 fdt_set_totalsize(fdt, bufsize); 133 134 fdt_set_off_mem_rsvmap(fdt, hdrsize); 135 fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt)); 136 fdt_set_off_dt_strings(fdt, 0); 137 138 return 0; 139 } 140 141 int fdt_create(void *buf, int bufsize) 142 { 143 return fdt_create_with_flags(buf, bufsize, 0); 144 } 145 146 int fdt_resize(void *fdt, void *buf, int bufsize) 147 { 148 size_t headsize, tailsize; 149 char *oldtail, *newtail; 150 151 FDT_SW_PROBE(fdt); 152 153 headsize = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt); 154 tailsize = fdt_size_dt_strings(fdt); 155 156 if ((headsize + tailsize) > fdt_totalsize(fdt)) 157 return -FDT_ERR_INTERNAL; 158 159 if ((headsize + tailsize) > bufsize) 160 return -FDT_ERR_NOSPACE; 161 162 oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize; 163 newtail = (char *)buf + bufsize - tailsize; 164 165 /* Two cases to avoid clobbering data if the old and new 166 * buffers partially overlap */ 167 if (buf <= fdt) { 168 memmove(buf, fdt, headsize); 169 memmove(newtail, oldtail, tailsize); 170 } else { 171 memmove(newtail, oldtail, tailsize); 172 memmove(buf, fdt, headsize); 173 } 174 175 fdt_set_totalsize(buf, bufsize); 176 if (fdt_off_dt_strings(buf)) 177 fdt_set_off_dt_strings(buf, bufsize); 178 179 return 0; 180 } 181 182 int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size) 183 { 184 struct fdt_reserve_entry *re; 185 int offset; 186 187 FDT_SW_PROBE_MEMRSV(fdt); 188 189 offset = fdt_off_dt_struct(fdt); 190 if ((offset + sizeof(*re)) > fdt_totalsize(fdt)) 191 return -FDT_ERR_NOSPACE; 192 193 re = (struct fdt_reserve_entry *)((char *)fdt + offset); 194 re->address = cpu_to_fdt64(addr); 195 re->size = cpu_to_fdt64(size); 196 197 fdt_set_off_dt_struct(fdt, offset + sizeof(*re)); 198 199 return 0; 200 } 201 202 int fdt_finish_reservemap(void *fdt) 203 { 204 int err = fdt_add_reservemap_entry(fdt, 0, 0); 205 206 if (err) 207 return err; 208 209 fdt_set_off_dt_strings(fdt, fdt_totalsize(fdt)); 210 return 0; 211 } 212 213 int fdt_begin_node(void *fdt, const char *name) 214 { 215 struct fdt_node_header *nh; 216 int namelen; 217 218 FDT_SW_PROBE_STRUCT(fdt); 219 220 namelen = strlen(name) + 1; 221 nh = fdt_grab_space_(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen)); 222 if (! nh) 223 return -FDT_ERR_NOSPACE; 224 225 nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE); 226 memcpy(nh->name, name, namelen); 227 return 0; 228 } 229 230 int fdt_end_node(void *fdt) 231 { 232 fdt32_t *en; 233 234 FDT_SW_PROBE_STRUCT(fdt); 235 236 en = fdt_grab_space_(fdt, FDT_TAGSIZE); 237 if (! en) 238 return -FDT_ERR_NOSPACE; 239 240 *en = cpu_to_fdt32(FDT_END_NODE); 241 return 0; 242 } 243 244 static int fdt_add_string_(void *fdt, const char *s) 245 { 246 char *strtab = (char *)fdt + fdt_totalsize(fdt); 247 int strtabsize = fdt_size_dt_strings(fdt); 248 int len = strlen(s) + 1; 249 int struct_top, offset; 250 251 offset = -strtabsize - len; 252 struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt); 253 if (fdt_totalsize(fdt) + offset < struct_top) 254 return 0; /* no more room :( */ 255 256 memcpy(strtab + offset, s, len); 257 fdt_set_size_dt_strings(fdt, strtabsize + len); 258 return offset; 259 } 260 261 /* Must only be used to roll back in case of error */ 262 static void fdt_del_last_string_(void *fdt, const char *s) 263 { 264 int strtabsize = fdt_size_dt_strings(fdt); 265 int len = strlen(s) + 1; 266 267 fdt_set_size_dt_strings(fdt, strtabsize - len); 268 } 269 270 static int fdt_find_add_string_(void *fdt, const char *s, int *allocated) 271 { 272 char *strtab = (char *)fdt + fdt_totalsize(fdt); 273 int strtabsize = fdt_size_dt_strings(fdt); 274 const char *p; 275 276 *allocated = 0; 277 278 p = fdt_find_string_(strtab - strtabsize, strtabsize, s); 279 if (p) 280 return p - strtab; 281 282 *allocated = 1; 283 284 return fdt_add_string_(fdt, s); 285 } 286 287 int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp) 288 { 289 struct fdt_property *prop; 290 int nameoff; 291 int allocated; 292 293 FDT_SW_PROBE_STRUCT(fdt); 294 295 /* String de-duplication can be slow, _NO_NAME_DEDUP skips it */ 296 if (sw_flags(fdt) & FDT_CREATE_FLAG_NO_NAME_DEDUP) { 297 allocated = 1; 298 nameoff = fdt_add_string_(fdt, name); 299 } else { 300 nameoff = fdt_find_add_string_(fdt, name, &allocated); 301 } 302 if (nameoff == 0) 303 return -FDT_ERR_NOSPACE; 304 305 prop = fdt_grab_space_(fdt, sizeof(*prop) + FDT_TAGALIGN(len)); 306 if (! prop) { 307 if (allocated) 308 fdt_del_last_string_(fdt, name); 309 return -FDT_ERR_NOSPACE; 310 } 311 312 prop->tag = cpu_to_fdt32(FDT_PROP); 313 prop->nameoff = cpu_to_fdt32(nameoff); 314 prop->len = cpu_to_fdt32(len); 315 *valp = prop->data; 316 return 0; 317 } 318 319 int fdt_property(void *fdt, const char *name, const void *val, int len) 320 { 321 void *ptr; 322 int ret; 323 324 ret = fdt_property_placeholder(fdt, name, len, &ptr); 325 if (ret) 326 return ret; 327 memcpy(ptr, val, len); 328 return 0; 329 } 330 331 int fdt_finish(void *fdt) 332 { 333 char *p = (char *)fdt; 334 fdt32_t *end; 335 int oldstroffset, newstroffset; 336 uint32_t tag; 337 int offset, nextoffset; 338 339 FDT_SW_PROBE_STRUCT(fdt); 340 341 /* Add terminator */ 342 end = fdt_grab_space_(fdt, sizeof(*end)); 343 if (! end) 344 return -FDT_ERR_NOSPACE; 345 *end = cpu_to_fdt32(FDT_END); 346 347 /* Relocate the string table */ 348 oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt); 349 newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt); 350 memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt)); 351 fdt_set_off_dt_strings(fdt, newstroffset); 352 353 /* Walk the structure, correcting string offsets */ 354 offset = 0; 355 while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) { 356 if (tag == FDT_PROP) { 357 struct fdt_property *prop = 358 fdt_offset_ptr_w_(fdt, offset); 359 int nameoff; 360 361 nameoff = fdt32_to_cpu(prop->nameoff); 362 nameoff += fdt_size_dt_strings(fdt); 363 prop->nameoff = cpu_to_fdt32(nameoff); 364 } 365 offset = nextoffset; 366 } 367 if (nextoffset < 0) 368 return nextoffset; 369 370 /* Finally, adjust the header */ 371 fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt)); 372 373 /* And fix up fields that were keeping intermediate state. */ 374 fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION); 375 fdt_set_magic(fdt, FDT_MAGIC); 376 377 return 0; 378 } 379