add raspi4 32bit mode bsp

1 files changed, 20 insertions, 0 deletions

diff --git a/libcpu/arm/cortex-a/start_gcc.S b/libcpu/arm/cortex-a/start_gcc.S
index 268cc7ef56..a8f3a36630 100644
--- a/libcpu/arm/cortex-a/start_gcc.S
+++ b/libcpu/arm/cortex-a/start_gcc.S
@@ -52,6 +52,26 @@ stack_top:
 /* reset entry */
 .globl _reset
 _reset:
+#ifdef ARCH_ARMV8
+    /* Check for HYP mode */
+    mrs r0, cpsr_all
+    and r0, r0, #0x1F
+    mov r8, #0x1A
+    cmp r0, r8
+    beq overHyped
+    b continue
+
+overHyped: /* Get out of HYP mode */
+    ldr r1, =continue
+    msr ELR_hyp, r1
+    mrs r1, cpsr_all
+    and r1, r1, #0x1f    ;@ CPSR_MODE_MASK
+    orr r1, r1, #0x13    ;@ CPSR_MODE_SUPERVISOR
+    msr SPSR_hyp, r1
+    eret
+
+continue:
+#endif
     /* set the cpu to SVC32 mode and disable interrupt */
     cps #Mode_SVC