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