5 files changed, 41 insertions, 13 deletions

diff --git a/include/globals.h b/include/globals.h
index b33ebc7..16845d5 100644
--- a/include/globals.h
+++ b/include/globals.h
@@ -4,6 +4,7 @@
 #include <sys/schedule.h>
 
 #ifndef GLOBALS_C
+extern unsigned long irqlr;
 extern unsigned long cntfrq;
 extern unsigned long cmdidx;
 extern char cmd[2048];
diff --git a/include/sys/schedule.h b/include/sys/schedule.h
index b153bdb..90e82dd 100644
--- a/include/sys/schedule.h
+++ b/include/sys/schedule.h
@@ -41,7 +41,6 @@ struct ThreadQueues {
 };
 
 struct Scheduler {
-	void* sp;
 	struct Thread* rthread;
 	struct ThreadQueues thread_queues[PRIORITIES];
 };
diff --git a/src/globals.S b/src/globals.S
index 68101d7..3a5da83 100644
--- a/src/globals.S
+++ b/src/globals.S
@@ -1,4 +1,7 @@
 .section ".bss"
+.globl irqlr
+irqlr:
+	.word 0
 .globl cntfrq
 cntfrq:
 	.word 0
diff --git a/src/sys/schedule.S b/src/sys/schedule.S
index 298646a..4913034 100644
--- a/src/sys/schedule.S
+++ b/src/sys/schedule.S
@@ -1,26 +1,49 @@
 .section ".text"
 .globl schedule
-
-// Implicit: SVC Mode
 .macro preserve_ctx
 	cps #0x1f // Sys mode
-	push {r0-r12, lr} // Restore usr regs
-	cps #0x10 // Svc mode
+	// Store Usr regs
+	stmfd sp!, {r0-r12}
+	stmfd sp!, {lr}
 	ldr r3, =scheduler // struct Scheduler
-	ldr r3, [r3, #4] // struct Thread* rthread
-	str lr, [r3, #0] // svc_lr -> void* pc
+	ldr r2, [r3, #0] // struct Thread* rthread
+	str lr, [r2, #0] // svc_lr -> void* pc
+	str sp, [r2, #4] // svc_lr -> void* sp
+	cps #0x10 // Svc mode
 .endm
 
 .macro restore_ctx
 	ldr r3, =scheduler // struct Scheduler
-	ldr r3, [r3, #4] // struct Thread* rthread
-	ldr lr, [r3, #0] // void* pc -> lr_svc
+	ldr r2, [r3, #0] // struct Thread* rthread
+	ldr lr, [r2, #0] // void* pc -> lr_svc
+	ldr r0, [r2, #4] // void* sp -> r0
 	cps #0x1f // Sys mode
-	pop {r0-r12,lr} // Restore usr regs
+	mov sp, r0 // Set stack pointer
+	// Restore Usr regs
+	ldmfd sp!, {lr}
+	ldmfd sp!, {r0-r12}
 	cps #0x10 // Svc mode
 .endm
 
+// Assumption: Enter in SVC mode
 schedule:
-	//preserve_ctx
-	//restore_ctx
-	//bx lr
+	preserve_ctx
+	ldr r1, =irqlr
+	ldr r0, [r1]
+	cmp r0, #0
+	beq 1f
+	// Replace LR with IRQ's LR
+	mov lr, r0
+	// Clear IRQ's LR
+	mov r0, #0
+	str r0, [r1]
+1:
+	// bl get_next_thread // next_thread -> r0
+	ldr r3, =scheduler
+	ldr r2, [r3, #0] // struct Thread* current
+	cmp r0, r2 // current = next?
+	beq 2f
+	str r0, [r3, #0] // next -> rthread
+2:
+	restore_ctx
+	movs pc, lr
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index 96dc678..0dc89e9 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -1,3 +1,5 @@
 void init_scheduler(void)
 {
+	// Set rthread to usrloopthread - an infinitely running thread so that the pointer will never be null
+	// Initialize Rotating Buffers
 }