1// SPDX-License-Identifier: GPL-2.0+
2/*
3 * Copyright 2009 Benjamin Herrenschmidt, IBM Corp
4 * benh@kernel.crashing.org
5 *
6 * Based on parts of drivers/of/fdt.c from Linux v4.9
7 * Modifications for U-Boot
8 * Copyright (c) 2017 Google, Inc
9 */
10
11#define LOG_CATEGORY	LOGC_DT
12
13#include <abuf.h>
14#include <log.h>
15#include <linux/libfdt.h>
16#include <of_live.h>
17#include <malloc.h>
18#include <dm/of_access.h>
19#include <linux/err.h>
20#include <linux/sizes.h>
21
22enum {
23	BUF_STEP	= SZ_64K,
24};
25
26static void *unflatten_dt_alloc(void **mem, unsigned long size,
27				unsigned long align)
28{
29	void *res;
30
31	*mem = PTR_ALIGN(*mem, align);
32	res = *mem;
33	*mem += size;
34
35	return res;
36}
37
38/**
39 * unflatten_dt_node() - Alloc and populate a device_node from the flat tree
40 * @blob: The parent device tree blob
41 * @mem: Memory chunk to use for allocating device nodes and properties
42 * @poffset: pointer to node in flat tree
43 * @dad: Parent struct device_node
44 * @nodepp: The device_node tree created by the call
45 * @fpsize: Size of the node path up at t05he current depth.
46 * @dryrun: If true, do not allocate device nodes but still calculate needed
47 * memory size
48 */
49static void *unflatten_dt_node(const void *blob, void *mem, int *poffset,
50			       struct device_node *dad,
51			       struct device_node **nodepp,
52			       unsigned long fpsize, bool dryrun)
53{
54	const __be32 *p;
55	struct device_node *np;
56	struct property *pp, **prev_pp = NULL;
57	const char *pathp;
58	int l;
59	unsigned int allocl;
60	static int depth;
61	int old_depth;
62	int offset;
63	int has_name = 0;
64	int new_format = 0;
65
66	pathp = fdt_get_name(blob, *poffset, &l);
67	if (!pathp)
68		return mem;
69
70	allocl = ++l;
71
72	/*
73	 * version 0x10 has a more compact unit name here instead of the full
74	 * path. we accumulate the full path size using "fpsize", we'll rebuild
75	 * it later. We detect this because the first character of the name is
76	 * not '/'.
77	 */
78	if ((*pathp) != '/') {
79		new_format = 1;
80		if (fpsize == 0) {
81			/*
82			 * root node: special case. fpsize accounts for path
83			 * plus terminating zero. root node only has '/', so
84			 * fpsize should be 2, but we want to avoid the first
85			 * level nodes to have two '/' so we use fpsize 1 here
86			 */
87			fpsize = 1;
88			allocl = 2;
89			l = 1;
90			pathp = "";
91		} else {
92			/*
93			 * account for '/' and path size minus terminal 0
94			 * already in 'l'
95			 */
96			fpsize += l;
97			allocl = fpsize;
98		}
99	}
100
101	np = unflatten_dt_alloc(&mem, sizeof(struct device_node) + allocl,
102				__alignof__(struct device_node));
103	if (!dryrun) {
104		char *fn;
105
106		fn = (char *)np + sizeof(*np);
107		if (new_format) {
108			np->name = pathp;
109			has_name = 1;
110		}
111		np->full_name = fn;
112		if (new_format) {
113			/* rebuild full path for new format */
114			if (dad && dad->parent) {
115				strcpy(fn, dad->full_name);
116#ifdef DEBUG
117				if ((strlen(fn) + l + 1) != allocl) {
118					debug("%s: p: %d, l: %d, a: %d\n",
119					      pathp, (int)strlen(fn), l,
120					      allocl);
121				}
122#endif
123				fn += strlen(fn);
124			}
125			*(fn++) = '/';
126		}
127		memcpy(fn, pathp, l);
128
129		prev_pp = &np->properties;
130		if (dad != NULL) {
131			np->parent = dad;
132			np->sibling = dad->child;
133			dad->child = np;
134		}
135	}
136	/* process properties */
137	for (offset = fdt_first_property_offset(blob, *poffset);
138	     (offset >= 0);
139	     (offset = fdt_next_property_offset(blob, offset))) {
140		const char *pname;
141		int sz;
142
143		p = fdt_getprop_by_offset(blob, offset, &pname, &sz);
144		if (!p) {
145			offset = -FDT_ERR_INTERNAL;
146			break;
147		}
148
149		if (pname == NULL) {
150			debug("Can't find property name in list !\n");
151			break;
152		}
153		if (strcmp(pname, "name") == 0)
154			has_name = 1;
155		pp = unflatten_dt_alloc(&mem, sizeof(struct property),
156					__alignof__(struct property));
157		if (!dryrun) {
158			/*
159			 * We accept flattened tree phandles either in
160			 * ePAPR-style "phandle" properties, or the
161			 * legacy "linux,phandle" properties.  If both
162			 * appear and have different values, things
163			 * will get weird.  Don't do that. */
164			if ((strcmp(pname, "phandle") == 0) ||
165			    (strcmp(pname, "linux,phandle") == 0)) {
166				if (np->phandle == 0)
167					np->phandle = be32_to_cpup(p);
168			}
169			/*
170			 * And we process the "ibm,phandle" property
171			 * used in pSeries dynamic device tree
172			 * stuff */
173			if (strcmp(pname, "ibm,phandle") == 0)
174				np->phandle = be32_to_cpup(p);
175			pp->name = (char *)pname;
176			pp->length = sz;
177			pp->value = (__be32 *)p;
178			*prev_pp = pp;
179			prev_pp = &pp->next;
180		}
181	}
182	/*
183	 * with version 0x10 we may not have the name property, recreate
184	 * it here from the unit name if absent
185	 */
186	if (!has_name) {
187		const char *p1 = pathp, *ps = pathp, *pa = NULL;
188		int sz;
189
190		while (*p1) {
191			if ((*p1) == '@')
192				pa = p1;
193			if ((*p1) == '/')
194				ps = p1 + 1;
195			p1++;
196		}
197		if (pa < ps)
198			pa = p1;
199		sz = (pa - ps) + 1;
200		pp = unflatten_dt_alloc(&mem, sizeof(struct property) + sz,
201					__alignof__(struct property));
202		if (!dryrun) {
203			pp->name = "name";
204			pp->length = sz;
205			pp->value = pp + 1;
206			*prev_pp = pp;
207			prev_pp = &pp->next;
208			memcpy(pp->value, ps, sz - 1);
209			((char *)pp->value)[sz - 1] = 0;
210			debug("fixed up name for %s -> %s\n", pathp,
211			      (char *)pp->value);
212		}
213	}
214	if (!dryrun) {
215		*prev_pp = NULL;
216		if (!has_name)
217			np->name = of_get_property(np, "name", NULL);
218		np->type = of_get_property(np, "device_type", NULL);
219
220		if (!np->name)
221			np->name = "<NULL>";
222		if (!np->type)
223			np->type = "<NULL>";	}
224
225	old_depth = depth;
226	*poffset = fdt_next_node(blob, *poffset, &depth);
227	if (depth < 0)
228		depth = 0;
229	while (*poffset > 0 && depth > old_depth) {
230		mem = unflatten_dt_node(blob, mem, poffset, np, NULL,
231					fpsize, dryrun);
232		if (!mem)
233			return NULL;
234	}
235
236	if (*poffset < 0 && *poffset != -FDT_ERR_NOTFOUND) {
237		debug("unflatten: error %d processing FDT\n", *poffset);
238		return NULL;
239	}
240
241	/*
242	 * Reverse the child list. Some drivers assumes node order matches .dts
243	 * node order
244	 */
245	if (!dryrun && np->child) {
246		struct device_node *child = np->child;
247		np->child = NULL;
248		while (child) {
249			struct device_node *next = child->sibling;
250
251			child->sibling = np->child;
252			np->child = child;
253			child = next;
254		}
255	}
256
257	if (nodepp)
258		*nodepp = np;
259
260	return mem;
261}
262
263int unflatten_device_tree(const void *blob, struct device_node **mynodes)
264{
265	unsigned long size;
266	int start;
267	void *mem;
268
269	debug(" -> unflatten_device_tree()\n");
270
271	if (!blob) {
272		debug("No device tree pointer\n");
273		return -EINVAL;
274	}
275
276	debug("Unflattening device tree:\n");
277	debug("magic: %08x\n", fdt_magic(blob));
278	debug("size: %08x\n", fdt_totalsize(blob));
279	debug("version: %08x\n", fdt_version(blob));
280
281	if (fdt_check_header(blob)) {
282		debug("Invalid device tree blob header\n");
283		return -EINVAL;
284	}
285
286	/* First pass, scan for size */
287	start = 0;
288	size = (unsigned long)unflatten_dt_node(blob, NULL, &start, NULL, NULL,
289						0, true);
290	if (!size)
291		return -EFAULT;
292	size = ALIGN(size, 4);
293
294	debug("  size is %lx, allocating...\n", size);
295
296	/* Allocate memory for the expanded device tree */
297	mem = memalign(__alignof__(struct device_node), size + 4);
298	memset(mem, '\0', size);
299
300	/* Set up value for dm_test_livetree_align() */
301	*(u32 *)mem = BAD_OF_ROOT;
302
303	*(__be32 *)(mem + size) = cpu_to_be32(0xdeadbeef);
304
305	debug("  unflattening %p...\n", mem);
306
307	/* Second pass, do actual unflattening */
308	start = 0;
309	unflatten_dt_node(blob, mem, &start, NULL, mynodes, 0, false);
310	if (be32_to_cpup(mem + size) != 0xdeadbeef) {
311		debug("End of tree marker overwritten: %08x\n",
312		      be32_to_cpup(mem + size));
313		return -ENOSPC;
314	}
315
316	debug(" <- unflatten_device_tree()\n");
317
318	return 0;
319}
320
321int of_live_build(const void *fdt_blob, struct device_node **rootp)
322{
323	int ret;
324
325	debug("%s: start\n", __func__);
326	ret = unflatten_device_tree(fdt_blob, rootp);
327	if (ret) {
328		debug("Failed to create live tree: err=%d\n", ret);
329		return ret;
330	}
331	ret = of_alias_scan();
332	if (ret) {
333		debug("Failed to scan live tree aliases: err=%d\n", ret);
334		return ret;
335	}
336	debug("%s: stop\n", __func__);
337
338	return ret;
339}
340
341void of_live_free(struct device_node *root)
342{
343	/* the tree is stored as a contiguous block of memory */
344	free(root);
345}
346
347int of_live_create_empty(struct device_node **rootp)
348{
349	struct device_node *root;
350
351	root = calloc(1, sizeof(struct device_node));
352	if (!root)
353		return -ENOMEM;
354	root->name = strdup("");
355	if (!root->name) {
356		free(root);
357		return -ENOMEM;
358	}
359	root->type = "<NULL>";
360	root->full_name = "";
361	*rootp = root;
362
363	return 0;
364}
365
366static int check_space(int ret, struct abuf *buf)
367{
368	if (ret == -FDT_ERR_NOSPACE) {
369		if (!abuf_realloc_inc(buf, BUF_STEP))
370			return log_msg_ret("spc", -ENOMEM);
371		ret = fdt_resize(abuf_data(buf), abuf_data(buf),
372				 abuf_size(buf));
373		if (ret)
374			return log_msg_ret("res", -EFAULT);
375
376		return -EAGAIN;
377	}
378
379	return 0;
380}
381
382/**
383 * flatten_node() - Write out the node and its properties into a flat tree
384 */
385static int flatten_node(struct abuf *buf, const struct device_node *node)
386{
387	const struct device_node *np;
388	const struct property *pp;
389	int ret;
390
391	ret = fdt_begin_node(abuf_data(buf), node->name);
392	ret = check_space(ret, buf);
393	if (ret == -EAGAIN) {
394		ret = fdt_begin_node(abuf_data(buf), node->name);
395		if (ret) {
396			log_debug("Internal error a %d\n", ret);
397			return -EFAULT;
398		}
399	}
400	if (ret)
401		return log_msg_ret("beg", ret);
402
403	/* First write out the properties */
404	for (pp = node->properties; !ret && pp; pp = pp->next) {
405		ret = fdt_property(abuf_data(buf), pp->name, pp->value,
406				   pp->length);
407		ret = check_space(ret, buf);
408		if (ret == -EAGAIN) {
409			ret = fdt_property(abuf_data(buf), pp->name, pp->value,
410					   pp->length);
411		}
412	}
413
414	/* Next write out the subnodes */
415	for (np = node->child; np; np = np->sibling) {
416		ret = flatten_node(buf, np);
417		if (ret)
418			return log_msg_ret("sub", ret);
419	}
420
421	ret = fdt_end_node(abuf_data(buf));
422	ret = check_space(ret, buf);
423	if (ret == -EAGAIN) {
424		ret = fdt_end_node(abuf_data(buf));
425		if (ret) {
426			log_debug("Internal error b %d\n", ret);
427			return -EFAULT;
428		}
429	}
430	if (ret)
431		return log_msg_ret("end", ret);
432
433	return 0;
434}
435
436int of_live_flatten(const struct device_node *root, struct abuf *buf)
437{
438	int ret;
439
440	abuf_init(buf);
441	if (!abuf_realloc(buf, BUF_STEP))
442		return log_msg_ret("ini", -ENOMEM);
443
444	ret = fdt_create(abuf_data(buf), abuf_size(buf));
445	if (!ret)
446		ret = fdt_finish_reservemap(abuf_data(buf));
447	if (ret) {
448		log_debug("Failed to start FDT (err=%d)\n", ret);
449		return log_msg_ret("sta", -EINVAL);
450	}
451
452	ret = flatten_node(buf, root);
453	if (ret)
454		return log_msg_ret("flt", ret);
455
456	ret = fdt_finish(abuf_data(buf));
457	ret = check_space(ret, buf);
458	if (ret == -EAGAIN) {
459		ret = fdt_finish(abuf_data(buf));
460		if (ret) {
461			log_debug("Internal error c %d\n", ret);
462			return -EFAULT;
463		}
464	}
465	if (ret)
466		return log_msg_ret("fin", ret);
467
468	ret = fdt_pack(abuf_data(buf));
469	if (ret) {
470		log_debug("Failed to pack (err=%d)\n", ret);
471		return log_msg_ret("pac", -EFAULT);
472	}
473
474	if (!abuf_realloc(buf, fdt_totalsize(abuf_data(buf))))
475		return log_msg_ret("abu", -EFAULT);
476
477	return 0;
478}
479