From 6156975775384508dca8300585d97ca35bee4b5a Mon Sep 17 00:00:00 2001
From: Yi-Ting Shih <ytshih@it.cs.nycu.edu.tw>
Date: Sun, 23 Mar 2025 11:21:31 +0800
Subject: [PATCH] Draft: start lab 3

---
 include/logger.h           |  7 ++-
 kernel/Start.S             | 11 +++++
 kernel/exception_handler.S | 88 ++++++++++++++++++++++++++++++++++++++
 kernel/include/utils.h     | 12 ++++++
 kernel/main.c              |  5 +++
 5 files changed, 122 insertions(+), 1 deletion(-)
 create mode 100644 kernel/exception_handler.S

diff --git a/include/logger.h b/include/logger.h
index 1c720bf..eaa15ce 100644
--- a/include/logger.h
+++ b/include/logger.h
@@ -60,6 +60,10 @@ logger_cur = _Generic((msg), \
   logger_cur = logger_buf; \
 }
 
+#define CLEAN { \
+  logger_cur = logger_buf; \
+}
+
 #if LOGLEVEL >= 0
 #define ERROR(val) { \
   LOG(val); \
@@ -84,7 +88,8 @@ logger_cur = _Generic((msg), \
   FLUSH("[DEBUG]: "); \
 }
 #define DEBUG_DTB(val) DEBUG(val)
-#define DEBUG_MEM(val) DEBUG(val)
+// #define DEBUG_MEM(val) DEBUG(val)
+#define DEBUG_MEM(val) CLEAN
 #else // #if LOGLEVEL >= 2
 #define DEBUG(val)
 #define DEBUG_DTB(val)
diff --git a/kernel/Start.S b/kernel/Start.S
index 21d8f65..fafdd77 100644
--- a/kernel/Start.S
+++ b/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
diff --git a/kernel/exception_handler.S b/kernel/exception_handler.S
new file mode 100644
index 0000000..5b87f70
--- /dev/null
+++ b/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
diff --git a/kernel/include/utils.h b/kernel/include/utils.h
index fefc56f..8b975f9 100644
--- a/kernel/include/utils.h
+++ b/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) \
+  )
diff --git a/kernel/main.c b/kernel/main.c
index 79720e8..705b973 100644
--- a/kernel/main.c
+++ b/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);