add el1 transfer at the boot stage in case higher EL is implemented

[lunaix-os.git] / lunaix-os / arch / aarch64 / boot / init.c

diff --git a/lunaix-os/arch/aarch64/boot/init.c b/lunaix-os/arch/aarch64/boot/init.c
index 06b29802bcf45dd018fce6ff46deccc41bfa7034..42aba1fa6c917ff5108708c7c60ab654035019aa 100644 (file)
--- a/lunaix-os/arch/aarch64/boot/init.c
+++ b/lunaix-os/arch/aarch64/boot/init.c
@@ -1,5 +1,6 @@
 #include <lunaix/boot_generic.h>
-#include <asm/aa64_msrs.h>
+#include <asm/aa64.h>
+#include <asm/aa64_spsr.h>
 
 #include "init.h"
 
@@ -27,10 +28,12 @@ setup_pstate()
    set_sysreg(SPSel, 1);
 }
 
+extern void aa64_vbase();
+
 static inline void
 setup_evbar()
 {
-    // TODO install exception vectors, setup VBAR
+    set_sysreg(VBAR_EL1, __ptr(aa64_vbase));
 }
 
 static inline void
@@ -82,6 +85,71 @@ extract_dtb_bootinfo(ptr_t dtb, struct boot_handoff* handoff)
     // TODO extract /memory, /reserved-memories from dtb
 }
 
+static inline void
+setup_gic_sysreg()
+{
+    int el;
+    bool has_el2;
+    u64_t pfr;
+
+    pfr = read_sysreg(ID_AA64PFR0_EL1);
+    el = read_sysreg(CurrentEL) >> 2;
+
+    // Arm A-Profile: D19.2.69, check for GIC sysreg avaliability
+
+    if (!BITS_GET(pfr, BITFIELD(27, 24))) {
+        return;
+    }
+
+    has_el2 = !!BITS_GET(pfr, BITFIELD(11, 8));
+
+    // GIC spec (v3,v4): 12.1.7, table 12-2
+    // GIC spec (v3,v4): ICC_SRE_ELx accessing
+    // Arm A-Profile: R_PCDTX, PSTATE.EL reset value
+
+    if (el == 3) {
+        sysreg_flagging(ICC_SRE_EL3, ICC_SRE_SRE, 0);
+    }
+    
+    if (has_el2) {
+        // el must be >= 2
+        sysreg_flagging(ICC_SRE_EL2, ICC_SRE_SRE, 0);
+    }
+
+    sysreg_flagging(ICC_SRE_EL1, ICC_SRE_SRE, 0);
+}
+
+void
+aarch64_pre_el1_init()
+{
+    setup_gic_sysreg();
+}
+
+bool
+aarch64_prepare_el1_transfer()
+{
+    ptr_t spsr;
+    int el;
+
+    el = read_sysreg(CurrentEL) >> 2;
+
+    if (el == 1) {
+        return false;
+    }
+
+    spsr = SPSR_AllInt | SPSR_I | SPSR_F | SPSR_SP;
+    spsr = BITS_SET(spsr, SPSR_EL, 1);
+
+    if (el == 2) {
+        set_sysreg(SPSR_EL2, spsr);
+    }
+    else {
+        set_sysreg(SPSR_EL3, spsr);
+    }
+
+    return true;
+}
+
 struct boot_handoff*
 aarch64_init(ptr_t dtb)
 {