[PATCH 18/23] security: optee: add optee_extract_fdto helper

[Marco Felsch]

Add a helper to extract the added OP-TEE FDT overlay fragments from the
FDT provided via arg2 during the boot phase into a dedicated buffer.

The dedicated overlay buffer can be used later on by the PBL code to
apply the overlay to the barebox runtime DTB and to the kernel DTB.

Signed-off-by: Marco Felsch <m.fel...@pengutronix.de>
---
 include/tee/optee.h |  6 ++++++
 security/optee.c    | 44 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 50 insertions(+)

diff --git a/include/tee/optee.h b/include/tee/optee.h
index 
10e829c04f8832c4c04771800f1963e25f425482..679662a6e6944da59d24d6268ebcf5dff19d0980
 100644
--- a/include/tee/optee.h
+++ b/include/tee/optee.h
@@ -37,6 +37,7 @@ int optee_verify_header (const struct optee_header *hdr);
 
 void optee_set_membase(const struct optee_header *hdr);
 int optee_get_membase(u64 *membase);
+int optee_extract_fdto(const void *fdt, void *fdto, unsigned int fdto_sz);
 
 #else
 
@@ -49,6 +50,11 @@ static inline int optee_get_membase(u64 *membase)
        return -ENOSYS;
 }
 
+static inline int optee_extract_fdto(const void *fdt, void *fdto, unsigned int 
fdto_sz)
+{
+       return 0;
+}
+
 #endif /* CONFIG_HAVE_OPTEE */
 
 #ifdef __PBL__
diff --git a/security/optee.c b/security/optee.c
index 
422bc1c90924ba8ab266b1aa8d06e52c819d2010..3750bb6f496af990d77f3629b470b174dd7a7c7a
 100644
--- a/security/optee.c
+++ b/security/optee.c
@@ -5,7 +5,9 @@
 #include <tee/optee.h>
 #include <linux/printk.h>
 #include <linux/errno.h>
+#include <linux/libfdt.h>
 #include <linux/limits.h>
+#include <pbl.h>
 
 static u64 optee_membase = U64_MAX;
 
@@ -60,3 +62,45 @@ void optee_set_membase(const struct optee_header *hdr)
        optee_membase = (u64)hdr->init_load_addr_hi << 32;
        optee_membase |= hdr->init_load_addr_lo;
 }
+
+int optee_extract_fdto(const void *fdt, void *fdto, unsigned int fdto_sz)
+{
+       int err;
+       int subnode;
+
+       err = fdt_check_header(fdt);
+       if (err) {
+               pr_err("Invalid device tree blob: %s\n", fdt_strerror(err));
+               return err;
+       }
+
+       err = fdt_create_empty_tree(fdto, fdto_sz);
+       if (err) {
+               pr_err("Failed to initialize OP-TEE FDTO space: %s\n", 
fdt_strerror(err));
+               return err;
+       }
+
+       fdt_for_each_subnode(subnode, fdt, 0) {
+               const char *name;
+               int len;
+
+               name = fdt_get_name(fdt, subnode, &len);
+               if (!name) {
+                       pr_warn("Failed to query FDT node name: %s\n", 
fdt_strerror(len));
+                       continue;
+               }
+
+               if (strncmp(name, "fragment@", 9) != 0) {
+                       pr_debug("Skip node %s\n", name);
+                       continue;
+               }
+
+               err |= fdt_copy_node(fdt, fdto, "/", name);
+       }
+
+       /* Clear the FDTO memory to not supply incomplete overlays */
+       if (err)
+               memset(fdto, 0, fdto_sz);
+
+       return err;
+}

-- 
2.47.3