From 523e704b4fead2a0b286480d27510e3912d54b79 Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Sat, 22 Jan 2022 22:06:42 -0700
Subject: Allow calls to scheduler in middle of execution

---
 src/boot.S | 44 +++++++++++++++++++++++++-------------------
 1 file changed, 25 insertions(+), 19 deletions(-)

(limited to 'src/boot.S')

diff --git a/src/boot.S b/src/boot.S
index 27f6ddc..2106031 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -25,38 +25,44 @@ reset:
 	mrs r0, cpsr
 
 	// setup sp in IRQ mode.
-	bic r1, r0, #0x1f
-	orr r1, r1, #0x12
-	msr cpsr_c,r1
+	///bic r1, r0, #0x1f
+	///orr r1, r1, #0x12
+	///msr cpsr_c,r1
+	cps #0x12
 	mov sp,#0x4000
 	// setup sp in FIQ mode.
-	bic r1, r0, #0x1f
-	orr r1, r1, #0x11
-	msr cpsr_c,r1
+	///bic r1, r0, #0x1f
+	///orr r1, r1, #0x11
+	///msr cpsr_c,r1
+	cps #0x11
 	mov sp,#0x2000
 	// setup sp in UNDEF mode.
-	bic r1, r0, #0x1f
-	orr r1, r1, #0x1B
-	msr cpsr_c,r1
+	///bic r1, r0, #0x1f
+	///orr r1, r1, #0x1B
+	///msr cpsr_c,r1
+	cps #0x1B
 	mov sp,#0x1000
 	// setup sp in ABORT mode.
-	bic r1, r0, #0x1f
-	orr r1, r1, #0x17
-	msr cpsr_c,r1
+	///bic r1, r0, #0x1f
+	///orr r1, r1, #0x17
+	///msr cpsr_c,r1
+	cps #0x17
 	mov sp,#0x0800
 	// Setup sp in USR/SYS mode.
-	bic r1, r0, #0x1f
-	orr r1, r1, #0x1f
-	msr cpsr_c,r1
+	///bic r1, r0, #0x1f
+	///orr r1, r1, #0x1f
+	///msr cpsr_c,r1
+	cps #0x1f
 	mov sp,#0x6000
 
 	// restore to SVC (in the case of a reset)
-	bic r1, r0, #0x1f
-	orr r1, r1, #0x13
-	msr cpsr_c, r1
+	///bic r1, r0, #0x1f
+	///orr r1, r1, #0x13
+	///msr cpsr_c, r1
 	//msr cpsr_c, r0
 
 	// setup the stack in SVC mode.
+	cps #0x13
 	mov sp, #0x8000
  
 	// Clear out bss.
@@ -137,7 +143,7 @@ svc:
 	cmp r0, #2
 	bne 1f
 	ldmfd sp!, {r0-r12,lr}
-	bl schedule
+	b schedule
 1:
 	ldmfd sp!, {r0-r12,pc}^
 io_halt_prefetch:
-- 
cgit v1.2.1