From 47df8864aaa7e8e0b01d864d98e1f7b972928060 Mon Sep 17 00:00:00 2001
From: Mathieu Maret <mmaret@prophesee.ai>
Date: Sat, 12 Mar 2022 23:31:26 +0200
Subject: [PATCH] Set Excution Level to 1

---
 asm_utils.S |  6 ++++
 crt0.S      | 95 ++++++++++++++++++++++++++++++++++++++++++++++++++++-
 hello.c     |  3 ++
 utils.h     |  3 ++
 4 files changed, 106 insertions(+), 1 deletion(-)
 create mode 100644 asm_utils.S

diff --git a/asm_utils.S b/asm_utils.S
new file mode 100644
index 0000000..9f67593
--- /dev/null
+++ b/asm_utils.S
@@ -0,0 +1,6 @@
+.global get_el
+
+get_el:
+	mrs x0, CurrentEL
+        lsr x0, x0, #2 // Shift Right by 2
+	ret
diff --git a/crt0.S b/crt0.S
index 90ec9b4..3a66e8c 100644
--- a/crt0.S
+++ b/crt0.S
@@ -2,14 +2,107 @@
 	.global __start
 	.type __start, %function
 
+// From https://developer.arm.com/docs/ddi0487/ca/arm-architecture-reference-manual-armv8-for-armv8-a-architecture-profile
+// ***************************************
+// SCTLR_EL1, System Control Register (EL1), Page 2654 of AArch64-Reference-Manual.
+// ***************************************
+
+#define SCTLR_RESERVED                  (3 << 28) | (3 << 22) | (1 << 20) | (1 << 11)
+#define SCTLR_EE_LITTLE_ENDIAN          (0 << 25)
+#define SCTLR_EOE_LITTLE_ENDIAN         (0 << 24)
+#define SCTLR_I_CACHE_DISABLED          (0 << 12)
+#define SCTLR_D_CACHE_DISABLED          (0 << 2)
+#define SCTLR_MMU_DISABLED              (0 << 0)
+#define SCTLR_MMU_ENABLED               (1 << 0)
+
+#define SCTLR_VALUE_MMU_DISABLED	(SCTLR_RESERVED | SCTLR_EE_LITTLE_ENDIAN | SCTLR_I_CACHE_DISABLED | SCTLR_D_CACHE_DISABLED | SCTLR_MMU_DISABLED)
+
+// ***************************************
+// HCR_EL2, Hypervisor Configuration Register (EL2), Page 2487 of AArch64-Reference-Manual.
+// ***************************************
+
+#define HCR_RW	    			(1 << 31)
+#define HCR_VALUE			HCR_RW
+
+// ***************************************
+// SCR_EL3, Secure Configuration Register (EL3), Page 2648 of AArch64-Reference-Manual.
+// ***************************************
+
+#define SCR_RESERVED	    		(3 << 4)
+#define SCR_RW				(1 << 10)
+#define SCR_NS				(1 << 0)
+#define SCR_VALUE	    	    	(SCR_RESERVED | SCR_RW | SCR_NS)
+
+// ***************************************
+// SPSR_EL3, Saved Program Status Register (EL3) Page 389 of AArch64-Reference-Manual.
+// ***************************************
+
+#define SPSR_MASK_ALL 			(7 << 6)
+#define SPSR_EL1h			(5 << 0)
+#define SPSR_VALUE			(SPSR_MASK_ALL | SPSR_EL1h)
+
+#define SIMD_ENABLE			(3 << 20)
+
 __start:
 	//Use core 0 only
 	mrs	x7, mpidr_el1
 	and		x7, x7, #3
-	cbz		x7, __start_master
+	cbz		x7, __switch_level
 0: wfe
 	b			0b
 
+__switch_level:
+	mrs		x0, CurrentEL
+        lsr		x0, x0, #2 // Shift Right by 2
+	and		x0, x0, #3
+	cmp		x0, #3
+	beq		__switch_l3_l1
+	b __switch_l2_l1
+
+__switch_l3_l1:
+    ldr    x0, =SCTLR_VALUE_MMU_DISABLED
+    msr    sctlr_el1, x0
+
+    ldr    x0, =HCR_VALUE
+    msr    hcr_el2, x0
+
+    ldr    x0, =SCR_VALUE
+    msr    scr_el3, x0
+
+    ldr    x0, =SPSR_VALUE
+    msr    spsr_el3, x0
+
+    mrs    x0, CPACR_EL1
+    and	   x0, x0, #SIMD_ENABLE
+    msr	   CPACR_EL1, x0
+
+    adr    x0, __start_master
+    msr    elr_el3, x0
+
+    eret
+
+
+__switch_l2_l1:
+    ldr    x0, =SCTLR_VALUE_MMU_DISABLED
+    msr    sctlr_el1, x0
+
+    ldr    x0, =HCR_VALUE
+    msr    hcr_el2, x0
+
+    ldr    x0, =SPSR_VALUE
+    msr    spsr_el2, x0
+
+    mrs    x0, CPACR_EL1
+    and	   x0, x0, #SIMD_ENABLE
+    msr	   CPACR_EL1, x0
+
+
+    adr    x0, __start_master
+    msr    elr_el2, x0
+
+
+    eret
+
 __start_master:
 	//Setup stack pointer
 	ldr		x2, = __stack_start
diff --git a/hello.c b/hello.c
index dad755f..efa6fc8 100644
--- a/hello.c
+++ b/hello.c
@@ -49,6 +49,9 @@ int kernelmain(void)
 
   printclock();
 
+  int el = get_el();
+  puts("Exception leve: ");
+  printdec(el);
   fb  = fb_get();
   ptr = fb->fbp;
 
diff --git a/utils.h b/utils.h
index c3abbaa..f9250b8 100644
--- a/utils.h
+++ b/utils.h
@@ -8,3 +8,6 @@ void printdec(unsigned int d);
 void printclock();
 int strcmp(const char s1[], const char s2[]);
 int readline(char *buf, int maxlen);
+
+/* From asm_utils.S*/
+int get_el(void);