6 files changed, 146 insertions, 0 deletions
diff --git a/src/lib/ll.c b/src/lib/ll.c
new file mode 100644
index 0000000..6d96f45
--- /dev/null
+++ b/src/lib/ll.c
@@ -0,0 +1,32 @@
+#include "../lib/ll.h"
+#include "../lib/mem.h"
+
+struct LL* new(void* val)
+{
+	struct LL* ll = (struct LL*)malloc(sizeof(struct LL));
+	ll->prev = ll;
+	ll->next = ll;
+	ll->data = val;
+	return ll;
+}
+
+void push(struct LL* l, void* val)
+{
+	struct LL* ll = (struct LL*)malloc(sizeof(struct LL));
+	ll->prev = l->prev;
+	ll->next = l;
+	ll->prev->next = ll;
+	l->prev = ll;
+	ll->data = val;
+}
+
+void remove(struct LL* l, unsigned long idx)
+{
+	struct LL* t = l;
+	for(unsigned long i = 0; i < idx; i++) {
+		t = t->next;
+	}
+	t->prev->next = t->next;
+	t->next->prev = t->prev;
+	free(t);
+}
diff --git a/src/lib/ll.h b/src/lib/ll.h
new file mode 100644
index 0000000..4a11620
--- /dev/null
+++ b/src/lib/ll.h
@@ -0,0 +1,24 @@
+#ifndef LIB_LL_H
+#define LIB_LL_H
+
+struct LL {
+	struct LL* prev;
+	struct LL* next;
+	void* data;
+};
+
+struct LL* new(void* val);
+void push(struct LL* l, void* val);
+void remove(struct LL* l, unsigned long idx);
+
+#define showl(L, TYPE) { \
+	struct LL* t = L; \
+	do { \
+		uart_hex(*(TYPE*)t->data); \
+		t = t->next; \
+		if (t != l) \
+			uart_char(' '); \
+	} while (t != l); \
+}
+
+#endif
diff --git a/src/lib/mem.c b/src/lib/mem.c
index 3683ae7..0069623 100644
--- a/src/lib/mem.c
+++ b/src/lib/mem.c
@@ -41,3 +41,22 @@ unsigned char memcmp32(unsigned long* a, unsigned long* b, unsigned int n)
 	}
 	return 1;
 }
+
+#define MAX_MM 0x100000
+static unsigned char rpi_heap[MAX_MM] = {0,};
+
+void* malloc(unsigned char size)
+{
+	unsigned char* mem = (unsigned char*)rpi_heap;
+	unsigned long i = 0;
+	while (mem[i] != 0) {
+		i += mem[i]+1;
+	}
+	mem[i] = size;
+	return (void*)&mem[i+1];
+}
+
+void free(__attribute__((unused)) void* memloc)
+{
+	// TODO: Implement free
+}
diff --git a/src/lib/mem.h b/src/lib/mem.h
index 67e2717..4fdae78 100644
--- a/src/lib/mem.h
+++ b/src/lib/mem.h
@@ -8,4 +8,7 @@ void memshow32(unsigned long* addr, unsigned int n);
 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 free(void* memloc);
+
 #endif
diff --git a/src/lib/q.c b/src/lib/q.c
new file mode 100644
index 0000000..e40fee8
--- /dev/null
+++ b/src/lib/q.c
@@ -0,0 +1,38 @@
+#include "../lib/q.h"
+#include "../lib/mem.h"
+
+struct Q_base* newq()
+{
+	struct Q_base* q = (struct Q_base*)malloc(sizeof(struct Q_base));
+	q->next = 0;
+	q->last = 0;
+	return q;
+}
+
+void pushq(struct Q_base* qb, void* val)
+{
+	struct Q* n = (struct Q*)malloc(sizeof(struct Q));
+	n->next = 0;
+	n->data = val;
+	if (qb->last != 0)
+		qb->last->next = n;
+	else {
+		qb->next = n;
+	}
+	qb->last = n;
+}
+
+void popq(struct Q_base* qb)
+{
+	if (qb->next == 0)
+		return;
+	if (qb->next == qb->last) {
+		free(qb->next);
+		qb->next = 0;
+		qb->last = 0;
+		return;
+	}
+	struct Q* t = qb->next;
+	qb->next = qb->next->next;
+	free(t);
+}
diff --git a/src/lib/q.h b/src/lib/q.h
new file mode 100644
index 0000000..51ff1a1
--- /dev/null
+++ b/src/lib/q.h
@@ -0,0 +1,30 @@
+#ifndef LIB_Q_H
+#define LIB_Q_H
+
+struct Q_base {
+	struct Q* next;
+	struct Q* last;
+};
+
+struct Q {
+	struct Q* next;
+	void* data;
+};
+
+struct Q_base* newq();
+void pushq(struct Q_base* qb, void* val);
+void popq(struct Q_base* qb);
+
+#define showq(QQ, TYPE) { \
+	if (QQ->next != 0) { \
+		struct Q* t = QQ->next; \
+		while (t->next != 0) { \
+			uart_hex(*(TYPE*)t->data); \
+			t = t->next; \
+			uart_char(' '); \
+		} \
+		uart_hex(*(TYPE*)t->data); \
+	} \
+}
+
+#endif