[PATCH] aarch64: improve start code for aarch32 mode

[Gedare Bloom]

---
 bsps/aarch64/shared/start/start.S | 31 ++++++++++++++++++++++++++++---
 1 file changed, 28 insertions(+), 3 deletions(-)

diff --git a/bsps/aarch64/shared/start/start.S b/bsps/aarch64/shared/start/start.S
index f4c39dacdf..f47625ef99 100644
--- a/bsps/aarch64/shared/start/start.S
+++ b/bsps/aarch64/shared/start/start.S
@@ -107,30 +107,43 @@ _start:
 _el3_start:
   /* Drop from EL3 to EL2 */
 
+  msr CPTR_EL3, XZR
+
   /* Initialize HCR_EL2 and SCTLR_EL2 */
   msr HCR_EL2, XZR
   msr SCTLR_EL2, XZR
+
   /* Set EL2 Execution state via SCR_EL3 */
   mrs x0, SCR_EL3
+#ifdef AARCH64_MULTILIB_ARCH_V8_ILP32
+  bic x0, x0, #(1<<10)
+#else
   /* Set EL2 to AArch64 */
   orr x0, x0, #(1<<10)
-#ifdef AARCH64_IS_NONSECURE
+#endif
+#if defined(AARCH64_IS_NONSECURE)
   /* Set EL1 to NS */
   orr x0, x0, #1
 #endif
   msr SCR_EL3, x0
 
+#if defined(AARCH64_IS_NONSECURE)
   /* set EL2h mode for eret */
-#ifdef AARCH64_IS_NONSECURE
   mov x0, #0b01001
 #else
+  /* set EL1h mode for eret */
   mov x0, #0b00101
 #endif
 
+#if defined(AARCH64_MULTILIB_ARCH_V8_ILP32)
+  /* set aarch32 mode */
+  orr x0, x0, #(1<<4)
+#endif
+
   msr SPSR_EL3, x0
 
   /* Set EL2 entry point */
-#ifdef AARCH64_IS_NONSECURE
+#if defined(AARCH64_IS_NONSECURE)
   adr x0, _el2_start
 #else
   adr x0, _el1_start
@@ -143,8 +156,12 @@ _el2_start:
 
   /* Configure HCR_EL2 */
   mrs x0, HCR_EL2
+
+#ifndef AARCH64_MULTILIB_ARCH_V8_ILP32
   /* Set EL1 Execution state to AArch64 */
   orr x0, x0, #(1<<31)
+#endif
+
   /* Disable ID traps */
   bic x0, x0, #(1<<15)
   bic x0, x0, #(1<<16)
@@ -152,8 +169,16 @@ _el2_start:
   bic x0, x0, #(1<<18)
   msr HCR_EL2, x0
 
+  /* Disable coprocessor traps */
+  msr CPTR_EL2, XZR
+  msr HSTR_EL2, XZR
+
   /* Set to EL1h mode for eret */
   mov x0, #0b00101
+#if defined(AARCH64_MULTILIB_ARCH_V8_ILP32)
+  /* set aarch32 mode */
+  orr x0, x0, #(1<<4)
+#endif
   msr SPSR_EL2, x0
 
   /* Set EL1 entry point */
-- 
2.25.1