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 ---
Changes in v2: - Return the existing node with -EEXIST - Allow len to be passed in as -1 - Add a comment as to why strncmp() is not enough - Add a comment why the len parameter is needed - Add a comment why the new_name variable is used - Add tests for the memory checks in of_add_subnode() - Fix memory checks - Fix 'property' comment - Add lots more tests and a few other fixes - Gosh that code was a mess
drivers/core/of_access.c | 63 +++++++++++++++++++++++++++++++++++ drivers/core/ofnode.c | 35 ++++++++++++++++++++ include/dm/of_access.h | 15 +++++++++ include/dm/ofnode.h | 13 ++++++++ test/dm/ofnode.c | 71 ++++++++++++++++++++++++++++++++++++++++ 5 files changed, 197 insertions(+)
diff --git a/drivers/core/of_access.c b/drivers/core/of_access.c index ee09bbb7550..8306d69eed0 100644 --- a/drivers/core/of_access.c +++ b/drivers/core/of_access.c @@ -931,3 +931,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 bf8eaf6d09a..855f5f40647 100644 --- a/drivers/core/ofnode.c +++ b/drivers/core/ofnode.c @@ -1227,3 +1227,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 */ +} diff --git a/include/dm/of_access.h b/include/dm/of_access.h index c0ac91a416a..ed94dd3d3a4 100644 --- a/include/dm/of_access.h +++ b/include/dm/of_access.h @@ -533,4 +533,19 @@ 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 (so the caller does not need to nul-terminate a + * partial string), or -1 for strlen(@name) + * @subnodep: returns pointer to new subnode (valid if the function returns 0 + * or -EEXIST) + * Returns 0 if OK, -EEXIST if already exists, -ENOMEM if out of memory, other + * -ve on other 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..e09127d1ce2 100644 --- a/include/dm/ofnode.h +++ b/include/dm/ofnode.h @@ -1260,6 +1260,19 @@ 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 (valid if the function returns 0 + * or -EEXIST) + * 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 4624a08d275..543dc546b95 100644 --- a/test/dm/ofnode.c +++ b/test/dm/ofnode.c @@ -605,3 +605,74 @@ 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; + char buf[128]; + + /* temporarily disable this test due to a failure fixed later */ + if (!of_live_active()) + return 0; + + node = ofnode_path("/lcd"); + ut_assert(ofnode_valid(node)); + ut_assertok(ofnode_add_subnode(node, "edmund", &subnode)); + check = ofnode_path("/lcd/edmund"); + ut_asserteq(subnode.of_offset, check.of_offset); + ut_assertok(ofnode_get_path(subnode, buf, sizeof(buf))); + ut_asserteq_str("/lcd/edmund", buf); + + if (of_live_active()) { + struct device_node *child; + + ut_assertok(of_add_subnode((void *)ofnode_to_np(node), "edmund", + 2, &child)); + ut_asserteq_str("ed", child->name); + ut_asserteq_str("/lcd/ed", child->full_name); + check = ofnode_path("/lcd/ed"); + ut_asserteq_ptr(child, check.np); + ut_assertok(ofnode_get_path(np_to_ofnode(child), buf, + sizeof(buf))); + ut_asserteq_str("/lcd/ed", buf); + } + + /* An existing node should be returned with -EEXIST */ + ut_asserteq(-EEXIST, ofnode_add_subnode(node, "edmund", &check)); + ut_asserteq(subnode.of_offset, check.of_offset); + + /* add a root node */ + node = ofnode_path("/"); + ut_assert(ofnode_valid(node)); + ut_assertok(ofnode_add_subnode(node, "lcd2", &subnode)); + check = ofnode_path("/lcd2"); + ut_asserteq(subnode.of_offset, check.of_offset); + ut_assertok(ofnode_get_path(subnode, buf, sizeof(buf))); + ut_asserteq_str("/lcd2", buf); + + if (of_live_active()) { + ulong start; + int i; + + /* + * Make sure each of the three malloc()checks in + * of_add_subnode() work + */ + for (i = 0; i < 3; i++) { + malloc_enable_testing(i); + start = ut_check_free(); + ut_asserteq(-ENOMEM, ofnode_add_subnode(node, "anthony", + &check)); + ut_assertok(ut_check_delta(start)); + } + + /* This should pass since we allow 3 allocations */ + malloc_enable_testing(3); + ut_assertok(ofnode_add_subnode(node, "anthony", &check)); + malloc_disable_testing(); + } + + return 0; +} +DM_TEST(dm_test_ofnode_add_subnode, + UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT | UT_TESTF_LIVE_OR_FLAT);