1 files changed, 118 insertions, 0 deletions
diff --git a/later/schedule.q.c b/later/schedule.q.c
new file mode 100644
index 0000000..4a5ff85
--- /dev/null
+++ b/later/schedule.q.c
@@ -0,0 +1,118 @@
+#if !(defined(LL) || defined(FLAT))
+#include "../sys/schedule.h"
+#include "../lib/q.h"
+#include "../lib/mem.h"
+#include "../drivers/uart.h"
+
+extern void preserveregs(void*);
+extern void restoreregs(void*);
+extern void exetask(void*);
+
+static unsigned long next_pid = 3;
+unsigned char table[256] = {0, };
+
+static struct __attribute__((packed,align(4))) Q_base bq = {
+	.next = 0,
+	.last = 0,
+};
+struct __attribute__((packed,align(4))) Scheduler scheduler = {
+	.running_pid = 0,
+	.tasks = &bq,
+};
+
+extern unsigned long __stacks_start;
+
+unsigned long getstack(void)
+{
+	for(unsigned int i = 0; i < 256; i++) {
+		if (table[i] == 0) {
+			table[i] = 1;
+			return i;
+		}
+	}
+	return -1;
+}
+
+void add_fxn(void (*task)(void), unsigned char priority)
+{
+	struct Task* t = (struct Task*)malloc(sizeof(struct Task));
+	// Allocate a stack frame and space for registers to be preserved on context switches
+	t->priority = priority;
+	t->task = task;
+	t->stacki = getstack();
+	t->pid = next_pid;
+	next_pid += 1;
+	if(t->stacki > 256)
+		t->stack = 0;
+	else
+		t->stack = (void*)(__stacks_start + STACK_SIZE*t->stacki);
+	t->status = S_READY;
+	for(unsigned char i = 0; i < 13; i++)
+		t->reg[i] = 0;
+	t->reg[13] = (unsigned long)t->stack;
+	t->reg[14] = 0; // LR
+	t->reg[15] = (unsigned long)task; // PC
+	pushq(scheduler.tasks, t);
+}
+
+unsigned char exists(void (*task)(void))
+{
+	if (scheduler.tasks->next == 0)
+		return 0;
+	struct Q* q = scheduler.tasks->next;
+	while (q != 0) {
+		struct Task* t = q->data;
+		if (t->task == task)
+			return 1;
+		q = q->next;
+	}
+	return 0;
+}
+
+void add_no_repeat(void (*task)(void), unsigned char priority)
+{
+	if (!(exists(task)))
+		add_fxn(task, priority);
+}
+
+unsigned int get_task_length(void)
+{
+	unsigned int length = 0;
+	if (scheduler.tasks->last == 0)
+		return length;
+	else if (scheduler.tasks->next == scheduler.tasks->last)
+		return 1;
+	else {
+		struct Q* q = scheduler.tasks->next;
+		length += 1;
+		while (q->next != 0) {
+			q = q->next;
+			length += 1;
+		}
+		return length;
+	}
+}
+void execute_task(void)
+{
+	// Preserve Current Running PID's Data
+	if (scheduler.tasks->last != 0) {
+		struct Q* q = (struct Q*)scheduler.tasks->next;
+		while (((struct Task*)q->data)->status == S_WAITING) {
+			q = q->next;
+		}
+		struct Task* tsk = (struct Task*)q->data;
+		// Restore registers 
+		//  Including program counter as the entry point
+		//  Including allocated stack position
+		//  Set lr to the return address to restore system stack
+		//preserveregs(tsk->reg);
+		//restoreregs(tsk->reg);
+		scheduler.running_pid = tsk->pid;
+		exetask(tsk->reg);
+		scheduler.running_pid = 0;
+		table[tsk->stacki] = 0;
+		popq(scheduler.tasks);
+	}
+}
+
+#endif