5 files changed, 75 insertions, 46 deletions
diff --git a/src/sys/core.c b/src/sys/core.c
index e590e5c..b492548 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -50,19 +50,16 @@ void sysinit(void)
 	lfb_init();
 	lfb_showpicture();
 
+	// Start Scheduler
+	init_scheduler();
+
 	// Enable IRQ & FIQ
 	enableirq();
 	enablefiq();
 
-	// Start Scheduler
-	init_scheduler();
 	add_thread(testlocal, 0);
 	add_thread(testlocal, 1);
 	add_thread(testlocal, 3);
-	//delay(0x20000000);
-	schedule();
-	heap_info();
-	sched_info();
 }
 
 struct Mutex testm = {.addr = (void*)0xDEADBEEF, .pid = NULL_PID};
@@ -95,5 +92,23 @@ void testlocal(void)
 		add_thread(testlocal1, 1);
 		schedule();
 	}
-	uart_string("Done!\n");
+	if (t->data.pid == 3) {
+		yield();
+		yield();
+		yield();
+		yield();
+		yield();
+		yield();
+		yield();
+		// Example
+		/*
+			while (uart_tx_full) {
+				t->data.status = THREAD_WAITING;
+				schedule();
+			} // Will wait until uart_tx is not full
+		*/
+	}
+	uart_string("Done! ");
+	uart_10(t->data.pid);
+	uart_char('\n');
 }
diff --git a/src/sys/kernel.S b/src/sys/kernel.S
index e0333ca..cee385d 100644
--- a/src/sys/kernel.S
+++ b/src/sys/kernel.S
@@ -4,6 +4,7 @@
 kernel_main:
 	bl sysinit
 kernel_main.loop:
+	//bl schedule
 	wfe
 	b kernel_main.loop
 
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
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index ac5b3e2..1b02476 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -15,6 +15,7 @@ struct Scheduler scheduler = {
 		{.prev = 0, .next = 0, .data = 0},
 	},
 	.rthread_ll = 0,
+	.ctx = 0,
 };
 unsigned long syssp = 0;
 struct cpu_context syscpu = {
@@ -183,4 +184,6 @@ void sched_info(void)
 		}
 		uart_char('\n');
 	}
+	uart_string("Stacks:\n");
+	memshow32((unsigned long*)stacks_table, 6);
 }
diff --git a/src/sys/timer.c b/src/sys/timer.c
index 5a3b8d4..d35ef83 100644
--- a/src/sys/timer.c
+++ b/src/sys/timer.c
@@ -9,7 +9,7 @@
 #include <symbols.h>
 
 /// Cycles Per Second
-#define CPS 10
+#define CPS 100
 
 #define SYS_TIMER_C
 static unsigned long exe_cnt = 0;