3 files changed, 135 insertions, 6 deletions

diff --git a/src/sys/core.c b/src/sys/core.c
index 1329c12..fc3d9e1 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -23,7 +23,6 @@ void sysinit(void)
 	// Clear System Globals
 	exe_cnt_m.addr = &exe_cnt;
 	exe_cnt_m.pid = NULL_PID;
-	nextpid = SCHED_PID + 1;
 	rpi_heap_top = &rpi_heap;
 	stimeh = *(unsigned long*)SYS_TIMER_CHI;
 	stimel = *(unsigned long*)SYS_TIMER_CLO;
@@ -64,9 +63,12 @@ void sysinit(void)
 	enableirq();
 	enablefiq();
 
-	//add_thread(testlocal, 0);
-	//add_thread(testlocal, 1);
-	//add_thread(testlocal, 3);
+	add_thread(testlocal, 0, 0);
+	add_thread(testlocal, 0, 1);
+	add_thread(testlocal, 0, 1);
+	add_thread(testlocal, 0, 3);
+	add_thread(testlocal, 0, 5);
+	uart_scheduler();
 }
 
 void testlocal1(void)
diff --git a/src/sys/schedule.S b/src/sys/schedule.S
index 4913034..f80b707 100644
--- a/src/sys/schedule.S
+++ b/src/sys/schedule.S
@@ -1,3 +1,6 @@
+// TODO: - Scheduler info function
+//       - Get next thread
+//       - Yield supervisor call
 .section ".text"
 .globl schedule
 .macro preserve_ctx
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index 3aca0ff..e20abe7 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -1,18 +1,55 @@
 #include <globals.h>
+#include <drivers/uart.h>
 #include <sys/schedule.h>
+#include <util/mutex.h>
+
+void loop(void);
+void cleanup(void);
 
 void init_scheduler(void)
 {
 	// Set rthread to usrloopthread - an infinitely running thread so that the pointer will never be null
 	usrloopthread.pc = (void*)loop;
-	usrloopthread.sp = 0x0FCC;
-	usrloopthread.sp_base = 0x1000;
+	usrloopthread.sp = (void*)0x5FC8;
+	*(unsigned long**)usrloopthread.sp = (unsigned long*)loop;
+	usrloopthread.sp_base = (void*)0x6000;
 	usrloopthread.mptr = 0;
 	usrloopthread.pid = -1;
 	usrloopthread.priority = -1;
 	usrloopthread.status = THREAD_READY;
 	scheduler.rthread = &usrloopthread;
 	// Initialize Rotating Buffers
+	struct ThreadQueues* tq;
+	for (int i = 0; i < PRIORITIES; i++) {
+		tq = &scheduler.thread_queues[i];
+		struct ThreadRotBuffer* trb = &tq->ready;
+		trb->roffset = 0;
+		trb->woffset = 0;
+		for (int j = 0; j < TQUEUE_MAX; j++)
+			trb->queue[j] = 0;
+		trb = &tq->mwait;
+		trb->roffset = 0;
+		trb->woffset = 0;
+		for (int j = 0; j < TQUEUE_MAX; j++)
+			trb->queue[j] = 0;
+		trb = &tq->swait;
+		trb->roffset = 0;
+		trb->woffset = 0;
+		for (int j = 0; j < TQUEUE_MAX; j++)
+			trb->queue[j] = 0;
+		trb = &tq->finish;
+		trb->roffset = 0;
+		trb->woffset = 0;
+		for (int j = 0; j < TQUEUE_MAX; j++)
+			trb->queue[j] = 0;
+		trb = &tq->serror;
+		trb->roffset = 0;
+		trb->woffset = 0;
+		for (int j = 0; j < TQUEUE_MAX; j++)
+			trb->queue[j] = 0;
+	}
+	// Initialize nextpid
+	nextpid = SCHED_PID + 1;
 }
 
 void loop(void)
@@ -20,3 +57,90 @@ void loop(void)
 	while(1)
 		asm volatile ("wfe");
 }
+
+void* get_stack(void)
+{
+	for (int i = 0; i < MAX_THREADS; i++) {
+		if (stacks_table[i] == 0) {
+			stacks_table[i] = 1;
+			return (void*)0x20000000 - STACK_SIZE*i;
+		}
+	}
+	return 0;
+}
+
+void add_thread(void* pc, void* arg, unsigned char priority)
+{
+	void* sp = get_stack();
+	struct Thread* thread = (struct Thread*)malloca(sizeof(struct Thread), 4);
+	thread->pc = pc;
+	if (sp) {
+		thread->sp_base = sp;
+		unsigned long* argp = sp;
+		argp -= 1;
+		*argp = (unsigned long)arg; // Set r0 to the argument
+		argp -= 13;
+		*(unsigned long**)argp = (unsigned long*)cleanup; // Set lr to the cleanup function
+		thread->sp = (void*)argp;
+		thread->status = THREAD_READY;
+	} else {
+		thread->sp_base = 0;
+		thread->sp = 0;
+		thread->status = THREAD_SERROR;
+	}
+	thread->mptr = (void*)0;
+	thread->pid = nextpid++;
+	thread->priority = priority % PRIORITIES;
+	thread->preempt = 0;
+	// Add Thread* to scheduler's appropriate buffer
+	struct ThreadQueues* tq = &scheduler.thread_queues[thread->priority];
+	struct ThreadRotBuffer* trb;
+	if (thread->status == THREAD_SERROR) {
+		trb = &tq->serror;
+	} else {
+		trb = &tq->ready;
+	}
+	trb->queue[trb->woffset++] = thread;
+	trb->woffset %= TQUEUE_MAX;
+}
+
+void uart_scheduler(void)
+{
+	uart_string("Scheduler Info\n==============\nCurrent\n");
+	uart_hex((unsigned long)scheduler.rthread);
+	uart_char(' ');
+	memshow32((void*)scheduler.rthread, 6);
+	struct ThreadQueues* tq;
+	for(int p = 0; p < PRIORITIES; p++) {
+		uart_string("Priority ");
+		uart_10(p);
+		uart_char('\n');
+		tq = &scheduler.thread_queues[p];
+		struct ThreadRotBuffer* trb;
+		trb = &tq->ready;
+		for(int i = 0; i < TQUEUE_CNT; i++) {
+			if (trb->roffset == trb->woffset) {
+				trb += 1;
+				continue;
+			}
+			uart_string("Queue ");
+			uart_10(i);
+			uart_char('\n');
+			unsigned long roffset = trb->roffset;
+			while (roffset != trb->woffset) {
+				uart_hex((unsigned long)&trb->queue[roffset]);
+				uart_char(' ');
+				memshow32((void*)trb->queue[roffset], 6);
+				memshow32((void*)trb->queue[roffset]->sp, 14);
+				roffset++;
+				roffset %= TQUEUE_MAX;
+			}
+			trb += 1;
+		}
+	}
+	uart_string("==============\n");
+}
+
+void cleanup(void)
+{
+}