[PATCH 15/18] ARM: i.MX8M: atf: add support for optee hdr parsing

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

The OP-TEE binary can contain a header with useful information like
loadaddr and size. This commit adds the support to take the header
loadaddr into account. If the header is not found the common builtin
loadaddr is used.

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

diff --git a/arch/arm/mach-imx/atf.c b/arch/arm/mach-imx/atf.c
index 327e160777df..eef3235b84f9 100644
--- a/arch/arm/mach-imx/atf.c
+++ b/arch/arm/mach-imx/atf.c
@@ -13,6 +13,7 @@
 #include <mach/imx/imx8m-regs.h>
 #include <soc/fsl/fsl_udc.h>
 #include <soc/fsl/caam.h>
+#include <tee/optee.h>
 
 /**
  * imx8m_atf_load_bl31 - Load ATF BL31 blob and transfer control to it
@@ -116,6 +117,24 @@ void imx8mm_load_bl33(void *bl33)
 	memcpy(bl33, __image_start, barebox_pbl_size);
 }
 
+static void imx_adjust_optee_memory(void **bl32, void **bl32_image, size_t *bl32_size)
+{
+	struct optee_header *hdr = *bl32_image;
+	u64 membase;
+
+	if (optee_verify_header(hdr))
+		return;
+
+	imx8m_scratch_save_optee_hdr(hdr);
+
+	membase = (u64)hdr->init_load_addr_hi << 32;
+	membase |= hdr->init_load_addr_lo;
+
+	*bl32 = (void *)membase;
+	*bl32_size -= sizeof(*hdr);
+	*bl32_image += sizeof(*hdr);
+}
+
 __noreturn void imx8mm_load_and_start_image_via_tfa(void)
 {
 	const void *bl31;
@@ -137,6 +156,8 @@ __noreturn void imx8mm_load_and_start_image_via_tfa(void)
 				bl33, &bl32_image,
 				&bl32_size);
 
+		imx_adjust_optee_memory(&bl32, &bl32_image, &bl32_size);
+
 		memcpy(bl32, bl32_image, bl32_size);
 
 		get_builtin_firmware(imx8mm_bl31_bin_optee, &bl31, &bl31_size);
@@ -201,6 +222,8 @@ __noreturn void imx8mp_load_and_start_image_via_tfa(void)
 				bl33, &bl32_image,
 				&bl32_size);
 
+		imx_adjust_optee_memory(&bl32, &bl32_image, &bl32_size);
+
 		memcpy(bl32, bl32_image, bl32_size);
 
 		get_builtin_firmware(imx8mp_bl31_bin_optee, &bl31, &bl31_size);
@@ -266,6 +289,8 @@ __noreturn void imx8mn_load_and_start_image_via_tfa(void)
 				bl33, &bl32_image,
 				&bl32_size);
 
+		imx_adjust_optee_memory(&bl32, &bl32_image, &bl32_size);
+
 		memcpy(bl32, bl32_image, bl32_size);
 
 		get_builtin_firmware(imx8mn_bl31_bin_optee, &bl31, &bl31_size);
@@ -324,6 +349,8 @@ __noreturn void imx8mq_load_and_start_image_via_tfa(void)
 				bl33, &bl32_image,
 				&bl32_size);
 
+		imx_adjust_optee_memory(&bl32, &bl32_image, &bl32_size);
+
 		memcpy(bl32, bl32_image, bl32_size);
 
 		get_builtin_firmware(imx8mq_bl31_bin_optee, &bl31, &bl31_size);
-- 
2.39.2