9 files changed, 172 insertions, 6 deletions

diff --git a/src/lib/mem.c b/src/lib/mem.c
index 0069623..1a974f1 100644
--- a/src/lib/mem.c
+++ b/src/lib/mem.c
@@ -44,19 +44,78 @@ unsigned char memcmp32(unsigned long* a, unsigned long* b, unsigned int n)
 
 #define MAX_MM 0x100000
 static unsigned char rpi_heap[MAX_MM] = {0,};
+static void* rpi_heap_top = &rpi_heap;
 
 void* malloc(unsigned char size)
 {
 	unsigned char* mem = (unsigned char*)rpi_heap;
 	unsigned long i = 0;
-	while (mem[i] != 0) {
-		i += mem[i]+1;
+	// TODO: Use Null PID
+	while ((mem[i] != 0) && !(mem[i] == size && mem[i+1]==0)) {
+		i += mem[i]+2;
 	}
 	mem[i] = size;
-	return (void*)&mem[i+1];
+	// Use allocator's PID
+	mem[i+1] = 1;
+	// Update top of heap
+	rpi_heap_top = (void*)&mem[i+2+size];
+	return (void*)&mem[i+2];
 }
 
-void free(__attribute__((unused)) void* memloc)
+void free(void* memloc)
 {
-	// TODO: Implement free
+	// Don't try to free memory outside of heap
+	if(!(((void*)rpi_heap <= memloc) && (memloc < rpi_heap_top)))
+		return;
+	unsigned char* base = memloc - 2;
+	unsigned char size = *base;
+	// TODO: Use Null PID
+	base[1] = 0;
+	// Clear out old memory
+	for(unsigned int i = 0; i < size; i++) {
+		base[i+2] = 0;
+	}
+}
+
+void* heap_base(void)
+{
+	return (void*)rpi_heap;
+}
+
+void* heap_top(void)
+{
+	return rpi_heap_top;
+}
+
+void heap_info(void)
+{
+	unsigned char* base = rpi_heap;
+	while ((void*)base < rpi_heap_top) {
+		if(base[1] == 0) {
+			uart_char('F');
+			uart_char(' ');
+		}
+		uart_hex((unsigned long)(base+2));
+		uart_string(" Size: ");
+		uart_10(base[0]);
+		uart_string("\n");
+		static char* data = "00 \0";
+		static unsigned char temp = 0;
+		for(unsigned int i = 0; i < base[0]; 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 += *base + 2;
+	}
+	uart_char('\n');
 }
diff --git a/src/lib/mem.h b/src/lib/mem.h
index 4fdae78..4d02236 100644
--- a/src/lib/mem.h
+++ b/src/lib/mem.h
@@ -10,5 +10,8 @@ unsigned char memcmp32(unsigned long* a, unsigned long* b, unsigned int n);
 
 void* malloc(unsigned char size);
 void free(void* memloc);
+void* heap_base(void);
+void* heap_top(void);
+void heap_info(void);
 
 #endif
diff --git a/src/lib/q.c b/src/lib/q.c
index e40fee8..494c712 100644
--- a/src/lib/q.c
+++ b/src/lib/q.c
@@ -27,6 +27,7 @@ void popq(struct Q_base* qb)
 	if (qb->next == 0)
 		return;
 	if (qb->next == qb->last) {
+		free(qb->next->data);
 		free(qb->next);
 		qb->next = 0;
 		qb->last = 0;
@@ -34,5 +35,6 @@ void popq(struct Q_base* qb)
 	}
 	struct Q* t = qb->next;
 	qb->next = qb->next->next;
+	free(t->data);
 	free(t);
 }
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index f920b0e..8cf2780 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -2,6 +2,7 @@
 #include "../lib/ll.h"
 #include "../lib/q.h"
 
+#ifdef IGNORE
 #ifdef FLAT
 static struct Task* task_list[256];
 
@@ -79,3 +80,4 @@ void execute_task(void)
 	}
 }
 #endif
+#endif
diff --git a/src/sys/schedule.flat.c b/src/sys/schedule.flat.c
new file mode 100644
index 0000000..6eb0d14
--- /dev/null
+++ b/src/sys/schedule.flat.c
@@ -0,0 +1,32 @@
+#ifdef FLAT
+
+#include "../sys/schedule.h"
+static struct Task* task_list[256];
+
+static struct Scheduler scheduler = {
+	.tasks = task_list,
+};
+
+static unsigned int ntask_i = 0;
+
+void add_task(struct Task* t)
+{
+	scheduler.tasks[ntask_i] = t;
+	ntask_i += 1;
+	if (ntask_i > 256) {
+		ntask_i = 0;
+	}
+}
+
+unsigned int get_task_length(void)
+{
+	return ntask_i;
+}
+
+void execute_task(void)
+{
+	if (scheduler.tasks[ntask_i-1] != 0)
+		scheduler.tasks[ntask_i-1]->task();
+}
+
+#endif
diff --git a/src/sys/schedule.h b/src/sys/schedule.h
index 04dad6a..2fd6acd 100644
--- a/src/sys/schedule.h
+++ b/src/sys/schedule.h
@@ -22,6 +22,7 @@ struct Scheduler {
 };
 #endif
 
+void add_fxn(void (*task)(void), unsigned char priority);
 void add_task(struct Task*);
 unsigned int get_task_length(void);
 void execute_task(void);
diff --git a/src/sys/schedule.ll.c b/src/sys/schedule.ll.c
new file mode 100644
index 0000000..d9ab954
--- /dev/null
+++ b/src/sys/schedule.ll.c
@@ -0,0 +1,13 @@
+#ifdef LL
+
+#include "../sys/schedule.h"
+#include "../lib/ll.h"
+static struct LL bl = {
+	.prev = 0,
+	.next = 0,
+};
+static struct Scheduler scheduler = {
+	.tasks = &bl,
+};
+
+#endif
diff --git a/src/sys/schedule.q.c b/src/sys/schedule.q.c
new file mode 100644
index 0000000..818ba66
--- /dev/null
+++ b/src/sys/schedule.q.c
@@ -0,0 +1,54 @@
+#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;
+	add_task(t);
+}
+
+void add_task(struct Task* t)
+{
+	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.h b/src/util/mutex.h
index f99b7a8..f8178a2 100644
--- a/src/util/mutex.h
+++ b/src/util/mutex.h
@@ -8,7 +8,7 @@
 struct Mutex {
 	unsigned long* addr;
 	unsigned long pid;
-};
+} __attribute__((packed, aligned(4)));;
 
 unsigned char lock_mutex(struct Mutex*, unsigned long);
 unsigned char release_mutex(struct Mutex*, unsigned long);