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 <[email protected]> --- 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
