[PATCH] arm: spl: flush and disable cache before jumping to OPTEE

[Oleksandr Suvorov <oleksandr.suvorov@foundries.io>]

From: Ricardo Salveti <ricardo@foundries.io>

Make sure to flush, disable caches and interrupts before jumpint to
OPTEE. This fixes the SDP->SPL->OPTEE boot flow on iMX6Q and most
likely on some other ARM SoCs.

Signed-off-by: Ricardo Salveti <ricardo@foundries.io>
Co-developed-by: Oleksandr Suvorov <oleksandr.suvorov@foundries.io>
Signed-off-by: Oleksandr Suvorov <oleksandr.suvorov@foundries.io>
---

 arch/arm/lib/spl.c | 11 +++++++++++
 common/spl/spl.c   | 11 +++++++++--
 include/spl.h      |  9 +++++++++
 3 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/arch/arm/lib/spl.c b/arch/arm/lib/spl.c
index 8e2bdf3536..16e3cd557a 100644
--- a/arch/arm/lib/spl.c
+++ b/arch/arm/lib/spl.c
@@ -77,3 +77,14 @@ void __noreturn jump_to_image_linux(struct spl_image_info *spl_image)
 }
 #endif	/* CONFIG_ARM64 */
 #endif
+
+#if CONFIG_IS_ENABLED(OPTEE)
+void __noreturn jump_to_image_optee(struct spl_image_info *spl_image)
+{
+	/* flush and turn off caches before jumping to OPTEE */
+	cleanup_before_linux();
+
+	spl_optee_entry(NULL, NULL, spl_image->fdt_addr,
+			(void *)spl_image->entry_point);
+}
+#endif
diff --git a/common/spl/spl.c b/common/spl/spl.c
index d55d3c2848..f3fab3c4d9 100644
--- a/common/spl/spl.c
+++ b/common/spl/spl.c
@@ -171,6 +171,14 @@ __weak void spl_board_prepare_for_optee(void *fdt)
 {
 }
 
+#if CONFIG_IS_ENABLED(OPTEE)
+__weak void __noreturn jump_to_image_optee(struct spl_image_info *spl_image)
+{
+	spl_optee_entry(NULL, NULL, spl_image->fdt_addr,
+			(void *)spl_image->entry_point);
+}
+#endif
+
 __weak void spl_board_prepare_for_boot(void)
 {
 	/* Nothing to do! */
@@ -777,8 +785,7 @@ void board_init_r(gd_t *dummy1, ulong dummy2)
 	case IH_OS_TEE:
 		debug("Jumping to U-Boot via OP-TEE\n");
 		spl_board_prepare_for_optee(spl_image.fdt_addr);
-		spl_optee_entry(NULL, NULL, spl_image.fdt_addr,
-				(void *)spl_image.entry_point);
+		jump_to_image_optee(&spl_image);
 		break;
 #endif
 #if CONFIG_IS_ENABLED(OPENSBI)
diff --git a/include/spl.h b/include/spl.h
index afbf39bef4..8e7173d294 100644
--- a/include/spl.h
+++ b/include/spl.h
@@ -460,6 +460,15 @@ int spl_board_boot_device(u32 boot_device);
  */
 void __noreturn jump_to_image_linux(struct spl_image_info *spl_image);
 
+/**
+ * jump_to_image_linux() - Jump to OP-TEE OS from SPL
+ *
+ * This jumps into OP-TEE OS using the information in @spl_image.
+ *
+ * @spl_image: Image description to set up
+ */
+void __noreturn jump_to_image_optee(struct spl_image_info *spl_image);
+
 /**
  * spl_start_uboot() - Check if SPL should start the kernel or U-Boot
  *
-- 
2.31.1