Exit Hypervisor Mode

2 files changed, 8 insertions, 33 deletions

diff --git a/src/boot.S b/src/boot.S
index 9d431d7..6da500b 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -9,6 +9,13 @@
 _start:
 reset:
 	cpsid aif
+	mrs r0, cpsr
+	bic r0, r0, #0x1f
+	orr r0, r0, #0x13
+	msr spsr_cxsf, r0
+	add r0, pc, #4
+	msr ELR_hyp, r0
+	eret
 	// disable core0,1,2.
 	mrc p15, #0, r1, c0, c0, #5
 	and r1, r1, #3
@@ -54,15 +61,12 @@ reset:
 
 runcore1:
 	init_core 1
-	core_task 1
 	b io_halt
 runcore2:
 	init_core 2
-	core_task 2
 	b io_halt
 runcore3:
 	init_core 3
-	core_task 3
 	b io_halt
 
 .globl io_halt
diff --git a/src/sys/kernel.S b/src/sys/kernel.S
index 6452a4f..cfd396f 100644
--- a/src/sys/kernel.S
+++ b/src/sys/kernel.S
@@ -9,39 +9,10 @@ kernel_main:
 	mrc p15, 0, r0, c0, c1, 4
 	bl sysinit
 
-	assign_ctask testf, 2
-	assign_ctask testf, 1
-	assign_ctask testf, 2
-	assign_ctask testf, 3
-	assign_ctask testf, 1
 	// Intentional undefined instruction
-	// .word 0xf7f0a000
+	//.word 0xf7f0a000
 	cpsie aif, #0x10
 	svc #2 // Start scheduling!
 1:
 	wfe
 	b 1b
-
-testf:
-	push {lr}
-	cmp r0, #2
-	blo 1f
-	beq 2f
-	bgt 3f
-1:
-	ldr r0, =t1
-	bl uart_print
-	pop {pc}
-2:
-	ldr r0, =t2
-	bl uart_print
-	pop {pc}
-3:
-	ldr r0, =t3
-	bl uart_print
-	pop {pc}
-
-.section .data
-t1: .asciz "C1\n"
-t2: .asciz "C2\n"
-t3: .asciz "C3\n"