[PATCH 08/21] dm: core: Allow adding ofnode subnodes

[Simon Glass]

Add this feature to the ofnode interface, supporting both livetree and
flattree.

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

 drivers/core/of_access.c | 50 ++++++++++++++++++++++++++++++++++++++++
 drivers/core/ofnode.c    | 30 ++++++++++++++++++++++++
 include/dm/of_access.h   | 12 ++++++++++
 include/dm/ofnode.h      | 12 ++++++++++
 test/dm/ofnode.c         | 17 ++++++++++++++
 5 files changed, 121 insertions(+)

diff --git a/drivers/core/of_access.c b/drivers/core/of_access.c
index ee09bbb7550..765b21cecdb 100644
--- a/drivers/core/of_access.c
+++ b/drivers/core/of_access.c
@@ -931,3 +931,53 @@ 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;
+
+	__for_each_child_of_node(parent, child) {
+		if (!strncmp(child->name, name, len) && strlen(name) == len)
+			return -EEXIST;
+		last_sibling = child;
+	}
+
+	/* Property does not exist -> append new property */
+	new = calloc(1, sizeof(struct device_node));
+	if (!new)
+		return -ENOMEM;
+
+	new_name = malloc(len + 1);
+	if (!name) {
+		free(new);
+		return -ENOMEM;
+	}
+	strlcpy(new_name, name, len + 1);
+	new->name = new_name;
+
+	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;
+	}
+	strcpy(full_name, parent->full_name);
+	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 bf8eaf6d09a..f01bfac8f69 100644
--- a/drivers/core/ofnode.c
+++ b/drivers/core/ofnode.c
@@ -1227,3 +1227,33 @@ 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;
+
+	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, strlen(name), &child);
+		if (ret)
+			return ret;
+		subnode = np_to_ofnode(child);
+	} else {
+		int offset;
+
+		offset = fdt_add_subnode((void *)gd->fdt_blob,
+					 ofnode_to_offset(node), name);
+		if (offset < 0)
+			return offset == -FDT_ERR_EXISTS ? -EEXIST : 0;
+		subnode = offset_to_ofnode(offset);
+	}
+
+	*subnodep = subnode;
+
+	return 0;
+}
diff --git a/include/dm/of_access.h b/include/dm/of_access.h
index c0ac91a416a..20bf588be45 100644
--- a/include/dm/of_access.h
+++ b/include/dm/of_access.h
@@ -533,4 +533,16 @@ struct device_node *of_get_stdout(void);
 int of_write_prop(struct device_node *np, const char *propname, int len,
 		  const void *value);
 
+/**
+ * of_add_subnode() - add a new subnode to a node
+ *
+ * @node:	parent node to add to
+ * @name:	name of subnode
+ * @len:	length of name
+ * @subnodep:	returns pointer to new subnode
+ * Returns 0 if OK, -ve on error
+ */
+int of_add_subnode(struct device_node *node, const char *name, int len,
+		   struct device_node **subnodep);
+
 #endif
diff --git a/include/dm/ofnode.h b/include/dm/ofnode.h
index 018ee70c2ff..ebef8a6e2bb 100644
--- a/include/dm/ofnode.h
+++ b/include/dm/ofnode.h
@@ -1260,6 +1260,18 @@ static inline const char *ofnode_conf_read_str(const char *prop_name)
 {
 	return NULL;
 }
+
 #endif /* CONFIG_DM */
 
+/**
+ * of_add_subnode() - add a new subnode to a node
+ *
+ * @parent:	parent node to add to
+ * @name:	name of subnode
+ * @nodep:	returns pointer to new subnode
+ * Returns 0 if OK, -EEXIST if already exists, -ENOMEM if out of memory, other
+ * -ve on other error
+ */
+int ofnode_add_subnode(ofnode parent, const char *name, ofnode *nodep);
+
 #endif
diff --git a/test/dm/ofnode.c b/test/dm/ofnode.c
index f80993f8927..f5aabf42dbb 100644
--- a/test/dm/ofnode.c
+++ b/test/dm/ofnode.c
@@ -601,3 +601,20 @@ static int dm_test_ofnode_u32(struct unit_test_state *uts)
 }
 DM_TEST(dm_test_ofnode_u32,
 	UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT | UT_TESTF_LIVE_OR_FLAT);
+
+static int dm_test_ofnode_add_subnode(struct unit_test_state *uts)
+{
+	ofnode node, check, subnode;
+
+	node = ofnode_path("/lcd");
+	ut_assert(ofnode_valid(node));
+	ut_assertok(ofnode_add_subnode(node, "edmund", &subnode));
+	check = ofnode_path("/lcd/edmund");
+	ut_asserteq(check.of_offset, subnode.of_offset);
+
+	ut_asserteq(-EEXIST, ofnode_add_subnode(node, "edmund", &subnode));
+
+	return 0;
+}
+DM_TEST(dm_test_ofnode_add_subnode,
+	UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT | UT_TESTF_LIVE_OR_FLAT);
-- 
2.37.2.672.g94769d06f0-goog