From e0ba2e06ace89f0e17dd912e684f91c5a7f28f7c Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Sat, 5 Feb 2022 12:40:47 -0700
Subject: Fixed SPSR preservation

---
 src/sys/schedule.S |  6 +++---
 src/tests/test.S   | 10 ++++++++++
 src/tests/test.c   |  6 ++++++
 3 files changed, 19 insertions(+), 3 deletions(-)
 create mode 100644 src/tests/test.S

diff --git a/src/sys/schedule.S b/src/sys/schedule.S
index e8ab326..3c49494 100644
--- a/src/sys/schedule.S
+++ b/src/sys/schedule.S
@@ -8,9 +8,9 @@
 	ldr r3, =scheduler // struct Scheduler
 	ldr r2, [r3, #0] // struct Thread* rthread
 	str sp, [r2, #4] // svc_lr -> void* sp
-	mrs r1, cpsr
-	str r1, [r2, #0xc] // preserve cpsr
 	cps #0x13 // Svc mode
+	mrs r1, spsr
+	str r1, [r2, #0xc] // preserve cpsr
 	str lr, [r2, #0] // svc_lr -> void* pc
 .endm
 
@@ -20,8 +20,8 @@
 	ldr lr, [r2, #0] // void* pc -> lr_svc
 	ldr r0, [r2, #4] // void* sp -> r0
 	ldr r1, [r2, #0xc] // restore cpsr
+	msr spsr_f, r1
 	cps #0x1f // Sys mode
-	msr cpsr, r1
 	mov sp, r0 // Set stack pointer
 	// Restore Usr regs
 	pop {lr}
diff --git a/src/tests/test.S b/src/tests/test.S
new file mode 100644
index 0000000..ebb901c
--- /dev/null
+++ b/src/tests/test.S
@@ -0,0 +1,10 @@
+.section .text
+
+btest:
+	push {lr}
+	pop {pc}
+
+.globl atest
+atest:
+	push {lr}
+	pop {pc}
diff --git a/src/tests/test.c b/src/tests/test.c
index f2f3e6b..3903d95 100644
--- a/src/tests/test.c
+++ b/src/tests/test.c
@@ -3,6 +3,9 @@
 #include <graphics/lfb.h>
 #include <lib/kmem.h>
 #include <sys/core.h>
+#include <sys/schedule.h>
+
+extern void atest(void);
 
 void test_entry(void)
 {
@@ -15,4 +18,7 @@ void test_entry(void)
 	}
 	DRAW64(0, 19, dt/64);
 	DRAW64(17, 19, dt%64);
+
+	// atest
+	add_thread(atest, 0, 0);
 }
-- 
cgit v1.2.1