From a9e89946ad7f6918b954d7416c755a90c0eace9d Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Thu, 24 Feb 2022 18:38:20 -0700
Subject: Fixed HYP Code

---
 Common.mk        | 3 +++
 src/boot.S       | 7 +++++++
 src/sys/kernel.S | 2 +-
 3 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/Common.mk b/Common.mk
index 66e103c..b2664b0 100644
--- a/Common.mk
+++ b/Common.mk
@@ -108,3 +108,6 @@ tree:
 test: clean build/kernel.elf
 	@tput setaf 6 2> /dev/null || true; echo Running Tests; tput sgr0 2> /dev/null || true
 	@./tests/run.sh
+
+copy: clean build/kernel7.img
+	sudo cp build/kernel7.img /mnt/sd0
diff --git a/src/boot.S b/src/boot.S
index 6da500b..46ef3d0 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -9,13 +9,20 @@
 _start:
 reset:
 	cpsid aif
+
+	// Exit Hypervisor Mode
 	mrs r0, cpsr
+	and r1, r0, #0x1F
+	cmp r1, #0x1A
+	bne 1f
 	bic r0, r0, #0x1f
 	orr r0, r0, #0x13
 	msr spsr_cxsf, r0
 	add r0, pc, #4
 	msr ELR_hyp, r0
 	eret
+
+1:
 	// disable core0,1,2.
 	mrc p15, #0, r1, c0, c0, #5
 	and r1, r1, #3
diff --git a/src/sys/kernel.S b/src/sys/kernel.S
index cfd396f..654a9a7 100644
--- a/src/sys/kernel.S
+++ b/src/sys/kernel.S
@@ -10,7 +10,7 @@ kernel_main:
 	bl sysinit
 
 	// Intentional undefined instruction
-	//.word 0xf7f0a000
+	// .word 0xf7f0a000
 	cpsie aif, #0x10
 	svc #2 // Start scheduling!
 1:
-- 
cgit v1.2.1