Update stuff

9 files changed, 93 insertions, 51 deletions

diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index 0d6e9de..aa7c375 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -4,6 +4,7 @@
 #include <symbols.h>
 #include <sys/core.h>
 #include <sys/kernel.h>
+#include <sys/schedule.h>
 #include <sys/timer.h>
 #include <util/mutex.h>
 #include <util/status.h>
@@ -105,4 +106,12 @@ void c_irq_handler(void)
 
 void localtest(void)
 {
+	struct Thread* t = scheduler.rthread_ll->data;
+	uart_string("Running IRQ Task... ");
+	uart_10(t->data.pid);
+	uart_char('\n');
+	uart_string("Finished! ");
+	uart_10(t->data.pid);
+	uart_char('\n');
+	sched_info();
 }
diff --git a/src/drivers/uart.S b/src/drivers/uart.S
index e476799..cde6c11 100644
--- a/src/drivers/uart.S
+++ b/src/drivers/uart.S
@@ -49,5 +49,5 @@ uart_hex.hloop:
 uart_hex.print:
 	str r1, [r2]
 	subs r3, #1
-	bge uart_hex.hloop
+	bge uart_hex.loop // Jump back to wait for availablilty
 	pop {r4, pc}
diff --git a/src/graphics/lfb.c b/src/graphics/lfb.c
index 39d921d..3ad9917 100644
--- a/src/graphics/lfb.c
+++ b/src/graphics/lfb.c
@@ -8,6 +8,9 @@
 unsigned int width, height, pitch, isrgb;   /* dimensions and channel order */
 unsigned char *lfb;                         /* raw frame buffer address */
 
+#define SCR_WIDTH  1024
+#define SCR_HEIGHT 768
+
 /**
  * Set screen resolution to 1024x768
  */
@@ -19,14 +22,14 @@ void lfb_init(void)
 	mbox[2] = 0x48003;  //set phy wh
 	mbox[3] = 8;
 	mbox[4] = 8;
-	mbox[5] = 1024;         //FrameBufferInfo.width
-	mbox[6] = 768;          //FrameBufferInfo.height
+	mbox[5] = SCR_WIDTH;         //FrameBufferInfo.width
+	mbox[6] = SCR_HEIGHT;          //FrameBufferInfo.height
 
 	mbox[7] = 0x48004;  //set virt wh
 	mbox[8] = 8;
 	mbox[9] = 8;
-	mbox[10] = 1024;        //FrameBufferInfo.virtual_width
-	mbox[11] = 768;         //FrameBufferInfo.virtual_height
+	mbox[10] = SCR_WIDTH;        //FrameBufferInfo.virtual_width
+	mbox[11] = SCR_HEIGHT;         //FrameBufferInfo.virtual_height
 
 	mbox[12] = 0x48009; //set virt offset
 	mbox[13] = 8;
diff --git a/src/lib/mem.c b/src/lib/mem.c
index f8a8cc5..ed6d23f 100644
--- a/src/lib/mem.c
+++ b/src/lib/mem.c
@@ -47,6 +47,7 @@ unsigned char memcmp32(unsigned long* a, unsigned long* b, unsigned int n)
 static unsigned char rpi_heap[MAX_MM] = {0,};
 static void* rpi_heap_top = &rpi_heap;
 
+// TODO: Put size at end and cleanup from tail
 void* malloc(unsigned char size)
 {
 	unsigned char* mem = (unsigned char*)rpi_heap;
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;