X-Git-Url: https://scm.lunaixsky.com/lunaix-os.git/blobdiff_plain/22d06cbaaf660067bfea03f73bee9c8630c05d50..bb5ae5c85c0812c52e8d53187f2c6b0ee9525c8c:/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 ed11af9..489d4f7 100644
--- 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
+        blr x4
          
         // x0: ptr to boot_handoff
-        ldr x4, =kernel_bootstrap
-        blx x4
\ No newline at end of file
+        adr x4, kernel_bootstrap
+        blr x4
\ No newline at end of file