[PATCH 20/25] ARM: i.MX: atf: add imx93_load_and_start_image_via_tfa()

[Sascha Hauer]

Signed-off-by: Sascha Hauer <s.ha...@pengutronix.de>
---
 arch/arm/mach-imx/atf.c  | 34 ++++++++++++++++++++++++++++++++++
 include/mach/imx/atf.h   |  2 ++
 include/mach/imx/xload.h |  1 +
 3 files changed, 37 insertions(+)

diff --git a/arch/arm/mach-imx/atf.c b/arch/arm/mach-imx/atf.c
index 1a0ac72fff..e1a89ef543 100644
--- a/arch/arm/mach-imx/atf.c
+++ b/arch/arm/mach-imx/atf.c
@@ -329,3 +329,37 @@ __noreturn void imx8mq_load_and_start_image_via_tfa(void)
 
        imx8m_atf_start_bl31(bl31, bl31_size, (void *)MX8MQ_ATF_BL31_BASE_ADDR);
 }
+
+void __noreturn imx93_load_and_start_image_via_tfa(void)
+{
+       unsigned long atf_dest = MX93_ATF_BL31_BASE_ADDR;
+       void __noreturn (*bl31)(void) = (void *)atf_dest;
+       const void *tfa;
+       size_t tfa_size;
+
+       /*
+        * On completion the TF-A will jump to MX93_ATF_BL33_BASE_ADDR
+        * in EL2. Copy the image there, but replace the PBL part of
+        * that image with ourselves. On a high assurance boot only the
+        * currently running code is validated and contains the checksum
+        * for the piggy data, so we need to ensure that we are running
+        * the same code in DRAM.
+        *
+        * The second purpose of this memcpy is for USB booting. When booting
+        * from USB the image comes in as a stream, so the PBL is transferred
+        * only once. As we jump into the PBL again in SDRAM we need to copy
+        * it there. The USB protocol transfers data in chunks of 1024 bytes,
+        * so align the copy size up to the next 1KiB boundary.
+        */
+       memcpy((void *)MX93_ATF_BL33_BASE_ADDR, __image_start, 
ALIGN(barebox_pbl_size, 1024));
+
+       get_builtin_firmware(imx93_bl31_bin, &tfa, &tfa_size);
+
+       memcpy(bl31, tfa, tfa_size);
+
+       asm volatile("msr sp_el2, %0" : :
+                    "r" (MX93_ATF_BL33_BASE_ADDR - 16) :
+                    "cc");
+       bl31();
+       __builtin_unreachable();
+}
diff --git a/include/mach/imx/atf.h b/include/mach/imx/atf.h
index 587e778635..fb367d6a70 100644
--- a/include/mach/imx/atf.h
+++ b/include/mach/imx/atf.h
@@ -17,5 +17,7 @@
 #define MX8M_ATF_BL33_BASE_ADDR                0x40200000
 #define MX8MM_ATF_BL33_BASE_ADDR       MX8M_ATF_BL33_BASE_ADDR
 #define MX8MQ_ATF_BL33_BASE_ADDR       MX8M_ATF_BL33_BASE_ADDR
+#define MX93_ATF_BL31_BASE_ADDR                0x204e0000
+#define MX93_ATF_BL33_BASE_ADDR                0x80200000
 
 #endif
diff --git a/include/mach/imx/xload.h b/include/mach/imx/xload.h
index 11c2738c2d..2c2d2fa3c5 100644
--- a/include/mach/imx/xload.h
+++ b/include/mach/imx/xload.h
@@ -30,6 +30,7 @@ void __noreturn imx8mm_load_and_start_image_via_tfa(void);
 void __noreturn imx8mn_load_and_start_image_via_tfa(void);
 void __noreturn imx8mp_load_and_start_image_via_tfa(void);
 void __noreturn imx8mq_load_and_start_image_via_tfa(void);
+void __noreturn imx93_load_and_start_image_via_tfa(void);
 
 int imx_load_image(ptrdiff_t address, ptrdiff_t entry, u32 offset,
                   u32 ivt_offset, bool start, unsigned int alignment,
-- 
2.39.2