Scheduling

5 files changed, 217 insertions, 13 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