dm: core: Allow adding ofnode subnodes

Add this feature to the ofnode interface, supporting both livetree and
flattree. If the node exists it is returned, along with a -EEXIST error.
Update the functions it calls to handle this too.

Signed-off-by: Simon Glass <sjg@chromium.org>

2 files changed, 98 insertions, 0 deletions

diff --git a/drivers/core/of_access.c b/drivers/core/of_access.c
index f7743d4066..de6327199a 100644
--- a/drivers/core/of_access.c
+++ b/drivers/core/of_access.c
@@ -969,3 +969,66 @@ int of_write_prop(struct device_node *np, const char *propname, int len,
 
 	return 0;
 }
+
+int of_add_subnode(struct device_node *parent, const char *name, int len,
+		   struct device_node **childp)
+{
+	struct device_node *child, *new, *last_sibling = NULL;
+	char *new_name, *full_name;
+	int parent_fnl;
+
+	if (len == -1)
+		len = strlen(name);
+	__for_each_child_of_node(parent, child) {
+		/*
+		 * make sure we don't use a child called "trevor" when we are
+		 * searching for "trev".
+		 */
+		if (!strncmp(child->name, name, len) && strlen(name) == len) {
+			*childp = child;
+			return -EEXIST;
+		}
+		last_sibling = child;
+	}
+
+	/* Subnode does not exist -> append new subnode */
+	new = calloc(1, sizeof(struct device_node));
+	if (!new)
+		return -ENOMEM;
+
+	new_name = memdup(name, len + 1);
+	if (!new_name) {
+		free(new);
+		return -ENOMEM;
+	}
+	new_name[len] = '\0';
+
+	/*
+	 * if the parent is the root node (named "") we don't need to prepend
+	 * its full path
+	 */
+	parent_fnl = *parent->name ? strlen(parent->full_name) : 0;
+	full_name = calloc(1, parent_fnl + 1 + len + 1);
+	if (!full_name) {
+		free(new_name);
+		free(new);
+		return -ENOMEM;
+	}
+	new->name = new_name;	/* assign to constant pointer */
+
+	strcpy(full_name, parent->full_name); /* "" for root node */
+	full_name[parent_fnl] = '/';
+	strlcpy(&full_name[parent_fnl + 1], name, len + 1);
+	new->full_name = full_name;
+
+	/* Add as last sibling of the parent */
+	if (last_sibling)
+		last_sibling->sibling = new;
+	if (!parent->child)
+		parent->child = new;
+	new->parent = parent;
+
+	*childp = new;
+
+	return 0;
+}
diff --git a/drivers/core/ofnode.c b/drivers/core/ofnode.c
index b241be3b9f..8683e03c33 100644
--- a/drivers/core/ofnode.c
+++ b/drivers/core/ofnode.c
@@ -1289,3 +1289,38 @@ phy_interface_t ofnode_read_phy_mode(ofnode node)
 
 	return PHY_INTERFACE_MODE_NA;
 }
+
+int ofnode_add_subnode(ofnode node, const char *name, ofnode *subnodep)
+{
+	ofnode subnode;
+	int ret = 0;
+
+	assert(ofnode_valid(node));
+
+	if (ofnode_is_np(node)) {
+		struct device_node *np, *child;
+
+		np = (struct device_node *)ofnode_to_np(node);
+		ret = of_add_subnode(np, name, -1, &child);
+		if (ret && ret != -EEXIST)
+			return ret;
+		subnode = np_to_ofnode(child);
+	} else {
+		void *fdt = (void *)gd->fdt_blob;
+		int poffset = ofnode_to_offset(node);
+		int offset;
+
+		offset = fdt_add_subnode(fdt, poffset, name);
+		if (offset == -FDT_ERR_EXISTS) {
+			offset = fdt_subnode_offset(fdt, poffset, name);
+			ret = -EEXIST;
+		}
+		if (offset < 0)
+			return -EINVAL;
+		subnode = offset_to_ofnode(offset);
+	}
+
+	*subnodep = subnode;
+
+	return ret;	/* 0 or -EEXIST */
+}