[PATCH 16/23] pbl: fdt: add fdt_copy_node helper

[Marco Felsch <m.felsch@pengutronix.de>]

Add a helper to copy a node from a src FDT into a dest FDT. The helper
is used later on by OP-TEE because OP-TEE appends FTD overlay fragements
into the provided FTD.

Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
---
 include/pbl.h |  2 ++
 pbl/fdt.c     | 82 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 84 insertions(+)

diff --git a/include/pbl.h b/include/pbl.h
index bf0c9e2ba29428361cb16a572295e66f20acdd82..0961b6b91e585a59a3e8facb1af261857cb6f57e 100644
--- a/include/pbl.h
+++ b/include/pbl.h
@@ -28,6 +28,8 @@ struct fdt_device_id {
 const void *
 fdt_device_get_match_data(const void *fdt, const char *nodepath,
 			  const struct fdt_device_id ids[]);
+int fdt_copy_node(const void *src_fdt, void *dest_fdt, const char *path,
+		  const char *nodename);
 
 int pbl_barebox_verify(const void *compressed_start, unsigned int len,
 		       const void *hash, unsigned int hash_len);
diff --git a/pbl/fdt.c b/pbl/fdt.c
index 0cd9f5a0f11723e9f9b6848e8553da481b92f2be..fa61c3c71298a2e036e0d324c09d452d8f0f7bbe 100644
--- a/pbl/fdt.c
+++ b/pbl/fdt.c
@@ -260,3 +260,85 @@ int pbl_load_fdt(const void *fdt, void *dest, int destsize)
 
 	return -EINVAL;
 }
+
+static int _fdt_copy_node(const void *src_fdt, const int src_node,
+			  void *dest_fdt, int dest_node)
+{
+	int property;
+	int subnode;
+
+	fdt_for_each_property_offset(property, src_fdt, src_node) {
+		const char *name;
+		const void *prop;
+		int prop_len;
+		int ret;
+
+		prop = fdt_getprop_by_offset(src_fdt, property, &name,
+					     &prop_len);
+		if (prop_len == -FDT_ERR_NOTFOUND)
+			return -FDT_ERR_INTERNAL;
+		if (prop_len < 0)
+			return prop_len;
+
+		ret = fdt_setprop(dest_fdt, dest_node, name, prop, prop_len);
+		if (ret)
+			return ret;
+	}
+
+	fdt_for_each_subnode(subnode, src_fdt, src_node) {
+		const char *name = fdt_get_name(src_fdt, subnode, NULL);
+		int node;
+		int ret;
+
+		node = fdt_add_subnode(dest_fdt, dest_node, name);
+		if (node == -FDT_ERR_EXISTS) {
+			node = fdt_subnode_offset(dest_fdt, dest_node, name);
+			if (node == -FDT_ERR_NOTFOUND)
+				return -FDT_ERR_INTERNAL;
+		}
+
+		if (node < 0)
+			return node;
+
+		ret = _fdt_copy_node(src_fdt, subnode, dest_fdt, node);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+int fdt_copy_node(const void *src_fdt, void *dest_fdt, const char *path,
+		  const char *nodename)
+{
+	int src_parent, src_node;
+	int dest_parent, dest_node;
+
+	pr_debug("Copy node %s%s\n", path, nodename);
+
+	src_parent = fdt_path_offset(src_fdt, path);
+	if (src_parent < 0) {
+		pr_err("Cannot find path: %s\n", fdt_strerror(src_parent));
+		return src_parent;
+	}
+
+	src_node = fdt_subnode_offset(src_fdt, src_parent, nodename);
+	if (src_node < 0) {
+		pr_err("Cannot find node: %s\n", fdt_strerror(src_node));
+		return src_node;
+	}
+
+	dest_parent = fdt_path_offset(dest_fdt, path);
+	if (dest_parent < 0) {
+		pr_err("Cannot find path: %s\n", fdt_strerror(dest_parent));
+		return dest_parent;
+	}
+
+	dest_node = fdt_add_subnode(dest_fdt, dest_parent, nodename);
+	if (dest_node < 0) {
+		pr_err("Failed to add node:%s with: %s\n", nodename, fdt_strerror(dest_node));
+		return dest_node;
+	}
+
+	return _fdt_copy_node(src_fdt, src_node, dest_fdt, dest_node);
+}

-- 
2.47.3