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

[lunaix-os.git] / lunaix-os / arch / aarch64 / boot / start.S

diff --git a/lunaix-os/arch/aarch64/boot/start.S b/lunaix-os/arch/aarch64/boot/start.S
index ed11af997b6fe26d50b45dd93ff9ea9572e82593..05787b6f1f7dfcc3ca62b3215d0ef19ca1d24451 100644 (file)
--- a/lunaix-os/arch/aarch64/boot/start.S
+++ b/lunaix-os/arch/aarch64/boot/start.S
@@ -1,5 +1,7 @@
+#include <asm/aa64_msrs.h>
+
 .section .boot.data
-    .align 16
+    .align 4
     stack_end:
         .skip 512
     stack_top:
@@ -15,12 +17,29 @@
             mov x3, xzr
      */
     start_:
-        ldr sp, =stack_top
+        adr x4, stack_top
+        mov sp, x4
         mov fp, xzr
 
-        ldr x4, =aarch64_init
-        blx x4
+        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
          
         // x0: ptr to boot_handoff
-        ldr x4, =kernel_bootstrap
-        blx x4
\ No newline at end of file
+        adr x4, kernel_bootstrap
+        bl x4
\ No newline at end of file