3 files changed, 42 insertions, 27 deletions

diff --git a/src/include/device_tree.h b/src/include/device_tree.h
index 96902b893b..8280dad077 100644
--- a/src/include/device_tree.h
+++ b/src/include/device_tree.h
@@ -47,6 +47,7 @@ struct fdt_header {
 #define FDT_TOKEN_END_NODE	2
 #define FDT_TOKEN_PROPERTY	3
 #define FDT_TOKEN_END		9
+#define FDT_PHANDLE_ILLEGAL	0xdeadbeef
 
 struct fdt_property
 {
@@ -71,6 +72,8 @@ struct device_tree_property
 struct device_tree_node
 {
 	const char *name;
+	uint32_t phandle;
+
 	// List of struct device_tree_property-s.
 	struct list_node properties;
 	// List of struct device_tree_nodes.
@@ -91,6 +94,7 @@ struct device_tree
 {
 	const void *header;
 	uint32_t header_size;
+	uint32_t max_phandle;
 
 	struct list_node reserve_map;
 
@@ -136,6 +140,8 @@ void dt_read_cell_props(const struct device_tree_node *node, u32 *addrcp,
 // represented as an array of strings.
 struct device_tree_node *dt_find_node(struct device_tree_node *parent, const char **path,
 			     u32 *addrcp, u32 *sizecp, int create);
+struct device_tree_node *dt_find_node_by_phandle(struct device_tree_node *root,
+						 uint32_t phandle);
 // Look up or create a node relative to a parent node, through its path
 // represented as a string of '/' separated node names.
 struct device_tree_node *dt_find_node_by_path(struct device_tree_node *parent, const char *path,
@@ -150,8 +156,6 @@ struct device_tree_node *dt_find_next_compat_child(struct device_tree_node *pare
 // Look up a node relative to a parent node, through its property value.
 struct device_tree_node *dt_find_prop_value(struct device_tree_node *parent, const char *name,
 				   void *data, size_t size);
-// Return the phandle
-uint32_t dt_get_phandle(const struct device_tree_node *node);
 // Write src into *dest as a 'length'-byte big-endian integer.
 void dt_write_int(u8 *dest, u64 src, size_t length);
 // Delete a property
diff --git a/src/lib/device_tree.c b/src/lib/device_tree.c
index 7a3128efcf..44eca4e2a8 100644
--- a/src/lib/device_tree.c
+++ b/src/lib/device_tree.c
@@ -66,6 +66,12 @@ int fdt_node_name(const void *blob, uint32_t offset, const char **name)
 	return ALIGN_UP(strlen((char *)ptr) + 1, sizeof(uint32_t)) + 4;
 }
 
+static int dt_prop_is_phandle(struct device_tree_property *prop)
+{
+	return !(strcmp("phandle", prop->prop.name) &&
+		 strcmp("linux,phandle", prop->prop.name));
+}
+
 
 
 /*
@@ -156,6 +162,7 @@ int fdt_skip_node(const void *blob, uint32_t start_offset)
  */
 
 static int fdt_unflatten_node(const void *blob, uint32_t start_offset,
+			      struct device_tree *tree,
 			      struct device_tree_node **new_node)
 {
 	struct list_node *last;
@@ -178,6 +185,12 @@ static int fdt_unflatten_node(const void *blob, uint32_t start_offset,
 		struct device_tree_property *prop = xzalloc(sizeof(*prop));
 		prop->prop = fprop;
 
+		if (dt_prop_is_phandle(prop)) {
+			node->phandle = be32dec(prop->prop.data);
+			if (node->phandle > tree->max_phandle)
+				tree->max_phandle = node->phandle;
+		}
+
 		list_insert_after(&prop->list_node, last);
 		last = &prop->list_node;
 
@@ -186,7 +199,7 @@ static int fdt_unflatten_node(const void *blob, uint32_t start_offset,
 
 	struct device_tree_node *child;
 	last = &node->children;
-	while ((size = fdt_unflatten_node(blob, offset, &child))) {
+	while ((size = fdt_unflatten_node(blob, offset, tree, &child))) {
 		list_insert_after(&child->list_node, last);
 		last = &child->list_node;
 
@@ -260,7 +273,7 @@ struct device_tree *fdt_unflatten(const void *blob)
 		offset += size;
 	}
 
-	fdt_unflatten_node(blob, struct_offset, &tree->root);
+	fdt_unflatten_node(blob, struct_offset, tree, &tree->root);
 
 	return tree;
 }
@@ -590,6 +603,26 @@ struct device_tree_node *dt_find_node_by_path(struct device_tree_node *parent,
 	return node;
 }
 
+struct device_tree_node *dt_find_node_by_phandle(struct device_tree_node *root,
+						 uint32_t phandle)
+{
+	if (!root)
+		return NULL;
+
+	if (root->phandle == phandle)
+		return root;
+
+	struct device_tree_node *node;
+	struct device_tree_node *result;
+	list_for_each(node, root->children, list_node) {
+		result = dt_find_node_by_phandle(node, phandle);
+		if (result)
+			return result;
+	}
+
+	return NULL;
+}
+
 /*
  * Check if given node is compatible.
  *
@@ -719,28 +752,6 @@ struct device_tree_node *dt_find_prop_value(struct device_tree_node *parent,
 	return NULL;
 }
 
-/**
- * Find the phandle of a node.
- *
- * @param node Pointer to node containing the phandle
- * @return Zero on error, the phandle on success
- */
-uint32_t dt_get_phandle(const struct device_tree_node *node)
-{
-	const uint32_t *phandle;
-	size_t len;
-
-	dt_find_bin_prop(node, "phandle", (const void **)&phandle, &len);
-	if (phandle != NULL && len == sizeof(*phandle))
-		return be32_to_cpu(*phandle);
-
-	dt_find_bin_prop(node, "linux,phandle", (const void **)&phandle, &len);
-	if (phandle != NULL && len == sizeof(*phandle))
-		return be32_to_cpu(*phandle);
-
-	return 0;
-}
-
 /*
  * Write an arbitrary sized big-endian integer into a pointer.
  *
diff --git a/src/soc/cavium/cn81xx/soc.c b/src/soc/cavium/cn81xx/soc.c
index 98166b056f..05007493df 100644
--- a/src/soc/cavium/cn81xx/soc.c
+++ b/src/soc/cavium/cn81xx/soc.c
@@ -234,7 +234,7 @@ static int dt_platform_fixup(struct device_tree_fixup *fixup,
 			continue;
 		}
 		/* Store the phandle */
-		phandle = dt_get_phandle(dt_node);
+		phandle = dt_node->phandle;
 		printk(BIOS_INFO, "%s: Removing node %s\n", __func__, path);
 		list_remove(&dt_node->list_node);