Nucleus

diff --git a/lib/heap.c b/lib/heap.c
new file mode 100644
index 0000000..64e2d85
--- /dev/null
+++ b/lib/heap.c
@@ -0,0 +1,95 @@
+/*
+ * This file contains the functions related to the kernel's heap.  It uses a
+ * simple method of allocating and freeing blocks from a pool in the kernel's
+ * memory space.  This heap will be present in every virtual address space, so
+ * can be used to store any kernel data structures.
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+#include <nucleus/kernel.h>
+#include <nucleus/lib.h>
+
+#define KHEAP_START 0x200000 /* 2MB */
+#define KHEAP_END   0x800000 /* 8MB */
+#define BLOCK_SIZE  16
+
+/* Structure for a memory block header */
+struct Header {
+	struct Header *next, *prev;
+	size_t size;
+	char magic[4];
+};
+
+/* Allocate a region of the heap */
+void *
+kmalloc(size_t size)
+{
+	size_t blockSize, gapSize;
+	uintptr_t blockEnd;
+	struct Header *prev, *head, *next = NULL;
+	head = prev = (void *) KHEAP_START;
+
+	/* Minimum allocation */
+	if (size % BLOCK_SIZE)
+		size += BLOCK_SIZE - (size % BLOCK_SIZE);
+
+	/* Block does not exist, create heap */
+	if (head->prev != head) {
+		head->prev = head;
+		head->next = NULL;
+		goto found;
+	}
+
+	/* Find gap */
+	while (head) {
+		next = head->next;
+		blockSize = sizeof(struct Header) + head->size;
+		blockEnd = (uintptr_t) head + blockSize;
+		if (next)
+			gapSize = (size_t) next - blockEnd;
+		else
+			gapSize = (size_t) KHEAP_END - blockEnd;
+		prev = head;
+
+		/* Fit in gap */
+		if (gapSize >= size + sizeof(struct Header)) {
+			head = (void *) blockEnd;
+			head->next = next;
+			head->prev = prev;
+			prev->next = head;
+			if (next)
+				next->prev = head;
+			goto found;
+		}
+
+		head = head->next;
+	}
+
+	panic("Kernel heap exhausted");
+
+found:
+	/* Found block */
+	head->size = size;
+	memcpy(head->magic, "HEAP", 4);
+	memset(head + 1, 0, size);
+	return (void *) (head + 1);
+}
+
+/* Free an allocated region of the heap */
+void
+kfree(void *addr)
+{
+	struct Header *prev, *head, *next;
+	head = (struct Header *) addr - 1;
+
+	ASSERT(!memcmp(head->magic, "HEAP", 4));
+	prev = head->prev;
+	next = head->next;
+	memset(head, 0, sizeof(struct Header));
+
+	if (prev != head)
+		prev->next = next;
+	if (next)
+		next->prev = prev;
+}