Allow calls to scheduler in middle of execution

3 files changed, 47 insertions, 21 deletions

diff --git a/README.md b/README.md
index 91adff0..9d7f1bc 100644
--- a/README.md
+++ b/README.md
@@ -18,10 +18,16 @@
 - // MODE REGISTERS: https://developer.arm.com/documentation/ddi0406/c/System-Level-Architecture/The-System-Level-Programmers--Model/ARM-processor-modes-and-ARM-core-registers/ARM-core-registers?lang=en
 
 ## Todo
+- FIX IRQ Stack LEAK
+- FIX IRQ Stack LEAK
+- FIX IRQ Stack LEAK
+- FIX IRQ Stack LEAK
+- FIX IRQ Stack LEAK
 - Implement Scheduler for IRQ
 - Fix mutex with scheduler
 - Implement handlers
 - Put threads in user mode
+- Clear stacks during cleanup
 
 
 Note: SYS and USER share registers but differ in privilege
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:
diff --git a/src/sys/core.c b/src/sys/core.c
index aa10bdf..4a44e96 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -16,7 +16,8 @@
 #include <util/time.h>
 
 void testlocal(void);
-void longlocal(void);
+void testnew(void);
+void __attribute__((naked)) usr_schedule(void);
 
 // Initialize IRQs
 void sysinit(void)
@@ -69,7 +70,7 @@ void sysinit(void)
 	add_thread(testlocal, 0, 1);
 	add_thread(testlocal, 0, 3);
 	add_thread(testlocal, 0, 5);
-	add_thread(longlocal, 0, 5);
+	add_thread(testnew, 0, 4);
 	uart_scheduler();
 }
 
@@ -83,3 +84,16 @@ void testlocal(void)
 	uart_hexn((unsigned long)getsp());
 	uart_string("Exiting thread!\n");
 }
+
+void testnew(void)
+{
+	uart_string("Ran special\n");
+	add_thread(testlocal, 0, 0);
+	usr_schedule();
+	uart_string("Finish special!\n");
+}
+
+void __attribute__((naked)) usr_schedule(void)
+{
+	asm volatile ("svc #2");
+}