Draft: start lab 3

kernel/Start.S

@@ -14,6 +14,9 @@ wait:
 
 run: // cpu id == 0
   mov x20, x0
+
+	bl el2_to_el1
+
 	adrp x0, __stack_end
 	mov sp, x0
 
@@ -25,3 +28,11 @@ run: // cpu id == 0
 	mov x0, x20
 	bl main
 	b wait
+
+el2_to_el1:
+	mov x0, (1 << 31)
+	msr hcr_el2, x0
+	mov x0, 0x3c5
+	msr spsr_el2, x0
+	msr elr_el2, lr
+	eret

kernel/exception_handler.S

@@ -0,0 +1,88 @@
+.section ".text.exception"
+
+// save general registers to stack
+.macro save_all
+	sub sp, sp, 32 * 8
+	stp x0, x1, [sp ,16 * 0]
+	stp x2, x3, [sp ,16 * 1]
+	stp x4, x5, [sp ,16 * 2]
+	stp x6, x7, [sp ,16 * 3]
+	stp x8, x9, [sp ,16 * 4]
+	stp x10, x11, [sp ,16 * 5]
+	stp x12, x13, [sp ,16 * 6]
+	stp x14, x15, [sp ,16 * 7]
+	stp x16, x17, [sp ,16 * 8]
+	stp x18, x19, [sp ,16 * 9]
+	stp x20, x21, [sp ,16 * 10]
+	stp x22, x23, [sp ,16 * 11]
+	stp x24, x25, [sp ,16 * 12]
+	stp x26, x27, [sp ,16 * 13]
+	stp x28, x29, [sp ,16 * 14]
+	str x30, [sp, 16 * 15]
+.endm
+
+// load general registers from stack
+.macro load_all
+	ldp x0, x1, [sp ,16 * 0]
+	ldp x2, x3, [sp ,16 * 1]
+	ldp x4, x5, [sp ,16 * 2]
+	ldp x6, x7, [sp ,16 * 3]
+	ldp x8, x9, [sp ,16 * 4]
+	ldp x10, x11, [sp ,16 * 5]
+	ldp x12, x13, [sp ,16 * 6]
+	ldp x14, x15, [sp ,16 * 7]
+	ldp x16, x17, [sp ,16 * 8]
+	ldp x18, x19, [sp ,16 * 9]
+	ldp x20, x21, [sp ,16 * 10]
+	ldp x22, x23, [sp ,16 * 11]
+	ldp x24, x25, [sp ,16 * 12]
+	ldp x26, x27, [sp ,16 * 13]
+	ldp x28, x29, [sp ,16 * 14]
+	ldr x30, [sp, 16 * 15]
+	add sp, sp, 32 * 8
+.endm
+
+_null_handler:
+	save_all
+	bl not_implemented_interrupt
+	load_all
+	eret
+
+.align 11
+.global exception_vector_table
+exception_vector_table:
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7
+	b _null_handler
+	.align 7

kernel/include/utils.h

@@ -36,3 +36,15 @@ void exit(int);
   (ptr = (orig_type*)((bump_type*)ptr + 1)), \
   ((bump_type*)ptr - 1) \
 )
+
+#define R_SYSREG(ret, reg) \
+  asm volatile( \
+    "mrs %[a], " #reg \
+    : [a] "=r" (ret) \
+  )
+
+#define W_SYSREG(val, reg) \
+  asm volatile( \
+    "msr " #reg ", %[a]" \
+    : [a] "r" (val) \
+  )

kernel/main.c

@@ -6,6 +6,7 @@
 #include <mman.h>
 #include <shell.h>
 #include <vector.h>
+#include <utils.h>
 
 void init(void *dtb, file_node_t **initrd_root)
 {
@@ -34,6 +35,10 @@ void main(void *dtb)
   file_node_t *initrd_root = 0x0;
   init(dtb, &initrd_root);
 
+  uint64_t el;
+  R_SYSREG(el, CurrentEL);
+  INFOR(el);
+
   void *page1 = allocate_page(1);
   INFOR(page1);