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

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

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.felsch@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