From 0d551d712c098d6ebb75512340d7503da98361b4 Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Fri, 21 Jan 2022 18:52:25 -0700
Subject: Got some scheduling

---
 src/boot.S | 39 ++++++++++++++++++++++++++++-----------
 1 file changed, 28 insertions(+), 11 deletions(-)

(limited to 'src/boot.S')

diff --git a/src/boot.S b/src/boot.S
index 42e991a..27f6ddc 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -120,28 +120,34 @@ svc:
 	stmfd sp!, {r0-r12,lr}
 	ldr r0, [lr, #-4]
 	bic r0, #0xFF000000
-	push {r0}
-	ldr r0, =svc_msg
-	bl uart_string
-	ldr r0, [sp]
-	bl uart_hexn
-	pop {r0}
+	/// push {r0}
+	/// ldr r0, =svc_msg
+	/// bl uart_string
+	/// ldr r0, [sp]
+	/// bl uart_hexn
+	/// pop {r0}
 	// SVC #0 returns to supervisor mode
 	// TODO: Make supervisor mode return to a specific location
 	//  (rather than to a user location) such as the kernel loop
 	cmp r0, #0
+	bne 2f
+	cps #0x13
+	b   1f
+2:
+	cmp r0, #2
 	bne 1f
-	mrs r0, spsr
-	bic r0, #0x1f
-	// Return in supervisor mode
-	orr r0, #0x13
-	msr spsr, r0
+	ldmfd sp!, {r0-r12,lr}
+	bl schedule
 1:
 	ldmfd sp!, {r0-r12,pc}^
 io_halt_prefetch:
 	stmfd sp!, {r0-r12,lr}
+	push {lr}
 	ldr r0, =prefetch_msg
 	bl uart_string
+	pop {lr}
+	mov r0, lr
+	bl uart_hexn
 	ldmfd sp!, {r0-r12,pc}^
 io_halt_data:
 	stmfd sp!, {r0-r12,lr}
@@ -158,6 +164,17 @@ fiq:
 	//ldr r0, =fiq_msg
 	//bl uart_string
 	bl c_fiq_handler
+	cmp r0, #1
+	bne 1f
+	ldmfd sp!, {r0-r12,lr}
+	sub lr, #4
+	stmfd sp!, {r0-r12,lr}
+	ldr r3, =irqlr
+	str lr, [r3, #0]
+	ldmfd sp!, {r0-r12,lr}
+	cps #0x13
+	b schedule
+1:
 	ldmfd sp!, {r0-r12,lr}
 	subs pc, lr, #4
 
-- 
cgit v1.2.1