Nucleus

diff --git a/include/nucleus/memory.h b/include/nucleus/memory.h
index fe54c39..36c7bf1 100644
--- a/include/nucleus/memory.h
+++ b/include/nucleus/memory.h
@@ -47,6 +47,10 @@ void free_frame(uintptr_t frame);
 void init_frames(size_t memMapSize, void *memMap);
 
 void set_page(uintptr_t vaddr, page_t page);
+page_dir_t clone_dir(void);
 void init_paging(void);
 
+void *kmalloc(size_t size);
+void kfree(void *addr);
+
 #endif
diff --git a/memory/copy.S b/memory/copy.S
new file mode 100644
index 0000000..1d1c6a3
--- /dev/null
+++ b/memory/copy.S
@@ -0,0 +1,35 @@
+/*
+ * This is a small routine to copy the contents of one page frame to another.
+ * In the 32-bit kernel, only pages in the current page directory are
+ * accessible, so this routine disables paging to copy the physical frames.
+ */
+
+.section .text
+.global copy_page_frame
+.type copy_page_frame, @function
+.align 4
+copy_page_frame:
+.code32
+	push %ebx
+	pushf
+	cli
+	mov 12(%esp), %ebx
+	mov 16(%esp), %ecx
+	mov %cr0, %edx
+	and $0x7FFFFFFF, %edx
+	mov %edx, %cr0
+	mov $1024, %edx
+.loop:
+	mov (%ebx), %eax
+	mov %eax, (%ecx)
+	add $4, %ebx
+	add $4, %ecx
+	dec %edx
+	jnz .loop
+	mov %cr0, %edx
+	or $0x80000000, %edx
+	mov %edx, %cr0
+	sti
+	popf
+	pop %ebx
+	ret
diff --git a/memory/heap.c b/memory/heap.c
new file mode 100644
index 0000000..a61149c
--- /dev/null
+++ b/memory/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 <stdint.h>
+#include <stddef.h>
+#include <string.h>
+#include <nucleus/panic.h>
+
+#define KHEAP_START 0x200000 /* 2MB */
+#define KHEAP_END   0x800000 /* 2MB */
+#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;
+}
diff --git a/memory/paging.c b/memory/paging.c
index b4f505e..cd6deb5 100644
--- a/memory/paging.c
+++ b/memory/paging.c
@@ -10,6 +10,8 @@
 #include <nucleus/memory.h>
 #include "paging.h"
 
+void copy_page_frame(uintptr_t src, uintptr_t dest);
+
 /* Switch page directory */
 static void
 switch_dir(page_dir_t dir)
@@ -33,6 +35,60 @@ set_page(uintptr_t vaddr, page_t page)
 	mappings[address] = page;
 }
 
+/* Clone an entire page directory */
+page_dir_t
+clone_dir(void)
+{
+	page_table_t *oldTables = (void *) 0xFFFFF000;
+	page_table_t *newTables = (void *) 0xFFFFE000;
+	page_t *oldTable, *newTable;
+	page_dir_t dir = alloc_frame();
+	uint16_t tbl, pg;
+
+	/* Temporarily link new paging structures into current directory */
+	page_table_t restore = oldTables[1022];
+	oldTables[1022] = dir | PDE_PRESENT | PDE_WRITE;
+	flush_tlb((uintptr_t) newTables);
+
+	/* Iterate tables */
+	for (tbl = 0; tbl < 1022; tbl++) {
+		if (!(oldTables[tbl] & PDE_PRESENT))
+			continue;
+
+
+		/* Link kernel tables */
+		if (tbl < 2 || tbl >= 1008) {
+			newTables[tbl] = oldTables[tbl];
+			continue;
+		}
+
+		/* Copy everything else */
+		newTables[tbl] = alloc_frame() | PAGE_ATTR(oldTables[tbl]);
+		oldTable = (page_t *) 0xFFC00000 + (tbl * 1024);
+		newTable = (page_t *) 0xFF800000 + (tbl * 1024);
+		flush_tlb((uintptr_t) newTable);
+		for (pg = 0; pg < 1024; pg++) {
+			if (!(oldTable[pg] & PTE_PRESENT)) {
+				newTable[pg] = 0;
+				continue;
+			}
+
+			/* Copy page frame content */
+			newTable[pg] = alloc_frame() | PAGE_ATTR(oldTable[pg]);
+			flush_tlb(((tbl * 1024) + pg) << 12);
+			copy_page_frame(PAGE_ADDR(oldTable[pg]),
+			                PAGE_ADDR(newTable[pg]));
+		}
+	}
+	newTables[1023] = oldTables[1022];
+
+	/* Unlink paging structures from current directory */
+	oldTables[1022] = restore;
+	flush_tlb((uintptr_t) newTables);
+
+	return dir;
+}
+
 /* Initialise paging */
 void
 init_paging(void)