1 /* $NetBSD: fdt_rw.c,v 1.1.1.3 2019/12/22 12:30:37 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
fdt_blocks_misordered_(const void * fdt,int mem_rsv_size,int struct_size)15 static int fdt_blocks_misordered_(const void *fdt,
16 int mem_rsv_size, int struct_size)
17 {
18 return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8))
19 || (fdt_off_dt_struct(fdt) <
20 (fdt_off_mem_rsvmap(fdt) + mem_rsv_size))
21 || (fdt_off_dt_strings(fdt) <
22 (fdt_off_dt_struct(fdt) + struct_size))
23 || (fdt_totalsize(fdt) <
24 (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
25 }
26
fdt_rw_probe_(void * fdt)27 static int fdt_rw_probe_(void *fdt)
28 {
29 FDT_RO_PROBE(fdt);
30
31 if (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 (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
fdt_data_size_(void * fdt)49 static inline int fdt_data_size_(void *fdt)
50 {
51 return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
52 }
53
fdt_splice_(void * fdt,void * splicepoint,int oldlen,int newlen)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
fdt_splice_mem_rsv_(void * fdt,struct fdt_reserve_entry * p,int oldn,int newn)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
fdt_splice_struct_(void * fdt,void * p,int oldlen,int newlen)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 */
fdt_del_last_string_(void * fdt,const char * s)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
fdt_splice_string_(void * fdt,int newlen)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
fdt_find_add_string_(void * fdt,const char * s,int * allocated)117 static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
118 {
119 char *strtab = (char *)fdt + fdt_off_dt_strings(fdt);
120 const char *p;
121 char *new;
122 int len = strlen(s) + 1;
123 int err;
124
125 *allocated = 0;
126
127 p = fdt_find_string_(strtab, fdt_size_dt_strings(fdt), s);
128 if (p)
129 /* found it */
130 return (p - strtab);
131
132 new = strtab + fdt_size_dt_strings(fdt);
133 err = fdt_splice_string_(fdt, len);
134 if (err)
135 return err;
136
137 *allocated = 1;
138
139 memcpy(new, s, len);
140 return (new - strtab);
141 }
142
fdt_add_mem_rsv(void * fdt,uint64_t address,uint64_t size)143 int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size)
144 {
145 struct fdt_reserve_entry *re;
146 int err;
147
148 FDT_RW_PROBE(fdt);
149
150 re = fdt_mem_rsv_w_(fdt, fdt_num_mem_rsv(fdt));
151 err = fdt_splice_mem_rsv_(fdt, re, 0, 1);
152 if (err)
153 return err;
154
155 re->address = cpu_to_fdt64(address);
156 re->size = cpu_to_fdt64(size);
157 return 0;
158 }
159
fdt_del_mem_rsv(void * fdt,int n)160 int fdt_del_mem_rsv(void *fdt, int n)
161 {
162 struct fdt_reserve_entry *re = fdt_mem_rsv_w_(fdt, n);
163
164 FDT_RW_PROBE(fdt);
165
166 if (n >= fdt_num_mem_rsv(fdt))
167 return -FDT_ERR_NOTFOUND;
168
169 return fdt_splice_mem_rsv_(fdt, re, 1, 0);
170 }
171
fdt_resize_property_(void * fdt,int nodeoffset,const char * name,int len,struct fdt_property ** prop)172 static int fdt_resize_property_(void *fdt, int nodeoffset, const char *name,
173 int len, struct fdt_property **prop)
174 {
175 int oldlen;
176 int err;
177
178 *prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
179 if (!*prop)
180 return oldlen;
181
182 if ((err = fdt_splice_struct_(fdt, (*prop)->data, FDT_TAGALIGN(oldlen),
183 FDT_TAGALIGN(len))))
184 return err;
185
186 (*prop)->len = cpu_to_fdt32(len);
187 return 0;
188 }
189
fdt_add_property_(void * fdt,int nodeoffset,const char * name,int len,struct fdt_property ** prop)190 static int fdt_add_property_(void *fdt, int nodeoffset, const char *name,
191 int len, struct fdt_property **prop)
192 {
193 int proplen;
194 int nextoffset;
195 int namestroff;
196 int err;
197 int allocated;
198
199 if ((nextoffset = fdt_check_node_offset_(fdt, nodeoffset)) < 0)
200 return nextoffset;
201
202 namestroff = fdt_find_add_string_(fdt, name, &allocated);
203 if (namestroff < 0)
204 return namestroff;
205
206 *prop = fdt_offset_ptr_w_(fdt, nextoffset);
207 proplen = sizeof(**prop) + FDT_TAGALIGN(len);
208
209 err = fdt_splice_struct_(fdt, *prop, 0, proplen);
210 if (err) {
211 if (allocated)
212 fdt_del_last_string_(fdt, name);
213 return err;
214 }
215
216 (*prop)->tag = cpu_to_fdt32(FDT_PROP);
217 (*prop)->nameoff = cpu_to_fdt32(namestroff);
218 (*prop)->len = cpu_to_fdt32(len);
219 return 0;
220 }
221
fdt_set_name(void * fdt,int nodeoffset,const char * name)222 int fdt_set_name(void *fdt, int nodeoffset, const char *name)
223 {
224 char *namep;
225 int oldlen, newlen;
226 int err;
227
228 FDT_RW_PROBE(fdt);
229
230 namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen);
231 if (!namep)
232 return oldlen;
233
234 newlen = strlen(name);
235
236 err = fdt_splice_struct_(fdt, namep, FDT_TAGALIGN(oldlen+1),
237 FDT_TAGALIGN(newlen+1));
238 if (err)
239 return err;
240
241 memcpy(namep, name, newlen+1);
242 return 0;
243 }
244
fdt_setprop_placeholder(void * fdt,int nodeoffset,const char * name,int len,void ** prop_data)245 int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name,
246 int len, void **prop_data)
247 {
248 struct fdt_property *prop;
249 int err;
250
251 FDT_RW_PROBE(fdt);
252
253 err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop);
254 if (err == -FDT_ERR_NOTFOUND)
255 err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
256 if (err)
257 return err;
258
259 *prop_data = prop->data;
260 return 0;
261 }
262
fdt_setprop(void * fdt,int nodeoffset,const char * name,const void * val,int len)263 int fdt_setprop(void *fdt, int nodeoffset, const char *name,
264 const void *val, int len)
265 {
266 void *prop_data;
267 int err;
268
269 err = fdt_setprop_placeholder(fdt, nodeoffset, name, len, &prop_data);
270 if (err)
271 return err;
272
273 if (len)
274 memcpy(prop_data, val, len);
275 return 0;
276 }
277
fdt_appendprop(void * fdt,int nodeoffset,const char * name,const void * val,int len)278 int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
279 const void *val, int len)
280 {
281 struct fdt_property *prop;
282 int err, oldlen, newlen;
283
284 FDT_RW_PROBE(fdt);
285
286 prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
287 if (prop) {
288 newlen = len + oldlen;
289 err = fdt_splice_struct_(fdt, prop->data,
290 FDT_TAGALIGN(oldlen),
291 FDT_TAGALIGN(newlen));
292 if (err)
293 return err;
294 prop->len = cpu_to_fdt32(newlen);
295 memcpy(prop->data + oldlen, val, len);
296 } else {
297 err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
298 if (err)
299 return err;
300 memcpy(prop->data, val, len);
301 }
302 return 0;
303 }
304
fdt_delprop(void * fdt,int nodeoffset,const char * name)305 int fdt_delprop(void *fdt, int nodeoffset, const char *name)
306 {
307 struct fdt_property *prop;
308 int len, proplen;
309
310 FDT_RW_PROBE(fdt);
311
312 prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
313 if (!prop)
314 return len;
315
316 proplen = sizeof(*prop) + FDT_TAGALIGN(len);
317 return fdt_splice_struct_(fdt, prop, proplen, 0);
318 }
319
fdt_add_subnode_namelen(void * fdt,int parentoffset,const char * name,int namelen)320 int fdt_add_subnode_namelen(void *fdt, int parentoffset,
321 const char *name, int namelen)
322 {
323 struct fdt_node_header *nh;
324 int offset, nextoffset;
325 int nodelen;
326 int err;
327 uint32_t tag;
328 fdt32_t *endtag;
329
330 FDT_RW_PROBE(fdt);
331
332 offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen);
333 if (offset >= 0)
334 return -FDT_ERR_EXISTS;
335 else if (offset != -FDT_ERR_NOTFOUND)
336 return offset;
337
338 /* Try to place the new node after the parent's properties */
339 fdt_next_tag(fdt, parentoffset, &nextoffset); /* skip the BEGIN_NODE */
340 do {
341 offset = nextoffset;
342 tag = fdt_next_tag(fdt, offset, &nextoffset);
343 } while ((tag == FDT_PROP) || (tag == FDT_NOP));
344
345 nh = fdt_offset_ptr_w_(fdt, offset);
346 nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE;
347
348 err = fdt_splice_struct_(fdt, nh, 0, nodelen);
349 if (err)
350 return err;
351
352 nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
353 memset(nh->name, 0, FDT_TAGALIGN(namelen+1));
354 memcpy(nh->name, name, namelen);
355 endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE);
356 *endtag = cpu_to_fdt32(FDT_END_NODE);
357
358 return offset;
359 }
360
fdt_add_subnode(void * fdt,int parentoffset,const char * name)361 int fdt_add_subnode(void *fdt, int parentoffset, const char *name)
362 {
363 return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name));
364 }
365
fdt_del_node(void * fdt,int nodeoffset)366 int fdt_del_node(void *fdt, int nodeoffset)
367 {
368 int endoffset;
369
370 FDT_RW_PROBE(fdt);
371
372 endoffset = fdt_node_end_offset_(fdt, nodeoffset);
373 if (endoffset < 0)
374 return endoffset;
375
376 return fdt_splice_struct_(fdt, fdt_offset_ptr_w_(fdt, nodeoffset),
377 endoffset - nodeoffset, 0);
378 }
379
fdt_packblocks_(const char * old,char * new,int mem_rsv_size,int struct_size)380 static void fdt_packblocks_(const char *old, char *new,
381 int mem_rsv_size, int struct_size)
382 {
383 int mem_rsv_off, struct_off, strings_off;
384
385 mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8);
386 struct_off = mem_rsv_off + mem_rsv_size;
387 strings_off = struct_off + struct_size;
388
389 memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size);
390 fdt_set_off_mem_rsvmap(new, mem_rsv_off);
391
392 memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size);
393 fdt_set_off_dt_struct(new, struct_off);
394 fdt_set_size_dt_struct(new, struct_size);
395
396 memmove(new + strings_off, old + fdt_off_dt_strings(old),
397 fdt_size_dt_strings(old));
398 fdt_set_off_dt_strings(new, strings_off);
399 fdt_set_size_dt_strings(new, fdt_size_dt_strings(old));
400 }
401
fdt_open_into(const void * fdt,void * buf,int bufsize)402 int fdt_open_into(const void *fdt, void *buf, int bufsize)
403 {
404 int err;
405 int mem_rsv_size, struct_size;
406 int newsize;
407 const char *fdtstart = fdt;
408 const char *fdtend = fdtstart + fdt_totalsize(fdt);
409 char *tmp;
410
411 FDT_RO_PROBE(fdt);
412
413 mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
414 * sizeof(struct fdt_reserve_entry);
415
416 if (fdt_version(fdt) >= 17) {
417 struct_size = fdt_size_dt_struct(fdt);
418 } else {
419 struct_size = 0;
420 while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END)
421 ;
422 if (struct_size < 0)
423 return struct_size;
424 }
425
426 if (!fdt_blocks_misordered_(fdt, mem_rsv_size, struct_size)) {
427 /* no further work necessary */
428 err = fdt_move(fdt, buf, bufsize);
429 if (err)
430 return err;
431 fdt_set_version(buf, 17);
432 fdt_set_size_dt_struct(buf, struct_size);
433 fdt_set_totalsize(buf, bufsize);
434 return 0;
435 }
436
437 /* Need to reorder */
438 newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size
439 + struct_size + fdt_size_dt_strings(fdt);
440
441 if (bufsize < newsize)
442 return -FDT_ERR_NOSPACE;
443
444 /* First attempt to build converted tree at beginning of buffer */
445 tmp = buf;
446 /* But if that overlaps with the old tree... */
447 if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) {
448 /* Try right after the old tree instead */
449 tmp = (char *)(uintptr_t)fdtend;
450 if ((tmp + newsize) > ((char *)buf + bufsize))
451 return -FDT_ERR_NOSPACE;
452 }
453
454 fdt_packblocks_(fdt, tmp, mem_rsv_size, struct_size);
455 memmove(buf, tmp, newsize);
456
457 fdt_set_magic(buf, FDT_MAGIC);
458 fdt_set_totalsize(buf, bufsize);
459 fdt_set_version(buf, 17);
460 fdt_set_last_comp_version(buf, 16);
461 fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt));
462
463 return 0;
464 }
465
fdt_pack(void * fdt)466 int fdt_pack(void *fdt)
467 {
468 int mem_rsv_size;
469
470 FDT_RW_PROBE(fdt);
471
472 mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
473 * sizeof(struct fdt_reserve_entry);
474 fdt_packblocks_(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt));
475 fdt_set_totalsize(fdt, fdt_data_size_(fdt));
476
477 return 0;
478 }
479