1 // SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) 2 /* 3 * libfdt - Flat Device Tree manipulation 4 * Copyright (C) 2006 David Gibson, IBM Corporation. 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_blocks_misordered_(const void *fdt, 14 int mem_rsv_size, int struct_size) 15 { 16 return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8)) 17 || (fdt_off_dt_struct(fdt) < 18 (fdt_off_mem_rsvmap(fdt) + mem_rsv_size)) 19 || (fdt_off_dt_strings(fdt) < 20 (fdt_off_dt_struct(fdt) + struct_size)) 21 || (fdt_totalsize(fdt) < 22 (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt))); 23 } 24 25 static int fdt_rw_probe_(void *fdt) 26 { 27 if (can_assume(VALID_DTB)) 28 return 0; 29 FDT_RO_PROBE(fdt); 30 31 if (!can_assume(LATEST) && fdt_version(fdt) < 17) 32 return -FDT_ERR_BADVERSION; 33 if (fdt_blocks_misordered_(fdt, sizeof(struct fdt_reserve_entry), 34 fdt_size_dt_struct(fdt))) 35 return -FDT_ERR_BADLAYOUT; 36 if (!can_assume(LATEST) && fdt_version(fdt) > 17) 37 fdt_set_version(fdt, 17); 38 39 return 0; 40 } 41 42 #define FDT_RW_PROBE(fdt) \ 43 { \ 44 int err_; \ 45 if ((err_ = fdt_rw_probe_(fdt)) != 0) \ 46 return err_; \ 47 } 48 49 static inline int fdt_data_size_(void *fdt) 50 { 51 return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt); 52 } 53 54 static int fdt_splice_(void *fdt, void *splicepoint, int oldlen, int newlen) 55 { 56 char *p = splicepoint; 57 char *end = (char *)fdt + fdt_data_size_(fdt); 58 59 if (((p + oldlen) < p) || ((p + oldlen) > end)) 60 return -FDT_ERR_BADOFFSET; 61 if ((p < (char *)fdt) || ((end - oldlen + newlen) < (char *)fdt)) 62 return -FDT_ERR_BADOFFSET; 63 if ((end - oldlen + newlen) > ((char *)fdt + fdt_totalsize(fdt))) 64 return -FDT_ERR_NOSPACE; 65 memmove(p + newlen, p + oldlen, end - p - oldlen); 66 return 0; 67 } 68 69 static int fdt_splice_mem_rsv_(void *fdt, struct fdt_reserve_entry *p, 70 int oldn, int newn) 71 { 72 int delta = (newn - oldn) * sizeof(*p); 73 int err; 74 err = fdt_splice_(fdt, p, oldn * sizeof(*p), newn * sizeof(*p)); 75 if (err) 76 return err; 77 fdt_set_off_dt_struct(fdt, fdt_off_dt_struct(fdt) + delta); 78 fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta); 79 return 0; 80 } 81 82 static int fdt_splice_struct_(void *fdt, void *p, 83 int oldlen, int newlen) 84 { 85 int delta = newlen - oldlen; 86 int err; 87 88 if ((err = fdt_splice_(fdt, p, oldlen, newlen))) 89 return err; 90 91 fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta); 92 fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta); 93 return 0; 94 } 95 96 /* Must only be used to roll back in case of error */ 97 static void fdt_del_last_string_(void *fdt, const char *s) 98 { 99 int newlen = strlen(s) + 1; 100 101 fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) - newlen); 102 } 103 104 static int fdt_splice_string_(void *fdt, int newlen) 105 { 106 void *p = (char *)fdt 107 + fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt); 108 int err; 109 110 if ((err = fdt_splice_(fdt, p, 0, newlen))) 111 return err; 112 113 fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen); 114 return 0; 115 } 116 117 /** 118 * fdt_find_add_string_() - Find or allocate a string 119 * 120 * @fdt: pointer to the device tree to check/adjust 121 * @s: string to find/add 122 * @allocated: Set to 0 if the string was found, 1 if not found and so 123 * allocated. Ignored if can_assume(NO_ROLLBACK) 124 * @return offset of string in the string table (whether found or added) 125 */ 126 static int fdt_find_add_string_(void *fdt, const char *s, int *allocated) 127 { 128 char *strtab = (char *)fdt + fdt_off_dt_strings(fdt); 129 const char *p; 130 char *new; 131 int len = strlen(s) + 1; 132 int err; 133 134 if (!can_assume(NO_ROLLBACK)) 135 *allocated = 0; 136 137 p = fdt_find_string_(strtab, fdt_size_dt_strings(fdt), s); 138 if (p) 139 /* found it */ 140 return (p - strtab); 141 142 new = strtab + fdt_size_dt_strings(fdt); 143 err = fdt_splice_string_(fdt, len); 144 if (err) 145 return err; 146 147 if (!can_assume(NO_ROLLBACK)) 148 *allocated = 1; 149 150 memcpy(new, s, len); 151 return (new - strtab); 152 } 153 154 int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size) 155 { 156 struct fdt_reserve_entry *re; 157 int err; 158 159 FDT_RW_PROBE(fdt); 160 161 re = fdt_mem_rsv_w_(fdt, fdt_num_mem_rsv(fdt)); 162 err = fdt_splice_mem_rsv_(fdt, re, 0, 1); 163 if (err) 164 return err; 165 166 re->address = cpu_to_fdt64(address); 167 re->size = cpu_to_fdt64(size); 168 return 0; 169 } 170 171 int fdt_del_mem_rsv(void *fdt, int n) 172 { 173 struct fdt_reserve_entry *re = fdt_mem_rsv_w_(fdt, n); 174 175 FDT_RW_PROBE(fdt); 176 177 if (n >= fdt_num_mem_rsv(fdt)) 178 return -FDT_ERR_NOTFOUND; 179 180 return fdt_splice_mem_rsv_(fdt, re, 1, 0); 181 } 182 183 static int fdt_resize_property_(void *fdt, int nodeoffset, const char *name, 184 int len, struct fdt_property **prop) 185 { 186 int oldlen; 187 int err; 188 189 *prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen); 190 if (!*prop) 191 return oldlen; 192 193 if ((err = fdt_splice_struct_(fdt, (*prop)->data, FDT_TAGALIGN(oldlen), 194 FDT_TAGALIGN(len)))) 195 return err; 196 197 (*prop)->len = cpu_to_fdt32(len); 198 return 0; 199 } 200 201 static int fdt_add_property_(void *fdt, int nodeoffset, const char *name, 202 int len, struct fdt_property **prop) 203 { 204 int proplen; 205 int nextoffset; 206 int namestroff; 207 int err; 208 int allocated; 209 210 if ((nextoffset = fdt_check_node_offset_(fdt, nodeoffset)) < 0) 211 return nextoffset; 212 213 namestroff = fdt_find_add_string_(fdt, name, &allocated); 214 if (namestroff < 0) 215 return namestroff; 216 217 *prop = fdt_offset_ptr_w_(fdt, nextoffset); 218 proplen = sizeof(**prop) + FDT_TAGALIGN(len); 219 220 err = fdt_splice_struct_(fdt, *prop, 0, proplen); 221 if (err) { 222 /* Delete the string if we failed to add it */ 223 if (!can_assume(NO_ROLLBACK) && allocated) 224 fdt_del_last_string_(fdt, name); 225 return err; 226 } 227 228 (*prop)->tag = cpu_to_fdt32(FDT_PROP); 229 (*prop)->nameoff = cpu_to_fdt32(namestroff); 230 (*prop)->len = cpu_to_fdt32(len); 231 return 0; 232 } 233 234 int fdt_set_name(void *fdt, int nodeoffset, const char *name) 235 { 236 char *namep; 237 int oldlen, newlen; 238 int err; 239 240 FDT_RW_PROBE(fdt); 241 242 namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen); 243 if (!namep) 244 return oldlen; 245 246 newlen = strlen(name); 247 248 err = fdt_splice_struct_(fdt, namep, FDT_TAGALIGN(oldlen+1), 249 FDT_TAGALIGN(newlen+1)); 250 if (err) 251 return err; 252 253 memcpy(namep, name, newlen+1); 254 return 0; 255 } 256 257 int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name, 258 int len, void **prop_data) 259 { 260 struct fdt_property *prop; 261 int err; 262 263 FDT_RW_PROBE(fdt); 264 265 err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop); 266 if (err == -FDT_ERR_NOTFOUND) 267 err = fdt_add_property_(fdt, nodeoffset, name, len, &prop); 268 if (err) 269 return err; 270 271 *prop_data = prop->data; 272 return 0; 273 } 274 275 int fdt_setprop(void *fdt, int nodeoffset, const char *name, 276 const void *val, int len) 277 { 278 void *prop_data; 279 int err; 280 281 err = fdt_setprop_placeholder(fdt, nodeoffset, name, len, &prop_data); 282 if (err) 283 return err; 284 285 if (len) 286 memcpy(prop_data, val, len); 287 return 0; 288 } 289 290 int fdt_appendprop(void *fdt, int nodeoffset, const char *name, 291 const void *val, int len) 292 { 293 struct fdt_property *prop; 294 int err, oldlen, newlen; 295 296 FDT_RW_PROBE(fdt); 297 298 prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen); 299 if (prop) { 300 newlen = len + oldlen; 301 err = fdt_splice_struct_(fdt, prop->data, 302 FDT_TAGALIGN(oldlen), 303 FDT_TAGALIGN(newlen)); 304 if (err) 305 return err; 306 prop->len = cpu_to_fdt32(newlen); 307 memcpy(prop->data + oldlen, val, len); 308 } else { 309 err = fdt_add_property_(fdt, nodeoffset, name, len, &prop); 310 if (err) 311 return err; 312 memcpy(prop->data, val, len); 313 } 314 return 0; 315 } 316 317 int fdt_delprop(void *fdt, int nodeoffset, const char *name) 318 { 319 struct fdt_property *prop; 320 int len, proplen; 321 322 FDT_RW_PROBE(fdt); 323 324 prop = fdt_get_property_w(fdt, nodeoffset, name, &len); 325 if (!prop) 326 return len; 327 328 proplen = sizeof(*prop) + FDT_TAGALIGN(len); 329 return fdt_splice_struct_(fdt, prop, proplen, 0); 330 } 331 332 int fdt_add_subnode_namelen(void *fdt, int parentoffset, 333 const char *name, int namelen) 334 { 335 struct fdt_node_header *nh; 336 int offset, nextoffset; 337 int nodelen; 338 int err; 339 uint32_t tag; 340 fdt32_t *endtag; 341 342 FDT_RW_PROBE(fdt); 343 344 offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen); 345 if (offset >= 0) 346 return -FDT_ERR_EXISTS; 347 else if (offset != -FDT_ERR_NOTFOUND) 348 return offset; 349 350 /* Try to place the new node after the parent's properties */ 351 fdt_next_tag(fdt, parentoffset, &nextoffset); /* skip the BEGIN_NODE */ 352 do { 353 offset = nextoffset; 354 tag = fdt_next_tag(fdt, offset, &nextoffset); 355 } while ((tag == FDT_PROP) || (tag == FDT_NOP)); 356 357 nh = fdt_offset_ptr_w_(fdt, offset); 358 nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE; 359 360 err = fdt_splice_struct_(fdt, nh, 0, nodelen); 361 if (err) 362 return err; 363 364 nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE); 365 memset(nh->name, 0, FDT_TAGALIGN(namelen+1)); 366 memcpy(nh->name, name, namelen); 367 endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE); 368 *endtag = cpu_to_fdt32(FDT_END_NODE); 369 370 return offset; 371 } 372 373 int fdt_add_subnode(void *fdt, int parentoffset, const char *name) 374 { 375 return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name)); 376 } 377 378 int fdt_del_node(void *fdt, int nodeoffset) 379 { 380 int endoffset; 381 382 FDT_RW_PROBE(fdt); 383 384 endoffset = fdt_node_end_offset_(fdt, nodeoffset); 385 if (endoffset < 0) 386 return endoffset; 387 388 return fdt_splice_struct_(fdt, fdt_offset_ptr_w_(fdt, nodeoffset), 389 endoffset - nodeoffset, 0); 390 } 391 392 static void fdt_packblocks_(const char *old, char *new, 393 int mem_rsv_size, int struct_size) 394 { 395 int mem_rsv_off, struct_off, strings_off; 396 397 mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8); 398 struct_off = mem_rsv_off + mem_rsv_size; 399 strings_off = struct_off + struct_size; 400 401 memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size); 402 fdt_set_off_mem_rsvmap(new, mem_rsv_off); 403 404 memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size); 405 fdt_set_off_dt_struct(new, struct_off); 406 fdt_set_size_dt_struct(new, struct_size); 407 408 memmove(new + strings_off, old + fdt_off_dt_strings(old), 409 fdt_size_dt_strings(old)); 410 fdt_set_off_dt_strings(new, strings_off); 411 fdt_set_size_dt_strings(new, fdt_size_dt_strings(old)); 412 } 413 414 int fdt_open_into(const void *fdt, void *buf, int bufsize) 415 { 416 int err; 417 int mem_rsv_size, struct_size; 418 int newsize; 419 const char *fdtstart = fdt; 420 const char *fdtend = fdtstart + fdt_totalsize(fdt); 421 char *tmp; 422 423 FDT_RO_PROBE(fdt); 424 425 mem_rsv_size = (fdt_num_mem_rsv(fdt)+1) 426 * sizeof(struct fdt_reserve_entry); 427 428 if (can_assume(LATEST) || fdt_version(fdt) >= 17) { 429 struct_size = fdt_size_dt_struct(fdt); 430 } else { 431 struct_size = 0; 432 while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END) 433 ; 434 if (struct_size < 0) 435 return struct_size; 436 } 437 438 if (can_assume(LIBFDT_ORDER) | 439 !fdt_blocks_misordered_(fdt, mem_rsv_size, struct_size)) { 440 /* no further work necessary */ 441 err = fdt_move(fdt, buf, bufsize); 442 if (err) 443 return err; 444 fdt_set_version(buf, 17); 445 fdt_set_size_dt_struct(buf, struct_size); 446 fdt_set_totalsize(buf, bufsize); 447 return 0; 448 } 449 450 /* Need to reorder */ 451 newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size 452 + struct_size + fdt_size_dt_strings(fdt); 453 454 if (bufsize < newsize) 455 return -FDT_ERR_NOSPACE; 456 457 /* First attempt to build converted tree at beginning of buffer */ 458 tmp = buf; 459 /* But if that overlaps with the old tree... */ 460 if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) { 461 /* Try right after the old tree instead */ 462 tmp = (char *)(uintptr_t)fdtend; 463 if ((tmp + newsize) > ((char *)buf + bufsize)) 464 return -FDT_ERR_NOSPACE; 465 } 466 467 fdt_packblocks_(fdt, tmp, mem_rsv_size, struct_size); 468 memmove(buf, tmp, newsize); 469 470 fdt_set_magic(buf, FDT_MAGIC); 471 fdt_set_totalsize(buf, bufsize); 472 fdt_set_version(buf, 17); 473 fdt_set_last_comp_version(buf, 16); 474 fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt)); 475 476 return 0; 477 } 478 479 int fdt_pack(void *fdt) 480 { 481 int mem_rsv_size; 482 483 FDT_RW_PROBE(fdt); 484 485 mem_rsv_size = (fdt_num_mem_rsv(fdt)+1) 486 * sizeof(struct fdt_reserve_entry); 487 fdt_packblocks_(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt)); 488 fdt_set_totalsize(fdt, fdt_data_size_(fdt)); 489 490 return 0; 491 } 492