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

fix up the ICC_SRE_ELx setting in case of presence of both EL2 and 3

diff --git a/lunaix-os/arch/aarch64/boot/init.c b/lunaix-os/arch/aarch64/boot/init.c
index d10675e60ea616ebd49241e3807c447c3419b19e..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.h>
+#include <asm/aa64_spsr.h>
 
 #include "init.h"
 
@@ -84,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)
 {

diff --git a/lunaix-os/arch/aarch64/boot/start.S b/lunaix-os/arch/aarch64/boot/start.S
index 13878da38ac93e7c88e40df94efdbd48c06f1d2c..05787b6f1f7dfcc3ca62b3215d0ef19ca1d24451 100644 (file)
--- a/lunaix-os/arch/aarch64/boot/start.S
+++ b/lunaix-os/arch/aarch64/boot/start.S
@@ -1,3 +1,5 @@
+#include <asm/aa64_msrs.h>
+
 .section .boot.data
     .align 4
     stack_end:
@@ -19,6 +21,22 @@
         mov sp, x4
         mov fp, xzr
 
+        mov x19, x0
+        bl aarch64_pre_el1_init
+
+        bl aarch64_prepare_el1_transfer
+        cbz x0, 1f
+
+        /* transfer to EL1 if we are currently not in */
+        adr x0, stack_top
+        msr SP_EL1, x0      /* EL1 stack */
+
+        adr x0, 1f
+        msr elr, x0         /* PC to resume at EL1, elr aliased to ELR_ELx */
+
+        eret
+    1:
+        mov x0, x19
         adr x4, aarch64_init
         bl x4
          

diff --git a/lunaix-os/arch/aarch64/includes/asm/aa64.h b/lunaix-os/arch/aarch64/includes/asm/aa64.h
index d9e8d9ac4153fb6c9e330a9bd3885d57cb110087..d3e05b1848d12032d9a9e6ac0c456fac7c672990 100644 (file)
--- a/lunaix-os/arch/aarch64/includes/asm/aa64.h
+++ b/lunaix-os/arch/aarch64/includes/asm/aa64.h
@@ -5,5 +5,6 @@
 #include "aa64_mmu.h"
 #include "aa64_msrs.h"
 #include "aa64_sysinst.h"
+#include "aa64_gic.h"
 
 #endif /* __LUNAIX_AA64_H */

diff --git a/lunaix-os/arch/aarch64/includes/asm/aa64_msrs.h b/lunaix-os/arch/aarch64/includes/asm/aa64_msrs.h
index f1da5f2fa3ac650e7bc971a5208f816884e596bb..546a2413a579fa9f78dd77831df5127763b59a76 100644 (file)
--- a/lunaix-os/arch/aarch64/includes/asm/aa64_msrs.h
+++ b/lunaix-os/arch/aarch64/includes/asm/aa64_msrs.h
@@ -11,12 +11,18 @@
 #define CurrentEL       __sr_encode(3, 0,  4,  2, 2)
 #define ELR_E1          __sr_encode(3, 0,  4,  0, 1)
 #define SPSel           __sr_encode(3, 0,  4,  2, 0)
+
 #define SPSR_EL1        __sr_encode(3, 0,  4,  0, 0)
+#define SPSR_EL2        __sr_encode(3, 4,  4,  0, 0)
+#define SPSR_EL3        __sr_encode(3, 6,  4,  0, 0)
+
 #define DAIF_EL1        __sr_encode(3, 3,  4,  2, 1)
 #define ALLINT_EL1      __sr_encode(3, 0,  4,  3, 0)
 #define SP_EL0          __sr_encode(3, 0,  4,  1, 0)
 #define SP_EL1          __sr_encode(3, 4,  4,  1, 0)
 
+#define ID_AA64PFR0_EL1          __sr_encode(3, 0,  0,  4, 0)
+
 #ifndef __ASM__
 #define read_sysreg(reg)                                    \
         ({  unsigned long _x;                               \

diff --git a/lunaix-os/arch/aarch64/includes/asm/aa64_spsr.h b/lunaix-os/arch/aarch64/includes/asm/aa64_spsr.h
index 8b5b46fae92cb6cbb2785470211932204fbbbfa5..04482b2c3d16ff0b8da39aa29dd6eb69b90a02c1 100644 (file)
--- a/lunaix-os/arch/aarch64/includes/asm/aa64_spsr.h
+++ b/lunaix-os/arch/aarch64/includes/asm/aa64_spsr.h
@@ -5,7 +5,12 @@
 #include <lunaix/bits.h>
 
 #define SPSR_EL          BITFIELD(3, 2)
-#define SPSR_SP          BIT(0)
+
+#define SPSR_SP          BITFLAG(0)
+#define SPSR_I           BITFLAG(7)
+#define SPSR_F           BITFLAG(6)
+#define SPSR_I           BITFLAG(7)
+#define SPSR_AllInt      BITFLAG(13)
 
 static inline bool
 spsr_from_el0(reg_t spsr)