From 38f400d50ad315ee5cadb00a680f944818812314 Mon Sep 17 00:00:00 2001
From: bigmagic <zhaofujin@rt-thread.com>
Date: Mon, 25 May 2020 17:30:05 +0800
Subject: add raspi4 32bit mode bsp

---
 libcpu/arm/cortex-a/start_gcc.S | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

(limited to 'libcpu')

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
 
-- 
cgit v1.3.1