1 files changed, 48 insertions, 38 deletions
diff --git a/src/sys/schedule.S b/src/sys/schedule.S
index 302c4d8..aa33942 100644
--- a/src/sys/schedule.S
+++ b/src/sys/schedule.S
@@ -1,32 +1,52 @@
-.section .text
+.section ".text"
 .globl schedule
 // TODO: Implement Scheduler for IRQ
 
+
+// Save Context
+// reg = struct cpu_context*
+.macro save_context reg
+	str r4,  [\reg, #0x00]
+	str r5,  [\reg, #0x04]
+	str r6,  [\reg, #0x08]
+	str r7,  [\reg, #0x0c]
+	str r8,  [\reg, #0x10]
+	str r9,  [\reg, #0x14]
+	str r10, [\reg, #0x18]
+	str r11, [\reg, #0x1c]
+	str r12, [\reg, #0x20]
+	str lr,  [\reg, #0x24]
+.endm
+// Restore Context
+// reg = struct cpu_context*
+.macro restore_context reg
+	ldr r4, [\reg, #0x00]
+	ldr r5, [\reg, #0x04]
+	ldr r6, [\reg, #0x08]
+	ldr r7, [\reg, #0x0c]
+	ldr r8, [\reg, #0x10]
+	ldr r9, [\reg, #0x14]
+	ldr r10, [\reg, #0x18]
+	ldr r11, [\reg, #0x1c]
+	ldr r12, [\reg, #0x20]
+	ldr lr , [\reg, #0x24]
+.endm
+
 // Implemented the scheduler in Assembly since the C defined was messing around with the program stacks
 //  This way, I can be confident that the stacks will be unchanged
 schedule:
 	ldr r3, =scheduler
-	// r3 = struct Scheduler*
 	// Preserve context
-	//add r0, r3, #4 // struct cpu_context* ctx
 	ldr r0, [r3, #4]
 	// r0 = struct cpu_context*
-	str r4,  [r0, #0x00]
-	str r5,  [r0, #0x04]
-	str r6,  [r0, #0x08]
-	str r7,  [r0, #0x0c]
-	str r8,  [r0, #0x10]
-	str r9,  [r0, #0x14]
-	str r10, [r0, #0x18]
-	str r11, [r0, #0x1c]
-	str r12, [r0, #0x20]
-	str lr,  [r0, #0x24]
+	save_context r0
 	// Get the next available thread
-	push {r3, lr}
+	push {r1-r3, lr}
 	bl get_next_thread
-	// r0 = struct LL* next_thread_ll
-	pop {r3, lr}
+	pop {r1-r3, lr}
 	ldr r1,  [r3, #0] 
+	// r3 = struct Scheduler*
+	// r0 = struct LL* next_thread_ll
 	// r1 = struct LL* current_thread_ll
 	// Check if there is a valid currently running thread
 	cmp r1, #0
@@ -34,11 +54,20 @@ schedule:
 schedule.current_thread_exists:
 	cmp r0, r1
 	beq schedule.run_current
+	cmp r0, #0
+	moveq r0, r1 // Make the current running thread the next running thread if no next running thread
 	// Next is not the same as the current
 	// Preserve stack of current
 	ldr r2,  [r1, #0x8] // struct Thread* current
+	ldrh r1, [r2, #0x0e]
+	cmp r1, #2 // THREAD_WAITING
+	beq schedule.temp_status
+	cmp r1, #1 // THREAD_RUNNING
+	bne schedule.dont_modify_status
+schedule.temp_status:
 	mov r1, #0 // THREAD_READY
 	strh r1, [r2, #0x0e]
+schedule.dont_modify_status:
 	str sp,  [r2, #0x4] // void* stack // Preserve stack
 	// Preserve program counter of current
 	str lr,  [r2, #0x0] // void* thread // Preserve pc
@@ -84,16 +113,7 @@ schedule.dont_overwrite_sys_stack:
 schedule.run_current:
 	// Restore context
 	ldr r2,  [r3, #0x4] // struct cpu_context* ctx // Set new context
-	ldr r4,  [r2, #0x00]
-	ldr r5,  [r2, #0x04]
-	ldr r6,  [r2, #0x08]
-	ldr r7,  [r2, #0x0c]
-	ldr r8,  [r2, #0x10]
-	ldr r9,  [r2, #0x14]
-	ldr r10, [r2, #0x18]
-	ldr r11, [r2, #0x1c]
-	ldr r12, [r2, #0x20]
-	ldr lr,  [r2, #0x24]
+	restore_context r2
 	// Run
 	ldr r1, [r3, #0]
 	// r1 = struct LL* rthread_ll
@@ -117,17 +137,7 @@ schedule.no_next_thread:
 	mov r1, #0
 	str r1, [r0] // Clear stack pointer
 schedule.exit:
-	// Restore context
-	ldr r2,  [r3, #0x4] // struct cpu_context* ctx // Set new context
 	// Restore register context
-	ldr r4,  [r2, #0x00]
-	ldr r5,  [r2, #0x04]
-	ldr r6,  [r2, #0x08]
-	ldr r7,  [r2, #0x0c]
-	ldr r8,  [r2, #0x10]
-	ldr r9,  [r2, #0x14]
-	ldr r10, [r2, #0x18]
-	ldr r11, [r2, #0x1c]
-	ldr r12, [r2, #0x20]
-	ldr lr,  [r2, #0x24]
+	ldr r2,  [r3, #0x4] // struct cpu_context* ctx // Set new context
+	restore_context r2
 	bx lr