1 files changed, 75 insertions, 60 deletions

diff --git a/bsps/aarch64/shared/start/start.S b/bsps/aarch64/shared/start/start.S
index bc6a855217..0237583463 100644
--- a/bsps/aarch64/shared/start/start.S
+++ b/bsps/aarch64/shared/start/start.S
@@ -55,6 +55,11 @@ _start:
   mov x5, x1    /* machine type number or ~0 for DT boot */
   mov x6, x2    /* physical address of ATAGs or DTB */
 #else /* BSP_START_NEEDS_REGISTER_INITIALIZATION */
+  /*
+   * This block is dead code. No aarch64 targets require this. It might be
+   * needed for hardware simulations or in future processor variants with
+   * lock-step cores.
+   */
   mov x0, XZR
   mov x1, XZR
   mov x2, XZR
@@ -87,8 +92,42 @@ _start:
   mov x29, XZR
   mov x30, XZR
 #ifdef AARCH64_MULTILIB_VFP
-#endif
-#endif
+  mov CPTR_EL3, XZR
+  mov CPTR_EL2, XZR
+  mov d0, XZR
+  mov d1, XZR
+  mov d2, XZR
+  mov d3, XZR
+  mov d4, XZR
+  mov d5, XZR
+  mov d6, XZR
+  mov d7, XZR
+  mov d8, XZR
+  mov d9, XZR
+  mov d10, XZR
+  mov d11, XZR
+  mov d12, XZR
+  mov d13, XZR
+  mov d14, XZR
+  mov d15, XZR
+  mov d16, XZR
+  mov d17, XZR
+  mov d18, XZR
+  mov d19, XZR
+  mov d20, XZR
+  mov d21, XZR
+  mov d22, XZR
+  mov d23, XZR
+  mov d24, XZR
+  mov d25, XZR
+  mov d26, XZR
+  mov d27, XZR
+  mov d28, XZR
+  mov d29, XZR
+  mov d30, XZR
+  mov d31, XZR
+#endif /* AARCH64_MULTILIB_VFP */
+#endif /* BSP_START_NEEDS_REGISTER_INITIALIZATION */
 
   /* Initialize SCTLR_EL1 */
   mov x0, XZR
@@ -105,6 +144,33 @@ _start:
   b.eq _el2_start
 
 _el3_start:
+  /*
+   * Before leaving the Secure World, we need to initialize the GIC. We
+   * do that here in an early stack context in EL3. This will NOT work
+   * on secondary core boot! We assume only the primary boot core will
+   * start in EL3 if any. Usually on real hardware, we should be running
+   * on top of trusted firmware and will not boot in EL3. Qemu fakes it
+   * for us and will start the primary core in EL3 and secondary cores
+   * will be brought up in EL1NS as expected.
+   */
+  #ifdef AARCH64_MULTILIB_ARCH_V8_ILP32
+    ldr w1, =_ISR_Stack_size
+    ldr w2, =_ISR_Stack_area_begin
+  #else
+    ldr x1, =_ISR_Stack_size
+    ldr x2, =_ISR_Stack_area_begin
+  #endif
+  add x3, x1, x2
+  /* using SP0 for the early init stack context at EL3 */
+  msr spsel, #0
+  mov sp, x3
+
+  /*
+   * Invoke the start hook 0.
+   * We don't set up exception handling, so this hook better behave.
+   */
+  bl bsp_start_hook_0
+
   /* Drop from EL3 to EL2 */
 
   /* Initialize HCR_EL2 and SCTLR_EL2 */
@@ -114,27 +180,16 @@ _el3_start:
   mrs x0, SCR_EL3
   /* Set EL2 to AArch64 */
   orr x0, x0, #(1<<10)
-#ifdef AARCH64_IS_NONSECURE
   /* Set EL1 to NS */
   orr x0, x0, #1
-#endif
   msr SCR_EL3, x0
 
   /* set EL2h mode for eret */
-#ifdef AARCH64_IS_NONSECURE
   mov x0, #0b01001
-#else
-  mov x0, #0b00101
-#endif
-
   msr SPSR_EL3, x0
 
   /* Set EL2 entry point */
-#ifdef AARCH64_IS_NONSECURE
   adr x0, _el2_start
-#else
-  adr x0, _el1_start
-#endif
   msr ELR_EL3, x0
   eret
 
@@ -201,8 +256,8 @@ _el1_start:
 #endif
   add x3, x1, x2
 
-  /* Disable interrupts */
-  msr DAIFSet, #0x2
+  /* Disable interrupts and debug */
+  msr DAIFSet, #0xa
 
 #ifdef BSP_START_NEEDS_REGISTER_INITIALIZATION
   mov x8, XZR
@@ -252,54 +307,14 @@ _el1_start:
 
   /* FPU does not need to be enabled on AArch64 */
 
-#ifdef BSP_START_NEEDS_REGISTER_INITIALIZATION
-  mov x0, #0
-  mov CPTR_EL3, XZR
-  mov CPTR_EL2, XZR
-  mov d0, XZR
-  mov d1, XZR
-  mov d2, XZR
-  mov d3, XZR
-  mov d4, XZR
-  mov d5, XZR
-  mov d6, XZR
-  mov d7, XZR
-  mov d8, XZR
-  mov d9, XZR
-  mov d10, XZR
-  mov d11, XZR
-  mov d12, XZR
-  mov d13, XZR
-  mov d14, XZR
-  mov d15, XZR
-  mov d16, XZR
-  mov d17, XZR
-  mov d18, XZR
-  mov d19, XZR
-  mov d20, XZR
-  mov d21, XZR
-  mov d22, XZR
-  mov d23, XZR
-  mov d24, XZR
-  mov d25, XZR
-  mov d26, XZR
-  mov d27, XZR
-  mov d28, XZR
-  mov d29, XZR
-  mov d30, XZR
-  mov d31, XZR
-#endif /* BSP_START_NEEDS_REGISTER_INITIALIZATION */
+  /* Ensure FPU traps are disabled by default */
+  mrs x0, FPCR
+  bic x0, x0, #((1 << 8) | (1 << 9) | (1 << 10) | (1 << 11) | (1 << 12))
+  bic x0, x0, #(1 << 15)
+  msr FPCR, x0
 
 #endif /* AARCH64_MULTILIB_VFP */
 
-  /*
-   * Invoke the start hook 0.
-   *
-   */
-
-  mov x1, x5    /* machine type number or ~0 for DT boot */
-  bl bsp_start_hook_0
-
   /* Branch to start hook 1 */
   bl bsp_start_hook_1