1 files changed, 72 insertions, 11 deletions

diff --git a/drivers/core/of_access.c b/drivers/core/of_access.c
index df007e642bf..85f7da5a499 100644
--- a/drivers/core/of_access.c
+++ b/drivers/core/of_access.c
@@ -445,14 +445,15 @@ struct device_node *of_find_node_by_prop_value(struct device_node *from,
 	return np;
 }
 
-struct device_node *of_find_node_by_phandle(phandle handle)
+struct device_node *of_find_node_by_phandle(struct device_node *root,
+					    phandle handle)
 {
 	struct device_node *np;
 
 	if (!handle)
 		return NULL;
 
-	for_each_of_allnodes(np)
+	for_each_of_allnodes_from(root, np)
 		if (np->phandle == handle)
 			break;
 	(void)of_node_get(np);
@@ -470,8 +471,7 @@ struct device_node *of_find_node_by_phandle(phandle handle)
  * @len:	requested length of property value
  *
  * Return: the property value on success, -EINVAL if the property does not
- * exist, -ENODATA if property does not have a value, and -EOVERFLOW if the
- * property data isn't large enough.
+ * exist and -EOVERFLOW if the property data isn't large enough.
  */
 static void *of_find_property_value_of_size(const struct device_node *np,
 					    const char *propname, u32 len)
@@ -480,8 +480,6 @@ static void *of_find_property_value_of_size(const struct device_node *np,
 
 	if (!prop)
 		return ERR_PTR(-EINVAL);
-	if (!prop->value)
-		return ERR_PTR(-ENODATA);
 	if (len > prop->length)
 		return ERR_PTR(-EOVERFLOW);
 
@@ -697,7 +695,7 @@ static int __of_parse_phandle_with_args(const struct device_node *np,
 			 * below.
 			 */
 			if (cells_name || cur_index == index) {
-				node = of_find_node_by_phandle(phandle);
+				node = of_find_node_by_phandle(NULL, phandle);
 				if (!node) {
 					debug("%s: could not find phandle\n",
 					      np->full_name);
@@ -946,9 +944,6 @@ int of_write_prop(struct device_node *np, const char *propname, int len,
 		pp_last = pp;
 	}
 
-	if (!pp_last)
-		return -ENOENT;
-
 	/* Property does not exist -> append new property */
 	new = malloc(sizeof(struct property));
 	if (!new)
@@ -964,7 +959,73 @@ int of_write_prop(struct device_node *np, const char *propname, int len,
 	new->length = len;
 	new->next = NULL;
 
-	pp_last->next = new;
+	if (pp_last)
+		pp_last->next = new;
+	else
+		np->properties = new;
+
+	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;
 }