15 files changed, 315 insertions, 55 deletions

diff --git a/later/schedule.S b/later/schedule.S
new file mode 100644
index 0000000..1811c7c
--- /dev/null
+++ b/later/schedule.S
@@ -0,0 +1,58 @@
+.section .text
+.globl preserveregs
+preserveregs:
+	str r4,  [r0, #0x10]
+	str r5,  [r0, #0x14]
+	str r6,  [r0, #0x18]
+	str r7,  [r0, #0x1c]
+	str r8,  [r0, #0x20]
+	str r9,  [r0, #0x24]
+	str r10, [r0, #0x28]
+	str r11, [r0, #0x2c]
+	bx lr
+
+.globl restoreregs
+restoreregs:
+	ldr r4,  [r0, #0x10]
+	ldr r5,  [r0, #0x14]
+	ldr r6,  [r0, #0x18]
+	ldr r7,  [r0, #0x1c]
+	ldr r8,  [r0, #0x20]
+	ldr r9,  [r0, #0x24]
+	ldr r10, [r0, #0x28]
+	ldr r11, [r0, #0x2c]
+	bx lr
+
+.globl exetask
+exetask:
+	push {r0, r4, r5, r6, r7, r8, r9, r10, r11, lr}
+	// Restore registers from context switch
+	ldr r4,  [r0, #0x10]
+	ldr r5,  [r0, #0x14]
+	ldr r6,  [r0, #0x18]
+	ldr r7,  [r0, #0x1c]
+	ldr r8,  [r0, #0x20]
+	ldr r9,  [r0, #0x24]
+	ldr r10, [r0, #0x28]
+	ldr r11, [r0, #0x2c]
+
+	// Preserve system stack
+	ldr r1,  =msp
+	str sp, [r1]
+	// Setup task's stack
+	ldr sp, [r0, #0x34]
+	// Switch to task
+	ldr lr, [r0, #0x3c]
+	blx lr
+	// Restore system stack
+	ldr r1, =msp
+	ldr sp, [r1]
+	pop {r0, r4, r5, r6, r7, r8, r9, r10, r11, pc}
+
+.section .data
+msp:
+	.space 4
+.section .data.stacks
+.globl stacks
+stacks:
+	.space 0x100000
diff --git a/src/sys/schedule.c b/later/schedule.c
index 8cf2780..8cf2780 100644
--- a/src/sys/schedule.c
+++ b/later/schedule.c
diff --git a/src/sys/schedule.flat.c b/later/schedule.flat.c
index 6eb0d14..6eb0d14 100644
--- a/src/sys/schedule.flat.c
+++ b/later/schedule.flat.c
diff --git a/src/sys/schedule.h b/later/schedule.h
index c8e938d..77da7eb 100644
--- a/src/sys/schedule.h
+++ b/later/schedule.h
@@ -1,34 +1,42 @@
 #ifndef SYS_SCHEDULE_H
 #define SYS_SCHEDULE_H
 
-#define STACK_SIZE 0x1000
-struct TaskMemory {
-	unsigned long reg[16];
-	unsigned char stack[STACK_SIZE];
-};
+#define S_WAITING 0
+#define S_READY 1
 
+#define STACK_SIZE 0x800
 struct Task {
 	unsigned char priority;
+	unsigned char status;
+	unsigned long pid;
 	void (*task)(void);
+	unsigned long reg[16];
+	unsigned long stacki;
+	void* stack;
 };
 
 #ifdef FLAT
 struct Scheduler {
+	unsigned long running_pid;
 	struct Task** tasks;
 };
 #elseif LL
 #include "../lib/ll.h"
 struct Scheduler {
+	unsigned long running_pid;
 	struct LL* tasks;
 };
 #else
 #include "../lib/q.h"
 struct Scheduler {
+	unsigned long running_pid;
 	struct Q_base* tasks;
 };
 #endif
 
 void add_fxn(void (*task)(void), unsigned char priority);
+unsigned char exists(void (*task)(void));
+void add_no_repeat(void (*task)(void), unsigned char priority);
 unsigned int get_task_length(void);
 void execute_task(void);
 
diff --git a/src/sys/schedule.ll.c b/later/schedule.ll.c
index d9ab954..d9ab954 100644
--- a/src/sys/schedule.ll.c
+++ b/later/schedule.ll.c
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
diff --git a/linker.ld b/linker.ld
index df0eb95..33666f2 100644
--- a/linker.ld
+++ b/linker.ld
@@ -18,6 +18,8 @@ SECTIONS
     .data :
     {
         *(.data*)
+	__stacks_start = .;
+	KEEP(*(.data.stacks))
     }
     . = ALIGN(4096); /* align to page size */
     __data_end = .;
diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index 874e764..dbd4c10 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -4,11 +4,14 @@
 #include "../sys/core.h"
 #include "../sys/timer.h"
 #include "../util/mutex.h"
+#include "../util/status.h"
 #include "../util/time.h"
 
 extern unsigned long cmdidx;
 extern char cmd[2048];
 
+void localtest(void);
+
 void c_irq_handler(void)
 {
 	disableirq();
@@ -61,6 +64,10 @@ void c_irq_handler(void)
 							cmd[off] = (char) data;
 							off += 1;
 							release_mutex(&exe_cnt_m, SYS_PID);
+						// New Task Case
+						} else if (data == 0x61) {
+							cmd[off] = (char) data;
+							off += 1;
 						// Else output
 						} else {
 							cmd[off] = (char) data;
@@ -99,3 +106,7 @@ void c_irq_handler(void)
 	}
 	return;
 }
+
+void localtest(void)
+{
+}
diff --git a/src/lib/mem.c b/src/lib/mem.c
index 296e096..d738305 100644
--- a/src/lib/mem.c
+++ b/src/lib/mem.c
@@ -64,6 +64,45 @@ void* malloc(unsigned char size)
 	return (void*)&mem[i+2];
 }
 
+void* malloca(unsigned char size, unsigned char amnt)
+{
+	unsigned char* mem = (unsigned char*)rpi_heap;
+	unsigned long i = 0;
+	// TODO: Use Null PID
+	while(1) {
+		unsigned long diff = (unsigned long)mem + i + 2;
+		diff %= amnt;
+		diff = amnt - diff;
+		if((mem[i] == size) && mem[i+1]==0) {
+			if(diff == 0) {
+				mem[i] = size;
+				mem[i+1] = 1;
+				return (void*)&mem[i+2];
+			}
+		} else if (mem[i] == 0) {
+			if(diff == 0 || diff == amnt) {
+				mem[i] = size;
+				mem[i+1] = 1;
+				rpi_heap_top = (void*)&mem[i+2+size];
+				return (void*)&mem[i+2];
+			} else {
+				if(diff <= 2) {
+					diff += amnt;
+				}
+				mem[i] = diff-2;
+				mem[i+1] = 0;
+				i += diff;
+				mem[i] = size;
+				mem[i+1] = 1;
+				rpi_heap_top = (void*)&mem[i+2+size];
+				return (void*)&mem[i+2];
+			}
+		}
+
+		i += mem[i]+2;
+	}
+}
+
 void free(void* memloc)
 {
 	// Don't try to free memory outside of heap
@@ -122,3 +161,37 @@ void heap_info(void)
 	}
 	uart_char('\n');
 }
+
+void heap_info_u(void)
+{
+	unsigned char* base = rpi_heap;
+	while ((void*)base < rpi_heap_top) {
+		unsigned char size = *base;
+		if(base[1] == 0) {
+			base += size + 2;
+			continue;
+		}
+		uart_hex((unsigned long)(base+2));
+		uart_string(" Size: ");
+		uart_10(size);
+		uart_string("\n");
+		static char* data = "00 \0";
+		static unsigned char temp = 0;
+		for(unsigned int i = 0; i < size; i++) {
+			temp = (base[2+i]>>4)&0xF;
+			if(temp > 9)
+				temp += 7;
+			temp += 0x30;
+			data[0] = temp;
+			temp = (base[2+i])&0xF;
+			if(temp > 9)
+				temp += 7;
+			temp += 0x30;
+			data[1] = temp;
+			uart_string(data);
+		}
+		uart_char('\n');
+		base += size + 2;
+	}
+	uart_char('\n');
+}
diff --git a/src/lib/mem.h b/src/lib/mem.h
index ec1402f..5c5cc94 100644
--- a/src/lib/mem.h
+++ b/src/lib/mem.h
@@ -15,9 +15,11 @@ void memcpy32(unsigned long* src, unsigned long* dest, unsigned int n);
 unsigned char memcmp32(unsigned long* a, unsigned long* b, unsigned int n);
 
 void* malloc(unsigned char size);
+void* malloca(unsigned char size, unsigned char amnt);
 void free(void* memloc);
 void* heap_base(void);
 void* heap_top(void);
 void heap_info(void);
+void heap_info_u(void);
 
 #endif
diff --git a/src/sys/core.c b/src/sys/core.c
index fbe0536..8d37a88 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -8,7 +8,6 @@
 #include "../lib/strings.h"
 #include "../sys/core.h"
 #include "../sys/power.h"
-#include "../sys/schedule.h"
 #include "../sys/timer.h"
 #include "../util/mutex.h"
 #include "../util/status.h"
@@ -51,4 +50,10 @@ void sysinit(void)
 	enablefiq();
 
 	// Start Scheduler
+	unsigned long count1 = -5;
+	struct Mutex* m = create_mutex(&count1);
+	if (lock_mutex(m, SYS_PID) == 0) {
+		uart_hex(*(unsigned long*)m->addr);
+		release_mutex(m, SYS_PID);
+	}
 }
diff --git a/src/sys/schedule.q.c b/src/sys/schedule.q.c
deleted file mode 100644
index 88488da..0000000
--- a/src/sys/schedule.q.c
+++ /dev/null
@@ -1,49 +0,0 @@
-#if !(defined(LL) || defined(FLAT))
-#include "../sys/schedule.h"
-#include "../lib/q.h"
-#include "../lib/mem.h"
-
-static struct Q_base bq = {
-	.next = 0,
-	.last = 0,
-};
-static struct Scheduler  scheduler = {
-	.tasks = &bq,
-};
-
-void add_fxn(void (*task)(void), unsigned char priority)
-{
-	struct Task* t = (struct Task*)malloc(sizeof(struct Task));
-	t->priority = priority;
-	t->task = task;
-	pushq(scheduler.tasks, t);
-}
-
-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)
-{
-	if (scheduler.tasks->last != 0) {
-		struct Task* tsk = (struct Task*)scheduler.tasks->next->data;
-		(tsk->task)();
-		popq(scheduler.tasks);
-	}
-}
-
-#endif
diff --git a/src/util/mutex.c b/src/util/mutex.c
index 2637a6c..1763de2 100644
--- a/src/util/mutex.c
+++ b/src/util/mutex.c
@@ -1,4 +1,5 @@
 #include "../cpu/atomic/swap.h"
+#include "../lib/mem.h"
 #include "../util/mutex.h"
 
 unsigned char lock_mutex(struct Mutex* m, unsigned long pid)
@@ -21,3 +22,11 @@ unsigned char release_mutex(struct Mutex* m, unsigned long pid)
 	}
 	return 1;
 }
+
+struct Mutex* create_mutex(void* addr)
+{
+	// Ensure aligned to word - Important for Atomic Swap
+	struct Mutex* m = (struct Mutex*)malloca(sizeof(struct Mutex), 4);
+	m->addr = addr;
+	return m;
+}
diff --git a/src/util/mutex.h b/src/util/mutex.h
index 283be53..524a461 100644
--- a/src/util/mutex.h
+++ b/src/util/mutex.h
@@ -12,5 +12,6 @@ struct Mutex {
 
 unsigned char lock_mutex(struct Mutex*, unsigned long);
 unsigned char release_mutex(struct Mutex*, unsigned long);
+struct Mutex* create_mutex(void* addr);
 
 #endif
diff --git a/src/util/status.c b/src/util/status.c
index c4a7c80..1ac08e8 100644
--- a/src/util/status.c
+++ b/src/util/status.c
@@ -123,6 +123,28 @@ void status(void)
 		}
 		write_cchar(&g_Drawer, '!', 0xFF00FF);
 	}
+
+	g_Drawer.x = 0;
+	g_Drawer.y = 9;
+	for(int i = 0; i < 128; i++)
+		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;
+	asm volatile ("mov %0, sp": "=r"(sp));
+	write_hex32(&g_Drawer, sp);
+
 	g_Drawer.x = x;
 	g_Drawer.y = y;
 }