[PATCH 20/23] ARM: i.MX8M: Add support to extract OP-TEE provided informations

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

Extract the OP-TEE overlay fragements information into a dedicated
overlay buffer and add the handoff data on success. The handoff data is
used by next commit.

Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
---
 arch/arm/mach-imx/esdctl.c | 40 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/arch/arm/mach-imx/esdctl.c b/arch/arm/mach-imx/esdctl.c
index 964d438e3f3433332e351d9a70f91857ee33e0b4..b9393fba4ab0f14992d2fec32b0e1b7f1d4bbdfd 100644
--- a/arch/arm/mach-imx/esdctl.c
+++ b/arch/arm/mach-imx/esdctl.c
@@ -1015,10 +1015,50 @@ resource_size_t imx8m_barebox_earlymem_size(unsigned buswidth)
 	return min_t(resource_size_t, SZ_4G - MX8M_DDR_CSD1_BASE_ADDR, size);
 }
 
+static void imx8m_extract_optee_fdto(void)
+{
+	unsigned int early_fdt_sz, fdto_size;
+	void *early_fdt, *fdto_dst;
+
+	imx_scratch_get_fdt(&early_fdt, &early_fdt_sz);
+	/* SCRATCH_FDT_SIZE == 0, e.g. if OP-TEE w/o CFG_DT support is used */
+	if (early_fdt_sz == 0)
+		return;
+
+	imx_scratch_get_optee_fdto(&fdto_dst, &fdto_size);
+	/* SCRATCH_FDTO_SIZE == 0 if feature (back-channel) is not required */
+	if (fdto_size == 0) {
+		pr_debug("OP-TEE dest DTBO size == 0, skip extracting\n");
+		return;
+	}
+
+	/*
+	 * OP-TEE DT handling is really cumbersome. In case an external DT was
+	 * supplied, OP-TEE re-use this DT and appends overlays.
+	 *
+	 * Extract the overlays into a single overlay file to make it easier
+	 * to apply these onto the barebox and kernel DT.
+	 *
+	 * It will become easier once barebox supports the transfer-list
+	 * protocol and the protocol itself has overlay-entry support.
+	 */
+	if (optee_extract_fdto(early_fdt, fdto_dst, fdto_size)) {
+		pr_warn("Failed to extract OP-TEE FDTO, continue without FDTO\n");
+		/*
+		 * Don't BUG() because the system may have compile-time config
+		 * support
+		 */
+		return;
+	}
+
+	handoff_data_add(HANDOFF_DATA_BL32_DT_OVL, fdto_dst, fdto_size);
+}
+
 static void __noreturn imx8m_barebox_entry(void *boarddata, unsigned buswidth)
 {
 	imx8m_init_scratch_space(buswidth, false);
 	optee_set_membase(imx_scratch_get_optee_hdr());
+	imx8m_extract_optee_fdto();
 	barebox_arm_entry(MX8M_DDR_CSD1_BASE_ADDR,
 			  imx8m_barebox_earlymem_size(buswidth), boarddata);
 }

-- 
2.47.3