Added scheduling info output

7 files changed, 143 insertions, 17 deletions

diff --git a/include/sys/schedule.h b/include/sys/schedule.h
index c110533..4677aec 100644
--- a/include/sys/schedule.h
+++ b/include/sys/schedule.h
@@ -2,7 +2,7 @@
 #define SYS_SCHEDULE_H
 
 #define TQUEUE_MAX 0x100
-#define STACK_SIZE 0x1000
+#define STACK_SIZE 0x4000
 #define TQUEUE_CNT 5
 #define PRIORITIES 6
 #define MAX_THREADS TQUEUE_MAX*PRIORITIES*TQUEUE_CNT
@@ -47,7 +47,8 @@ struct Scheduler {
 
 
 void init_scheduler(void);
-// void add_thread(void* pc, void* arg, unsigned char priority);
+void add_thread(void* pc, void* arg, unsigned char priority);
+void uart_scheduler(void);
 /// TODO: ENSURE IRQ/ FIQ entry switches 
 ///  to user mode then calls the SVC call
 extern void schedule(void);
diff --git a/linker.ld b/linker.ld
index 2cacec0..17c951a 100644
--- a/linker.ld
+++ b/linker.ld
@@ -29,9 +29,9 @@ SECTIONS
     .bss :
     {
         bss = .;
-        *(.bss*)
-	KEEP(*(.bss.mmheap))
-	KEEP(*(.bss.heap))
+        *(.bss.*)
+	*(.bss.mmheap)
+	*(.bss.mmheap.base)
     }
     . = ALIGN(4096); /* align to page size */
     __bss_end = .;
diff --git a/src/globals.S b/src/globals.S
index 3a5da83..f6ec57f 100644
--- a/src/globals.S
+++ b/src/globals.S
@@ -11,7 +11,3 @@ cmdidx:
 .global cmd
 cmd:
 	.space 2049
-
-.section ".bss.heap"
-mheap:
-	.space 0x100000
diff --git a/src/globals.c b/src/globals.c
index 1313bd9..6e2e70c 100644
--- a/src/globals.c
+++ b/src/globals.c
@@ -12,8 +12,6 @@ char* os_info_v = VERSION;
 
 __attribute__((section(".bss"))) unsigned long exe_cnt;
 __attribute__((section(".bss"))) struct Mutex exe_cnt_m;
-__attribute__((section(".bss.mmheap"))) unsigned char rpi_heap[MAX_MM];
-__attribute__((section(".bss"))) void* rpi_heap_top;
 __attribute__((section(".bss"))) unsigned long nextpid;
 __attribute__((section(".bss"))) unsigned long stimel;
 __attribute__((section(".bss"))) unsigned long stimeh;
@@ -24,4 +22,6 @@ __attribute__((section(".bss"))) unsigned int gwidth;
 __attribute__((section(".bss"))) unsigned int gheight;
 __attribute__((section(".bss"))) unsigned int gpitch;
 __attribute__((section(".bss"))) unsigned int gisrgb;
+__attribute__((section(".bss.mmheap"))) unsigned char rpi_heap[MAX_MM];
+__attribute__((section(".bss.mmheap.base"))) void* rpi_heap_top;
 __attribute__((section(".bss"))) unsigned char stacks_table[MAX_THREADS];
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)
+{
+}