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, mem_rsv_size: 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, val: 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, splicepoint: p, oldlen: oldn * sizeof(*p), newlen: 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, splicepoint: 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, splicepoint: p, oldlen: 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, newlen: 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, n: fdt_num_mem_rsv(fdt)); |
163 | err = fdt_splice_mem_rsv_(fdt, p: re, oldn: 0, newn: 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, p: re, oldn: 1, newn: 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, lenp: &oldlen); |
191 | if (!*prop) |
192 | return oldlen; |
193 | |
194 | if ((err = fdt_splice_struct_(fdt, p: (*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, offset: nodeoffset)) < 0) |
212 | return nextoffset; |
213 | |
214 | namestroff = fdt_find_add_string_(fdt, s: name, allocated: &allocated); |
215 | if (namestroff < 0) |
216 | return namestroff; |
217 | |
218 | *prop = fdt_offset_ptr_w_(fdt, offset: nextoffset); |
219 | proplen = sizeof(**prop) + FDT_TAGALIGN(len); |
220 | |
221 | err = fdt_splice_struct_(fdt, p: *prop, oldlen: 0, newlen: 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, s: 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, lenp: &oldlen); |
244 | if (!namep) |
245 | return oldlen; |
246 | |
247 | newlen = strlen(name); |
248 | |
249 | err = fdt_splice_struct_(fdt, p: 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: &prop); |
267 | if (err == -FDT_ERR_NOTFOUND) |
268 | err = fdt_add_property_(fdt, nodeoffset, name, len, prop: &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: &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, lenp: &oldlen); |
300 | if (prop) { |
301 | newlen = len + oldlen; |
302 | err = fdt_splice_struct_(fdt, p: 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: &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, lenp: &len); |
326 | if (!prop) |
327 | return len; |
328 | |
329 | proplen = sizeof(*prop) + FDT_TAGALIGN(len); |
330 | return fdt_splice_struct_(fdt, p: prop, oldlen: proplen, newlen: 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, offset: parentoffset, nextoffset: &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: &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, p: nh, oldlen: 0, newlen: 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, p: fdt_offset_ptr_w_(fdt, offset: nodeoffset), |
393 | oldlen: endoffset - nodeoffset, newlen: 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(fdt: new, val: mem_rsv_off); |
409 | |
410 | memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size); |
411 | fdt_set_off_dt_struct(fdt: new, val: struct_off); |
412 | fdt_set_size_dt_struct(fdt: new, val: struct_size); |
413 | |
414 | memmove(new + strings_off, old + fdt_off_dt_strings(old), strings_size); |
415 | fdt_set_off_dt_strings(fdt: new, val: strings_off); |
416 | fdt_set_size_dt_strings(fdt: 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, offset: struct_size, nextoffset: &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(fdt: buf, val: 17); |
452 | fdt_set_size_dt_struct(fdt: buf, val: struct_size); |
453 | fdt_set_totalsize(fdt: buf, val: 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_(old: fdt, new: tmp, mem_rsv_size, struct_size, |
475 | fdt_size_dt_strings(fdt)); |
476 | memmove(buf, tmp, newsize); |
477 | |
478 | fdt_set_magic(fdt: buf, FDT_MAGIC); |
479 | fdt_set_totalsize(fdt: buf, val: bufsize); |
480 | fdt_set_version(fdt: buf, val: 17); |
481 | fdt_set_last_comp_version(fdt: buf, val: 16); |
482 | fdt_set_boot_cpuid_phys(fdt: 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_(old: fdt, new: fdt, mem_rsv_size, fdt_size_dt_struct(fdt), |
496 | fdt_size_dt_strings(fdt)); |
497 | fdt_set_totalsize(fdt, val: fdt_data_size_(fdt)); |
498 | |
499 | return 0; |
500 | } |
501 | |