[PATCH 19/23] security: optee: add helpers to apply OP-TEE FDTO

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

Add helpers which can be used later on by barebox core to apply an
overlay provided by OP-TEE.

Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
---
 drivers/tee/optee/Kconfig    |  1 +
 drivers/tee/optee/of_fixup.c | 46 ++++++++++++++++++++++++++++++++++++++++++++
 include/tee/optee.h          | 11 +++++++++++
 3 files changed, 58 insertions(+)

diff --git a/drivers/tee/optee/Kconfig b/drivers/tee/optee/Kconfig
index 805aba65edb72e04341db34a1ada6ff94ab30add..ebfc895203287261cd86fc77d15cc9922403e5c0 100644
--- a/drivers/tee/optee/Kconfig
+++ b/drivers/tee/optee/Kconfig
@@ -45,3 +45,4 @@ endif
 config OF_FIXUP_OPTEE
 	def_bool y
 	depends on HAVE_OPTEE && ARM
+	select LIBFDT
diff --git a/drivers/tee/optee/of_fixup.c b/drivers/tee/optee/of_fixup.c
index 152322901a7e6af9c83da4c6a2d43f6e2c9af329..9df663d970671b658e38073496720033066106b2 100644
--- a/drivers/tee/optee/of_fixup.c
+++ b/drivers/tee/optee/of_fixup.c
@@ -1,10 +1,14 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 
+#include <linux/libfdt.h>
 #include <of.h>
 #include <linux/ioport.h>
+#include <linux/printk.h>
 #include <asm/barebox-arm.h>
 #include <tee/optee.h>
 
+static bool optee_ovl_applied;
+
 int of_optee_fixup(struct device_node *root, void *_data)
 {
 	struct of_optee_fixup_data *fixup_data = _data;
@@ -64,3 +68,45 @@ int of_optee_fixup(struct device_node *root, void *_data)
 
 	return of_fixup_reserved_memory(root, &res_shm);
 }
+
+void optee_register_overlay(void)
+{
+	struct device_node *overlay;
+	size_t size = 0;
+	void *fdto;
+	int err;
+
+	if (optee_ovl_applied) {
+		pr_warn("OP-TEE overlay already applied, skip\n");
+		return;
+	}
+
+	fdto = handoff_data_get_entry(HANDOFF_DATA_BL32_DT_OVL, &size);
+	if (!fdto || size == 0)
+		return;
+
+	err = fdt_check_header(fdto);
+	if (err) {
+		pr_warn("Invalid OP-TEE overlay found: %s\n", fdt_strerror(err));
+		return;
+	}
+
+	overlay = of_unflatten_dtb(fdto, size);
+	if (IS_ERR(overlay)) {
+		pr_warn("Failed to unflatten OP-TEE: %pe\n", overlay);
+		return;
+	}
+
+	err = of_register_overlay(overlay);
+	if (err) {
+		pr_warn("Failed to register OP-TEE overlay: %pe\n", ERR_PTR(err));
+		return;
+	}
+
+	optee_ovl_applied = true;
+}
+
+bool optee_overlay_applied(void)
+{
+	return optee_ovl_applied;
+}
diff --git a/include/tee/optee.h b/include/tee/optee.h
index 679662a6e6944da59d24d6268ebcf5dff19d0980..a3f1917f63957e9cf3193150604cf7c93876e9ce 100644
--- a/include/tee/optee.h
+++ b/include/tee/optee.h
@@ -38,6 +38,8 @@ 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);
+void optee_register_overlay(void);
+bool optee_overlay_applied(void);
 
 #else
 
@@ -55,6 +57,15 @@ static inline int optee_extract_fdto(const void *fdt, void *fdto, unsigned int f
 	return 0;
 }
 
+static inline void optee_register_overlay(void)
+{
+}
+
+static inline bool optee_overlay_applied(void)
+{
+	return false;
+}
+
 #endif /* CONFIG_HAVE_OPTEE */
 
 #ifdef __PBL__

-- 
2.47.3