[J-u-boot.git] / lib / of_live.c

// SPDX-License-Identifier: GPL-2.0+
/*
 * Copyright 2009 Benjamin Herrenschmidt, IBM Corp
 * [email protected]
 *
 * Based on parts of drivers/of/fdt.c from Linux v4.9
 * Modifications for U-Boot
 * Copyright (c) 2017 Google, Inc
 */

#include <common.h>
#include <linux/libfdt.h>
#include <of_live.h>
#include <malloc.h>
#include <dm/of_access.h>
#include <linux/err.h>

static void *unflatten_dt_alloc(void **mem, unsigned long size,
				unsigned long align)
{
	void *res;

	*mem = PTR_ALIGN(*mem, align);
	res = *mem;
	*mem += size;

	return res;
}

/**
 * unflatten_dt_node() - Alloc and populate a device_node from the flat tree
 * @blob: The parent device tree blob
 * @mem: Memory chunk to use for allocating device nodes and properties
 * @poffset: pointer to node in flat tree
 * @dad: Parent struct device_node
 * @nodepp: The device_node tree created by the call
 * @fpsize: Size of the node path up at t05he current depth.
 * @dryrun: If true, do not allocate device nodes but still calculate needed
 * memory size
 */
static void *unflatten_dt_node(const void *blob, void *mem, int *poffset,
			       struct device_node *dad,
			       struct device_node **nodepp,
			       unsigned long fpsize, bool dryrun)
{
	const __be32 *p;
	struct device_node *np;
	struct property *pp, **prev_pp = NULL;
	const char *pathp;
	int l;
	unsigned int allocl;
	static int depth;
	int old_depth;
	int offset;
	int has_name = 0;
	int new_format = 0;

	pathp = fdt_get_name(blob, *poffset, &l);
	if (!pathp)
		return mem;

	allocl = ++l;

	/*
	 * version 0x10 has a more compact unit name here instead of the full
	 * path. we accumulate the full path size using "fpsize", we'll rebuild
	 * it later. We detect this because the first character of the name is
	 * not '/'.
	 */
	if ((*pathp) != '/') {
		new_format = 1;
		if (fpsize == 0) {
			/*
			 * root node: special case. fpsize accounts for path
			 * plus terminating zero. root node only has '/', so
			 * fpsize should be 2, but we want to avoid the first
			 * level nodes to have two '/' so we use fpsize 1 here
			 */
			fpsize = 1;
			allocl = 2;
			l = 1;
			pathp = "";
		} else {
			/*
			 * account for '/' and path size minus terminal 0
			 * already in 'l'
			 */
			fpsize += l;
			allocl = fpsize;
		}
	}

	np = unflatten_dt_alloc(&mem, sizeof(struct device_node) + allocl,
				__alignof__(struct device_node));
	if (!dryrun) {
		char *fn;

		fn = (char *)np + sizeof(*np);
		np->full_name = fn;
		if (new_format) {
			/* rebuild full path for new format */
			if (dad && dad->parent) {
				strcpy(fn, dad->full_name);
#ifdef DEBUG
				if ((strlen(fn) + l + 1) != allocl) {
					debug("%s: p: %d, l: %d, a: %d\n",
					      pathp, (int)strlen(fn), l,
					      allocl);
				}
#endif
				fn += strlen(fn);
			}
			*(fn++) = '/';
		}
		memcpy(fn, pathp, l);

		prev_pp = &np->properties;
		if (dad != NULL) {
			np->parent = dad;
			np->sibling = dad->child;
			dad->child = np;
		}
	}
	/* process properties */
	for (offset = fdt_first_property_offset(blob, *poffset);
	     (offset >= 0);
	     (offset = fdt_next_property_offset(blob, offset))) {
		const char *pname;
		int sz;

		p = fdt_getprop_by_offset(blob, offset, &pname, &sz);
		if (!p) {
			offset = -FDT_ERR_INTERNAL;
			break;
		}

		if (pname == NULL) {
			debug("Can't find property name in list !\n");
			break;
		}
		if (strcmp(pname, "name") == 0)
			has_name = 1;
		pp = unflatten_dt_alloc(&mem, sizeof(struct property),
					__alignof__(struct property));
		if (!dryrun) {
			/*
			 * We accept flattened tree phandles either in
			 * ePAPR-style "phandle" properties, or the
			 * legacy "linux,phandle" properties.  If both
			 * appear and have different values, things
			 * will get weird.  Don't do that. */
			if ((strcmp(pname, "phandle") == 0) ||
			    (strcmp(pname, "linux,phandle") == 0)) {
				if (np->phandle == 0)
					np->phandle = be32_to_cpup(p);
			}
			/*
			 * And we process the "ibm,phandle" property
			 * used in pSeries dynamic device tree
			 * stuff */
			if (strcmp(pname, "ibm,phandle") == 0)
				np->phandle = be32_to_cpup(p);
			pp->name = (char *)pname;
			pp->length = sz;
			pp->value = (__be32 *)p;
			*prev_pp = pp;
			prev_pp = &pp->next;
		}
	}
	/*
	 * with version 0x10 we may not have the name property, recreate
	 * it here from the unit name if absent
	 */
	if (!has_name) {
		const char *p1 = pathp, *ps = pathp, *pa = NULL;
		int sz;

		while (*p1) {
			if ((*p1) == '@')
				pa = p1;
			if ((*p1) == '/')
				ps = p1 + 1;
			p1++;
		}
		if (pa < ps)
			pa = p1;
		sz = (pa - ps) + 1;
		pp = unflatten_dt_alloc(&mem, sizeof(struct property) + sz,
					__alignof__(struct property));
		if (!dryrun) {
			pp->name = "name";
			pp->length = sz;
			pp->value = pp + 1;
			*prev_pp = pp;
			prev_pp = &pp->next;
			memcpy(pp->value, ps, sz - 1);
			((char *)pp->value)[sz - 1] = 0;
			debug("fixed up name for %s -> %s\n", pathp,
			      (char *)pp->value);
		}
	}
	if (!dryrun) {
		*prev_pp = NULL;
		np->name = of_get_property(np, "name", NULL);
		np->type = of_get_property(np, "device_type", NULL);

		if (!np->name)
			np->name = "<NULL>";
		if (!np->type)
			np->type = "<NULL>";	}

	old_depth = depth;
	*poffset = fdt_next_node(blob, *poffset, &depth);
	if (depth < 0)
		depth = 0;
	while (*poffset > 0 && depth > old_depth) {
		mem = unflatten_dt_node(blob, mem, poffset, np, NULL,
					fpsize, dryrun);
		if (!mem)
			return NULL;
	}

	if (*poffset < 0 && *poffset != -FDT_ERR_NOTFOUND) {
		debug("unflatten: error %d processing FDT\n", *poffset);
		return NULL;
	}

	/*
	 * Reverse the child list. Some drivers assumes node order matches .dts
	 * node order
	 */
	if (!dryrun && np->child) {
		struct device_node *child = np->child;
		np->child = NULL;
		while (child) {
			struct device_node *next = child->sibling;

			child->sibling = np->child;
			np->child = child;
			child = next;
		}
	}

	if (nodepp)
		*nodepp = np;

	return mem;
}

/**
 * unflatten_device_tree() - create tree of device_nodes from flat blob
 *
 * unflattens a device-tree, creating the
 * tree of struct device_node. It also fills the "name" and "type"
 * pointers of the nodes so the normal device-tree walking functions
 * can be used.
 * @blob: The blob to expand
 * @mynodes: The device_node tree created by the call
 * @return 0 if OK, -ve on error
 */
static int unflatten_device_tree(const void *blob,
				 struct device_node **mynodes)
{
	unsigned long size;
	int start;
	void *mem;

	debug(" -> unflatten_device_tree()\n");

	if (!blob) {
		debug("No device tree pointer\n");
		return -EINVAL;
	}

	debug("Unflattening device tree:\n");
	debug("magic: %08x\n", fdt_magic(blob));
	debug("size: %08x\n", fdt_totalsize(blob));
	debug("version: %08x\n", fdt_version(blob));

	if (fdt_check_header(blob)) {
		debug("Invalid device tree blob header\n");
		return -EINVAL;
	}

	/* First pass, scan for size */
	start = 0;
	size = (unsigned long)unflatten_dt_node(blob, NULL, &start, NULL, NULL,
						0, true);
	if (!size)
		return -EFAULT;
	size = ALIGN(size, 4);

	debug("  size is %lx, allocating...\n", size);

	/* Allocate memory for the expanded device tree */
	mem = malloc(size + 4);
	memset(mem, '\0', size);

	*(__be32 *)(mem + size) = cpu_to_be32(0xdeadbeef);

	debug("  unflattening %p...\n", mem);

	/* Second pass, do actual unflattening */
	start = 0;
	unflatten_dt_node(blob, mem, &start, NULL, mynodes, 0, false);
	if (be32_to_cpup(mem + size) != 0xdeadbeef) {
		debug("End of tree marker overwritten: %08x\n",
		      be32_to_cpup(mem + size));
		return -ENOSPC;
	}

	debug(" <- unflatten_device_tree()\n");

	return 0;
}

int of_live_build(const void *fdt_blob, struct device_node **rootp)
{
	int ret;

	debug("%s: start\n", __func__);
	ret = unflatten_device_tree(fdt_blob, rootp);
	if (ret) {
		debug("Failed to create live tree: err=%d\n", ret);
		return ret;
	}
	ret = of_alias_scan();
	if (ret) {
		debug("Failed to scan live tree aliases: err=%d\n", ret);
		return ret;
	}
	debug("%s: stop\n", __func__);

	return ret;
}
Commit	Line	Data
83d290c5	1	// SPDX-License-Identifier: GPL-2.0+
8b50d526 SG	2	/*
	3	* Copyright 2009 Benjamin Herrenschmidt, IBM Corp
	4	* [email protected]
	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
8b50d526 SG	9	*/
	10
	11	#include <common.h>
b08c8c48	12	#include <linux/libfdt.h>
8b50d526 SG	13	#include <of_live.h>
	14	#include <malloc.h>
	15	#include <dm/of_access.h>
	16	#include <linux/err.h>
	17
8b50d526 SG	18	static void unflatten_dt_alloc(void *mem, unsigned long size,
	19	unsigned long align)
	20	{
	21	void *res;
	22
	23	mem = PTR_ALIGN(mem, align);
	24	res = *mem;
	25	*mem += size;
	26
	27	return res;
	28	}
	29
	30	/**
	31	* unflatten_dt_node() - Alloc and populate a device_node from the flat tree
	32	* @blob: The parent device tree blob
	33	* @mem: Memory chunk to use for allocating device nodes and properties
	34	* @poffset: pointer to node in flat tree
	35	* @dad: Parent struct device_node
	36	* @nodepp: The device_node tree created by the call
	37	* @fpsize: Size of the node path up at t05he current depth.
	38	* @dryrun: If true, do not allocate device nodes but still calculate needed
	39	* memory size
	40	*/
	41	static void unflatten_dt_node(const void blob, void mem, int poffset,
	42	struct device_node *dad,
	43	struct device_node **nodepp,
	44	unsigned long fpsize, bool dryrun)
	45	{
	46	const __be32 *p;
	47	struct device_node *np;
	48	struct property pp, *prev_pp = NULL;
	49	const char *pathp;
	50	int l;
	51	unsigned int allocl;
	52	static int depth;
	53	int old_depth;
	54	int offset;
	55	int has_name = 0;
	56	int new_format = 0;
	57
	58	pathp = fdt_get_name(blob, *poffset, &l);
	59	if (!pathp)
	60	return mem;
	61
	62	allocl = ++l;
	63
	64	/*
	65	* version 0x10 has a more compact unit name here instead of the full
	66	* path. we accumulate the full path size using "fpsize", we'll rebuild
	67	* it later. We detect this because the first character of the name is
	68	* not '/'.
	69	*/
	70	if ((*pathp) != '/') {
	71	new_format = 1;
	72	if (fpsize == 0) {
	73	/*
	74	* root node: special case. fpsize accounts for path
	75	* plus terminating zero. root node only has '/', so
	76	* fpsize should be 2, but we want to avoid the first
	77	* level nodes to have two '/' so we use fpsize 1 here
	78	*/
	79	fpsize = 1;
	80	allocl = 2;
	81	l = 1;
82	pathp = "";
83	} else {
84	/*
85	* account for '/' and path size minus terminal 0
86	* already in 'l'
87	*/
88	fpsize += l;
89	allocl = fpsize;
90	}
91	}
92
93	np = unflatten_dt_alloc(&mem, sizeof(struct device_node) + allocl,
94	__alignof__(struct device_node));
95	if (!dryrun) {
96	char *fn;
97
98	fn = (char )np + sizeof(np);
99	np->full_name = fn;
100	if (new_format) {
101	/* rebuild full path for new format */
102	if (dad && dad->parent) {
103	strcpy(fn, dad->full_name);
104	#ifdef DEBUG
105	if ((strlen(fn) + l + 1) != allocl) {
106	debug("%s: p: %d, l: %d, a: %d\n",
107	pathp, (int)strlen(fn), l,
108	allocl);
109	}
110	#endif
111	fn += strlen(fn);
112	}
113	*(fn++) = '/';
114	}
115	memcpy(fn, pathp, l);
116
117	prev_pp = &np->properties;
118	if (dad != NULL) {
119	np->parent = dad;
120	np->sibling = dad->child;
121	dad->child = np;
122	}
123	}
124	/* process properties */
125	for (offset = fdt_first_property_offset(blob, *poffset);
126	(offset >= 0);
127	(offset = fdt_next_property_offset(blob, offset))) {
128	const char *pname;
129	int sz;
130
131	p = fdt_getprop_by_offset(blob, offset, &pname, &sz);
132	if (!p) {
133	offset = -FDT_ERR_INTERNAL;
134	break;
135	}
136
137	if (pname == NULL) {
138	debug("Can't find property name in list !\n");
139	break;
140	}
141	if (strcmp(pname, "name") == 0)
142	has_name = 1;
143	pp = unflatten_dt_alloc(&mem, sizeof(struct property),
144	__alignof__(struct property));
145	if (!dryrun) {
146	/*
147	* We accept flattened tree phandles either in
148	* ePAPR-style "phandle" properties, or the
149	* legacy "linux,phandle" properties. If both
150	* appear and have different values, things
151	* will get weird. Don't do that. */
152	if ((strcmp(pname, "phandle") == 0) \|\|
153	(strcmp(pname, "linux,phandle") == 0)) {
154	if (np->phandle == 0)
155	np->phandle = be32_to_cpup(p);
156	}
157	/*
158	* And we process the "ibm,phandle" property
159	* used in pSeries dynamic device tree
160	* stuff */
161	if (strcmp(pname, "ibm,phandle") == 0)
162	np->phandle = be32_to_cpup(p);
163	pp->name = (char *)pname;
164	pp->length = sz;
165	pp->value = (__be32 *)p;
166	*prev_pp = pp;
167	prev_pp = &pp->next;
168	}
169	}
170	/*
171	* with version 0x10 we may not have the name property, recreate
172	* it here from the unit name if absent
173	*/
174	if (!has_name) {
175	const char p1 = pathp, ps = pathp, *pa = NULL;
176	int sz;
177
178	while (*p1) {
179	if ((*p1) == '@')
180	pa = p1;
181	if ((*p1) == '/')
182	ps = p1 + 1;
183	p1++;
184	}
185	if (pa < ps)
186	pa = p1;
187	sz = (pa - ps) + 1;
188	pp = unflatten_dt_alloc(&mem, sizeof(struct property) + sz,
189	__alignof__(struct property));
190	if (!dryrun) {
191	pp->name = "name";
192	pp->length = sz;
193	pp->value = pp + 1;
194	*prev_pp = pp;
195	prev_pp = &pp->next;
196	memcpy(pp->value, ps, sz - 1);
197	((char *)pp->value)[sz - 1] = 0;
198	debug("fixed up name for %s -> %s\n", pathp,
199	(char *)pp->value);
200	}
201	}
202	if (!dryrun) {
203	*prev_pp = NULL;
204	np->name = of_get_property(np, "name", NULL);
205	np->type = of_get_property(np, "device_type", NULL);
206
207	if (!np->name)
208	np->name = "<NULL>";
209	if (!np->type)
210	np->type = "<NULL>"; }
211
212	old_depth = depth;
213	poffset = fdt_next_node(blob, poffset, &depth);
214	if (depth < 0)
215	depth = 0;
c1eb3d59	216	while (*poffset > 0 && depth > old_depth) {
8b50d526 SG	217	mem = unflatten_dt_node(blob, mem, poffset, np, NULL,
8b50d526 SG	218	fpsize, dryrun);
c1eb3d59 SG	219	if (!mem)
	220	return NULL;
	221	}
8b50d526 SG	222
	223	if (poffset < 0 && poffset != -FDT_ERR_NOTFOUND) {
	224	debug("unflatten: error %d processing FDT\n", *poffset);
	225	return NULL;
	226	}
	227
	228	/*
	229	* Reverse the child list. Some drivers assumes node order matches .dts
	230	* node order
	231	*/
	232	if (!dryrun && np->child) {
	233	struct device_node *child = np->child;
	234	np->child = NULL;
	235	while (child) {
	236	struct device_node *next = child->sibling;
	237
	238	child->sibling = np->child;
	239	np->child = child;
	240	child = next;
	241	}
	242	}
	243
	244	if (nodepp)
	245	*nodepp = np;
	246
	247	return mem;
	248	}
	249
	250	/**
	251	* unflatten_device_tree() - create tree of device_nodes from flat blob
	252	*
	253	* unflattens a device-tree, creating the
	254	* tree of struct device_node. It also fills the "name" and "type"
	255	* pointers of the nodes so the normal device-tree walking functions
	256	* can be used.
	257	* @blob: The blob to expand
	258	* @mynodes: The device_node tree created by the call
	259	* @return 0 if OK, -ve on error
	260	*/
	261	static int unflatten_device_tree(const void *blob,
	262	struct device_node **mynodes)
	263	{
	264	unsigned long size;
	265	int start;
	266	void *mem;
	267
	268	debug(" -> unflatten_device_tree()\n");
	269
	270	if (!blob) {
	271	debug("No device tree pointer\n");
	272	return -EINVAL;
	273	}
	274
	275	debug("Unflattening device tree:\n");
	276	debug("magic: %08x\n", fdt_magic(blob));
	277	debug("size: %08x\n", fdt_totalsize(blob));
	278	debug("version: %08x\n", fdt_version(blob));
	279
	280	if (fdt_check_header(blob)) {
	281	debug("Invalid device tree blob header\n");
	282	return -EINVAL;
	283	}
	284
	285	/* First pass, scan for size */
286	start = 0;
287	size = (unsigned long)unflatten_dt_node(blob, NULL, &start, NULL, NULL,
288	0, true);
c1eb3d59 SG	289	if (!size)
c1eb3d59 SG	290	return -EFAULT;
8b50d526 SG	291	size = ALIGN(size, 4);
	292
	293	debug(" size is %lx, allocating...\n", size);
	294
	295	/* Allocate memory for the expanded device tree */
	296	mem = malloc(size + 4);
	297	memset(mem, '\0', size);
	298
	299	(__be32 )(mem + size) = cpu_to_be32(0xdeadbeef);
	300
	301	debug(" unflattening %p...\n", mem);
	302
	303	/* Second pass, do actual unflattening */
	304	start = 0;
	305	unflatten_dt_node(blob, mem, &start, NULL, mynodes, 0, false);
	306	if (be32_to_cpup(mem + size) != 0xdeadbeef) {
	307	debug("End of tree marker overwritten: %08x\n",
	308	be32_to_cpup(mem + size));
	309	return -ENOSPC;
	310	}
	311
	312	debug(" <- unflatten_device_tree()\n");
	313
	314	return 0;
	315	}
	316
	317	int of_live_build(const void fdt_blob, struct device_node *rootp)
	318	{
	319	int ret;
	320
	321	debug("%s: start\n", __func__);
	322	ret = unflatten_device_tree(fdt_blob, rootp);
	323	if (ret) {
	324	debug("Failed to create live tree: err=%d\n", ret);
	325	return ret;
	326	}
	327	ret = of_alias_scan();
	328	if (ret) {
	329	debug("Failed to scan live tree aliases: err=%d\n", ret);
	330	return ret;
	331	}
	332	debug("%s: stop\n", __func__);
	333
	334	return ret;
	335	}