Scheduling

11 files changed, 461 insertions, 28 deletions

diff --git a/include/cpu.h b/include/cpu.h
new file mode 100644
index 0000000..c93b3b5
--- /dev/null
+++ b/include/cpu.h
@@ -0,0 +1,69 @@
+#ifndef CPU_H
+#define CPU_H
+
+static inline unsigned long getmode(void)
+{
+	unsigned long mode;
+	asm volatile ("mrs %0, cpsr" : "=r"(mode));
+	return mode;
+}
+
+static inline void setsvc(void)
+{
+	unsigned long mode;
+	asm volatile (
+	"mrs %0, cpsr\n"
+	"bic %0, %0, #0x1F\n"
+	"orr %0, %0, #0x13\n"
+	"msr cpsr_c, %0" 
+	: "=r"(mode));
+}
+
+static inline void setmode(unsigned long mode)
+{
+	asm volatile ("msr cpsr_c, %0" :: "r"(mode));
+}
+
+static inline void* getsvcstack(void)
+{
+	void* sp;
+	asm volatile (
+	"mrs r0, cpsr\n"
+	"bic r1, r0, #0x1F\n"
+	"orr r1, r1, #0x13\n"
+	"msr cpsr_c, r1\n"
+	"mov %0, sp\n"
+	"msr cpsr_c, r0"
+	: "=r"(sp));
+	return sp;
+}
+
+static inline void* getfiqstack(void)
+{
+	void* sp;
+	asm volatile (
+	"mrs r0, cpsr\n"
+	"bic r1, r0, #0x1F\n"
+	"orr r1, r1, #0x11\n"
+	"msr cpsr_c, r1\n"
+	"mov %0, sp\n"
+	"msr cpsr_c, r0"
+	: "=r"(sp));
+	return sp;
+}
+
+static inline void* getirqstack(void)
+{
+	void* sp;
+	asm volatile (
+	"mrs r0, cpsr\n"
+	"bic r1, r0, #0x1F\n"
+	"orr r1, r1, #0x12\n"
+	"msr cpsr_c, r1\n"
+	"mov %0, sp\n"
+	"msr cpsr_c, r0"
+	: "=r"(sp));
+	return sp;
+}
+
+#endif
diff --git a/include/lib/ll.h b/include/lib/ll.h
index ab4148d..a9c3722 100644
--- a/include/lib/ll.h
+++ b/include/lib/ll.h
@@ -5,11 +5,13 @@ struct LL {
 	struct LL* prev;
 	struct LL* next;
 	void* data;
-};
+} __attribute__((packed));
 
 struct LL* new_ll(void* val);
 void push_ll(struct LL* l, void* val);
+void pop_ll(struct LL* l);
 void remove_ll(struct LL* l, unsigned long idx);
+unsigned long length_ll(struct LL* l);
 
 #define show_ll(L, TYPE) { \
 	struct LL* t = L; \
diff --git a/include/lib/q.h b/include/lib/q.h
index cf75c6d..11d7ab7 100644
--- a/include/lib/q.h
+++ b/include/lib/q.h
@@ -4,16 +4,17 @@
 struct Q_base {
 	struct Q* next;
 	struct Q* last;
-};
+} __attribute__((packed));
 
 struct Q {
 	struct Q* next;
 	void* data;
-};
+} __attribute__((packed));
 
 struct Q_base* new_q();
 void push_q(struct Q_base* qb, void* val);
 void pop_q(struct Q_base* qb);
+unsigned long length_q(struct Q_base* qb);
 
 #define show_q(QQ, TYPE) { \
 	if (QQ->next != 0) { \
diff --git a/include/sys/core.h b/include/sys/core.h
index 2d611b3..361ffb1 100644
--- a/include/sys/core.h
+++ b/include/sys/core.h
@@ -21,16 +21,6 @@ static inline void delay(unsigned long cycles)
 			: "=r"(cycles): [cycles]"0"(cycles) : "cc");
 }
 
-static inline void preserveregs(void)
-{
-	asm volatile("push {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11}");
-}
-
-static inline void restoreregs(void)
-{
-	asm volatile("pop {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11}");
-}
-
 static inline void* getsp(void)
 {
 	void* out;
diff --git a/include/sys/schedule.h b/include/sys/schedule.h
new file mode 100644
index 0000000..cced851
--- /dev/null
+++ b/include/sys/schedule.h
@@ -0,0 +1,142 @@
+#ifndef SYS_SCHEDULE_H
+#define SYS_SCHEDULE_H
+#include <cpu.h>
+#include <lib/mem.h>
+#include <lib/ll.h>
+#include <sys/core.h>
+
+enum ThreadStatus {
+	THREAD_READY,
+	THREAD_WAITING,
+	THREAD_WAITING_FOR_MUTEX,
+	THREAD_STACK_ERROR,
+	THREAD_RUNNING,
+	THREAD_FINISHED,
+};
+
+struct ThreadData {
+	unsigned short status;
+	void* mutex_waiting;
+	unsigned long pid;
+	unsigned char priority;
+};
+
+struct Thread {
+	struct ThreadData data;
+	void (*thread)(void);
+	void* stack;
+	void* stack_base;
+};
+
+#define MAX_THREADS 0x100
+#define STACK_SIZE 0x1000
+#define PRIORITIES 6
+struct Scheduler {
+	struct LL tlist[PRIORITIES];
+	struct LL* rthread_ll;
+};
+
+#ifndef SYS_SCHEDULE_C
+#define SYS_SCHEDULE_C
+extern struct Scheduler scheduler;
+#endif
+
+void init_scheduler(void);
+void add_thread(void (*thread_fxn)(void), unsigned char priority);
+void schedule(void);
+void remove_running_thread(void);
+
+static inline void preserveregs(struct Thread* thread)
+{
+	// Preserve current stack pointer
+	void* sp = getsp();
+	// Get current mode
+	unsigned long mode = getmode();
+	// Set supervisor mode - "User mode"
+	setsvc();
+	void* ssp = getsp();
+	// Move stack to reserved register area
+	setsp(thread->stack_base - 0x1000 + 16*4);
+	// Push registers to the stack
+	asm volatile ("push {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, lr}");
+	// Restore stack to previous
+	setsp(ssp);
+	// Restore mode
+	setmode(mode);
+	// Restore current stack pointer
+	setsp(sp);
+}
+
+static inline void preservestack(struct Thread* thread)
+{
+	// Get current mode
+	unsigned long mode = getmode();
+	// Set supervisor mode - "User mode"
+	setsvc();
+	// Store the stack pointer
+	void* ssp = getsp();
+	thread->stack = ssp;
+	// Restore mode
+	setmode(mode);
+}
+
+static inline void restoreregs(struct Thread* thread)
+{
+	// Preserve current stack pointer
+	void* sp = getsp();
+	// Get current mode
+	unsigned long mode = getmode();
+	// Set supervisor mode - "User mode"
+	setsvc();
+	void* ssp = getsp();
+	// Move stack to reserved register area
+	setsp(thread->stack_base - 0x1000 + 16*4 - 14*4);
+	// Restore registers on the stack
+	asm volatile ("pop {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, lr}");
+	// Restore stack to previous
+	setsp(ssp);
+	// Restore mode
+	setmode(mode);
+	// Restore current stack pointer
+	setsp(sp);
+}
+
+static inline void restorestack(struct Thread* thread)
+{
+	// Get current mode
+	unsigned long mode = getmode();
+	// Set supervisor mode - "User mode"
+	setsvc();
+	// Set stack pointer to thread's stack pointer
+	asm volatile("mov sp, %0" :: "r"(thread->stack));
+	// Restore mode
+	setmode(mode);
+}
+
+static inline void preservesysstack(unsigned long* sp)
+{
+	if (*sp == 0) {
+		unsigned long mode = getmode();
+		setsvc();
+		*sp = (unsigned long)getsp();
+		setmode(mode);
+	}
+}
+
+static inline void restoresysstack(unsigned long* sp)
+{
+	if (*sp) {
+		unsigned long mode = getmode();
+		setsvc();
+		setsp((void*)*sp);
+		setmode(mode);
+		*sp = 0;
+	}
+}
+
+static inline void preservepc(struct Thread* t)
+{
+	asm volatile ("mov %0, lr" : "=r"(t->thread));
+}
+
+#endif
diff --git a/src/lib/ll.c b/src/lib/ll.c
index 4eaa291..4918194 100644
--- a/src/lib/ll.c
+++ b/src/lib/ll.c
@@ -13,13 +13,24 @@ struct LL* new_ll(void* val)
 void push_ll(struct LL* l, void* val)
 {
 	struct LL* ll = (struct LL*)malloc(sizeof(struct LL));
+	l->prev->next = ll;
 	ll->prev = l->prev;
 	ll->next = l;
-	ll->prev->next = ll;
 	l->prev = ll;
 	ll->data = val;
 }
 
+void pop_ll(struct LL* l)
+{
+	if ((l->prev == l->next) && (l->prev == l))
+		l->data = 0;
+	else {
+		l->prev->next = l->next;
+		l->next->prev = l->prev;
+		free(l);
+	}
+}
+
 void remove_ll(struct LL* l, unsigned long idx)
 {
 	struct LL* t = l;
@@ -30,3 +41,17 @@ void remove_ll(struct LL* l, unsigned long idx)
 	t->next->prev = t->prev;
 	free(t);
 }
+
+unsigned long length_ll(struct LL* l)
+{
+	struct LL* t = l;
+	unsigned long len = 0;
+	while (1) {
+		if (t->next == l) {
+			return len;
+		}
+		len++;
+		t = t->next;
+	}
+	return len;
+}
diff --git a/src/lib/q.c b/src/lib/q.c
index ebae2b9..2ade143 100644
--- a/src/lib/q.c
+++ b/src/lib/q.c
@@ -38,3 +38,17 @@ void pop_q(struct Q_base* qb)
 	free(t->data);
 	free(t);
 }
+
+unsigned long length_q(struct Q_base* qb)
+{
+	unsigned long length = 0;
+	if(qb->next == 0)
+		return length;
+	length++;
+	struct Q* q = qb->next;
+	while (q != qb->last) {
+		length++;
+		q = q->next;
+	}
+	return length;
+}
diff --git a/src/sys/core.c b/src/sys/core.c
index 6bd2abf..204eb55 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -2,7 +2,6 @@
 #include <drivers/uart.h>
 #include <graphics/drawer.h>
 #include <graphics/lfb.h>
-#include <lib/ll.h>
 #include <lib/mem.h>
 #include <lib/q.h>
 #include <lib/strings.h>
@@ -10,6 +9,7 @@
 #include <sys/core.h>
 #include <sys/kernel.h>
 #include <sys/power.h>
+#include <sys/schedule.h>
 #include <sys/timer.h>
 #include <util/mutex.h>
 #include <util/status.h>
@@ -22,6 +22,8 @@ char* os_info_v = "?";
 char* os_info_v = VERSION;
 #endif
 
+void testlocal(void);
+
 // Initialize IRQs
 void sysinit(void)
 {
@@ -53,4 +55,28 @@ void sysinit(void)
 	enablefiq();
 
 	// Start Scheduler
+	init_scheduler();
+	add_thread(testlocal, 0);
+	add_thread(testlocal, 1);
+	add_thread(testlocal, 3);
+	add_thread(testlocal, 0);
+	add_thread(testlocal, 5);
+	add_thread(testlocal, 8);
+	delay(0x80000000);
+	schedule();
+}
+
+struct Mutex testm = {.addr = (void*)0xDEADBEEF, .pid = NULL_PID};
+void testlocal(void)
+{
+	unsigned char testd = 0xDE;
+	struct Thread* t = scheduler.rthread_ll->data;
+	delay(0x04000000);
+	testd -= 50;
+	uart_string("Ran Thread ");
+	delay(0x04000000);
+	uart_10(t->data.pid);
+	uart_char(' ');
+	uart_10(testd);
+	uart_char('\n');
 }
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
new file mode 100644
index 0000000..bde12bc
--- /dev/null
+++ b/src/sys/schedule.c
@@ -0,0 +1,140 @@
+#include <drivers/uart.h>
+#include <sys/core.h>
+#include <sys/schedule.h>
+#include <util/mutex.h>
+
+#define SYS_SCHEDULE_C
+struct Scheduler scheduler = {
+	.tlist = {
+		{.prev = 0, .next = 0, .data = 0},
+		{.prev = 0, .next = 0, .data = 0},
+		{.prev = 0, .next = 0, .data = 0},
+		{.prev = 0, .next = 0, .data = 0},
+		{.prev = 0, .next = 0, .data = 0},
+		{.prev = 0, .next = 0, .data = 0},
+	},
+	.rthread_ll = 0,
+};
+
+void init_scheduler(void)
+{
+	for(int i = 0; i < PRIORITIES; i++) {
+		scheduler.tlist[i].prev = &scheduler.tlist[i];
+		scheduler.tlist[i].next = &scheduler.tlist[i];
+		scheduler.tlist[i].data = 0;
+	}
+	scheduler.rthread_ll = 0;
+}
+
+unsigned char stacks_table[MAX_THREADS] = {0, };
+
+void* get_stack(void)
+{
+	for (int i = 0; i < MAX_THREADS; i++) {
+		if (stacks_table[i] == 0) {
+			stacks_table[i] = 1;
+			return (void*)heap_end() - STACK_SIZE*i;
+		}
+	}
+	return 0;
+}
+
+static unsigned long nextpid = 3;
+void add_thread(void (*thread_fxn)(void), unsigned char priority)
+{
+	struct Thread* thread = (struct Thread*)malloc(sizeof(struct Thread));
+	// Set the program counter to the entry
+	thread->thread = thread_fxn;
+	// Get a stack frame
+	thread->stack = get_stack();
+	thread->stack_base = thread->stack;
+	// Put in error state for no stack
+	if(thread->stack == 0)
+		thread->data.status = THREAD_STACK_ERROR;
+	else
+		thread->data.status = THREAD_READY;
+	// Doesn't wait for mutex at start
+	thread->data.mutex_waiting = 0;
+	// Set PID
+	thread->data.pid = nextpid++;
+	unsigned char p = priority;
+	if (p >= PRIORITIES) {
+		p = PRIORITIES - 1;
+	}
+	thread->data.priority = p;
+	push_ll(&scheduler.tlist[p], thread);
+}
+
+struct LL* get_next_thread(void)
+{
+	for(unsigned long i = 0; i < PRIORITIES; i++) {
+		struct LL* thread_ll = scheduler.tlist[i].next;
+		if (thread_ll == &scheduler.tlist[i])
+			continue;
+		do {
+			struct Thread* thread = thread_ll->data;
+			if((thread->data.status == THREAD_RUNNING) || (thread->data.status == THREAD_READY))
+				return thread_ll;
+			thread_ll = thread_ll->next;
+		} while(thread_ll != &scheduler.tlist[i]);
+	}
+	return 0;
+}
+
+unsigned long syssp = 0;
+void schedule(void)
+{
+	struct LL* current_thread_ll = scheduler.rthread_ll;
+	struct LL* next_thread_ll = get_next_thread();
+	if (current_thread_ll) {
+		if (current_thread_ll != next_thread_ll) {
+			// Context switch
+			struct Thread* current_thread = current_thread_ll->data;
+			struct Thread* next_thread = next_thread_ll->data;
+			//preserveregs(current_thread);
+			preservestack(current_thread);
+			preservepc(current_thread);
+			restorestack(next_thread);
+			//restoreregs(next_thread);
+			scheduler.rthread_ll = next_thread_ll;
+		}
+	}
+	else if (next_thread_ll) {
+		struct Thread* next_thread = next_thread_ll->data;
+		preservesysstack(&syssp);
+		//preservesysregs(&regloc)
+		restorestack(next_thread);
+		//restoreregs(next_thread);
+		scheduler.rthread_ll = next_thread_ll;
+	}
+	if (scheduler.rthread_ll) {
+		struct Thread* rthread = scheduler.rthread_ll->data;
+		// Run the thread - i.e. jump to the pc
+		rthread->thread();
+		// Remove the currently running thread after completion
+		remove_running_thread();
+		// Schedule the next thread
+		schedule();
+	} else {
+		//restoresysregs(&regloc);
+		restoresysstack(&syssp);
+	}
+}
+
+void remove_running_thread(void)
+{
+	if (scheduler.rthread_ll != 0) {
+		struct LL* ll = scheduler.rthread_ll;
+		if ((ll->next == ll->prev) && (ll->next == ll)) {
+			ll->data = 0;
+		}
+		else {
+			struct LL* prev = ll->prev;
+			struct LL* next = ll->next;
+			prev->next = ll->next;
+			next->prev = ll->prev;
+			free(ll);
+		}
+		scheduler.rthread_ll = 0;
+	}
+}
diff --git a/src/util/mutex.c b/src/util/mutex.c
index ade0be3..de7a515 100644
--- a/src/util/mutex.c
+++ b/src/util/mutex.c
@@ -1,13 +1,23 @@
 #include <cpu/atomic/swap.h>
 #include <lib/mem.h>
+#include <sys/schedule.h>
 #include <util/mutex.h>
 
 unsigned char lock_mutex(struct Mutex* m, unsigned long pid)
 {
 	if (m->pid == NULL_PID) {
-		atm_lock(pid, &m->pid);
+		// Use currently running thread's PID if no pid given
+		if (pid == 0) {
+			struct Thread* thread = scheduler.rthread_ll->data;
+			atm_lock(thread->data.pid, &m->pid);
+		} else {
+			atm_lock(pid, &m->pid);
+		}
 		return 0;
 	}
+	struct Thread* thread = scheduler.rthread_ll->data;
+	thread->data.status = THREAD_WAITING_FOR_MUTEX;
+	thread->data.mutex_waiting = m;
 	return 1;
 }
 
@@ -16,7 +26,15 @@ unsigned char lock_mutex(struct Mutex* m, unsigned long pid)
 //  mutex's pid lock
 unsigned char release_mutex(struct Mutex* m, unsigned long pid)
 {
-	if (m->pid == pid) {
+	// Use current thread's PID if no pid
+	if (pid == 0) {
+		struct Thread* thread = scheduler.rthread_ll->data;
+		if (m->pid == thread->data.pid) {
+			atm_release(&m->pid);
+			return 0;
+		}
+	}
+	else if (m->pid == pid) {
 		atm_release(&m->pid);
 		return 0;
 	}
diff --git a/src/util/status.c b/src/util/status.c
index f59ede6..be19287 100644
--- a/src/util/status.c
+++ b/src/util/status.c
@@ -1,3 +1,4 @@
+#include <cpu.h>
 #include <graphics/drawer.h>
 #include <graphics/lfb.h>
 #include <symbols.h>
@@ -129,19 +130,24 @@ void status(void)
 		write_char(&g_Drawer, ' ');
 	g_Drawer.x = 0;
 	g_Drawer.y = 9;
-	/*
-	struct Q* q = scheduler.tasks->next;
-	while (q != 0) {
-		struct Task* t = q->data;
-		write_hex32(&g_Drawer, (unsigned long)t->task);
-		write_char(&g_Drawer, ' ');
-		q = q->next;
-	}
-	write_char(&g_Drawer, '\n');
-	*/
 
-	unsigned long sp = (unsigned long)getsp();
+	unsigned long sp = (unsigned long)getsvcstack();
+	write_hex32(&g_Drawer, sp);
+	write_char(&g_Drawer, ' ');
+	sp = (unsigned long)getirqstack();
 	write_hex32(&g_Drawer, sp);
+	write_char(&g_Drawer, ' ');
+	sp = (unsigned long)getfiqstack();
+	write_hex32(&g_Drawer, sp);
+	write_char(&g_Drawer, '\n');
+	for(unsigned long i = 1; i <= 14; i++) {
+		write_hex32(&g_Drawer, *(unsigned long*)(0x4000 - i*4));
+		if(i % 6 == 0) {
+			write_char(&g_Drawer, '\n');
+		} else {
+			write_char(&g_Drawer, ' ');
+		}
+	}
 
 	g_Drawer.x = x;
 	g_Drawer.y = y;