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