Nucleus

diff --git a/include/nucleus/cpu.h b/include/nucleus/cpu.h
index 69ef379..2a99c33 100644
--- a/include/nucleus/cpu.h
+++ b/include/nucleus/cpu.h
@@ -1,11 +1,16 @@
 #ifndef _NUCLEUS_CPU_H
 #define _NUCLEUS_CPU_H
 
+#include <stdint.h>
+
 /* Structure for an Interrupt Frame */
 struct InterruptFrame {
 	uint32_t eip, cs, eflags;
 };
 
+typedef void (*exc_handler_t)(struct InterruptFrame *, uint32_t);
+typedef void (*int_handler_t)(struct InterruptFrame *);
+
 extern uintptr_t lapicPtr, ioapicPtr;
 #define LAPIC(off)  (*((uint32_t *) ((uint32_t)  lapicPtr + (off))))
 #define IOAPIC(off) (*((uint32_t *) ((uint32_t) ioapicPtr + (off))))
@@ -14,4 +19,7 @@ extern uint8_t lapicNums[];
 #define CPUID lapicNums[(uint8_t) (LAPIC(0x20) >> 24)]
 #define MAX_CPUS 2
 
+void register_exception(int num, exc_handler_t addr);
+void register_interrupt(int num, int_handler_t addr);
+
 #endif
diff --git a/include/nucleus/memory.h b/include/nucleus/memory.h
index c6446b0..fe54c39 100644
--- a/include/nucleus/memory.h
+++ b/include/nucleus/memory.h
@@ -6,8 +6,47 @@
 
 #define PAGE_SIZE 0x1000
 
+#define PAGE_ADDR(pg) ((pg) & 0xFFFFF000)
+#define PAGE_ATTR(pg) ((pg) & 0x00000FFF)
+
+typedef uint32_t page_t;
+typedef uint32_t page_table_t;
+typedef uint32_t page_dir_t;
+
+/* Page Table Entry flags */
+enum PTEFlag {
+	PTE_PRESENT = (1 << 0),
+	PTE_WRITE   = (1 << 1),
+	PTE_USER    = (1 << 2),
+	PTE_THROUGH = (1 << 3),
+	PTE_NOCACHE = (1 << 4),
+	PTE_ACCESS  = (1 << 5),
+	PTE_DIRTY   = (1 << 6),
+	PTE_GLOBAL  = (1 << 8),
+};
+
+/* Page Directory Entry flags */
+enum PDEFlag {
+	PDE_PRESENT = (1 << 0),
+	PDE_WRITE   = (1 << 1),
+	PDE_USER    = (1 << 2),
+	PDE_THROUGH = (1 << 3),
+	PDE_NOCACHE = (1 << 4),
+	PDE_ACCESS  = (1 << 5),
+};
+
+/* Flush Translation Lookaside Buffer */
+static inline void
+flush_tlb(uintptr_t addr)
+{
+	asm volatile("invlpg (%0)" :: "r" (addr) : "memory");
+}
+
 uintptr_t alloc_frame(void);
 void free_frame(uintptr_t frame);
 void init_frames(size_t memMapSize, void *memMap);
 
+void set_page(uintptr_t vaddr, page_t page);
+void init_paging(void);
+
 #endif
diff --git a/kernel/idt.c b/kernel/idt.c
index 85f64c6..52076a8 100644
--- a/kernel/idt.c
+++ b/kernel/idt.c
@@ -367,3 +367,19 @@ cpu_load_idt(void)
 	};
 	asm volatile("lidt %0" :: "m" (ptr));
 }
+
+/* Register an exception handler */
+void
+register_exception(int num, exc_handler_t addr)
+{
+	if (num >= 0 && num < 32)
+		exceptions[num] = addr;
+}
+
+/* Register an interrupt handler */
+void
+register_interrupt(int num, int_handler_t addr)
+{
+	if (num >= 0 && num < 224)
+		interrupts[num] = addr;
+}
diff --git a/kernel/main.c b/kernel/main.c
index a343089..4536e45 100644
--- a/kernel/main.c
+++ b/kernel/main.c
@@ -57,5 +57,8 @@ kmain(uint32_t esp, struct MultibootInfo *mbinfo)
 	cpu_load();
 	init_acpi(ebda);
 
+	/* Initialise paging */
+	init_paging();
+
 	panic("End of kernel!");
 }
diff --git a/memory/fault.c b/memory/fault.c
new file mode 100644
index 0000000..e8d66a7
--- /dev/null
+++ b/memory/fault.c
@@ -0,0 +1,23 @@
+/*
+ * This file contains the page fault handler.
+ * There is an early handler which just gives out memory when requested.  This
+ * is used by the kernel before it has initialised multi-tasking and the virtual
+ * file system.
+ */
+
+#include <stdint.h>
+#include <nucleus/cpu.h>
+#include <nucleus/memory.h>
+#include <nucleus/panic.h>
+
+/* Early (pre-VFS/tasking) page fault handler */
+void
+early_page_fault_handler(struct InterruptFrame *frame, uint32_t err)
+{
+	uintptr_t addr;
+	asm volatile("mov %%cr2, %0" : "=r" (addr));
+	if (!PAGE_ADDR(addr))
+		panic("Null dereference @ %#.8x", frame->eip);
+	/* Allocate a page */
+	set_page(addr, alloc_frame() | PTE_PRESENT | PTE_WRITE | PTE_GLOBAL);
+}
diff --git a/memory/paging.c b/memory/paging.c
new file mode 100644
index 0000000..b4f505e
--- /dev/null
+++ b/memory/paging.c
@@ -0,0 +1,90 @@
+/*
+ * This file contains all the functions used to manipulate the virtual address
+ * spaces.  It controls all of the system's paging and virtual memory from a low
+ * level perspective.  On initialisation, a page directory is allocated for the
+ * kernel that identity maps everything.
+ */
+
+#include <stdint.h>
+#include <string.h>
+#include <nucleus/memory.h>
+#include "paging.h"
+
+/* Switch page directory */
+static void
+switch_dir(page_dir_t dir)
+{
+	asm volatile("mov %0, %%cr3" :: "r" (dir));
+}
+
+/* Set a page mapping */
+void
+set_page(uintptr_t vaddr, page_t page)
+{
+	page_t *mappings = (void *) 0xFFC00000;
+	page_table_t *tables = (void *) 0xFFFFF000;
+	uintptr_t address = vaddr >> 12;
+	uint32_t tbl = address / 1024;
+	/* Create table if not present */
+	if (!(tables[tbl] & PDE_PRESENT)) {
+		tables[tbl] = alloc_frame() | PDE_PRESENT | PDE_WRITE;
+		memset(mappings + (tbl * 1024), 0, PAGE_SIZE);
+	}
+	mappings[address] = page;
+}
+
+/* Initialise paging */
+void
+init_paging(void)
+{
+	uint16_t tbl, pg;
+	page_dir_t kernelDir = alloc_frame();
+	page_table_t *kernelTables = (page_table_t *) kernelDir;
+	page_t *table;
+	for (tbl = 0; tbl < 1024; tbl++)
+		kernelTables[tbl] = 0x00000000 | PDE_WRITE;
+	for (tbl = 0; tbl < 2; tbl++) {
+		table = (page_t *) alloc_frame();
+		kernelTables[tbl] = ((page_table_t) table)
+		                  | PDE_WRITE | PDE_PRESENT;
+		for (pg = 0; pg < 1024; pg++) {
+			/* Skip bottom page - catches NULL dereferences */
+			if (!tbl && !pg)
+				continue;
+			table[pg] = (((tbl * 1024) + pg) << 12)
+			          | PTE_WRITE | PTE_PRESENT | PTE_GLOBAL;
+		}
+	}
+	/* Map the directory into itself */
+	kernelTables[1023] = kernelDir | PDE_WRITE | PDE_PRESENT;
+
+	/*
+	 * By mapping the page directory as the last page table, the page
+	 * directory entries are read as page table entries, and the page
+	 * table entries become the pages. This means that each page contains
+	 * the contents of a page table, and the region in memory represented by
+	 * the last page table contains a contiguous list of all pages in
+	 * memory.  The very last page contains the contents of the page
+	 * directory itself.  This means that each virtual address space
+	 * contains it's own paging structures.
+	 */
+
+	/* Use kernel directory */
+	switch_dir(kernelDir);
+	register_exception(14, early_page_fault_handler);
+	asm volatile (
+		"movl %%cr0, %%eax;"
+		"orl $0x80000000, %%eax;"
+		"movl %%eax, %%cr0"
+		::: "eax"
+	);
+
+	/* Identity page the APIC registers */
+	set_page(lapicPtr, lapicPtr | PTE_PRESENT | PTE_WRITE | PTE_GLOBAL);
+	set_page(ioapicPtr, ioapicPtr | PTE_PRESENT | PTE_WRITE | PTE_GLOBAL);
+
+	/* Allocate a kernel stack */
+	uintptr_t stk;
+	for (stk = 0xF0400000; stk < 0xF0800000; stk += PAGE_SIZE)
+		set_page(stk, alloc_frame() | PTE_PRESENT | PTE_WRITE);
+}
diff --git a/memory/paging.h b/memory/paging.h
new file mode 100644
index 0000000..636ca86
--- /dev/null
+++ b/memory/paging.h
@@ -0,0 +1,9 @@
+#ifndef MEMORY_PAGING_H
+#define MEMORY_PAGING_H
+
+#include <stdint.h>
+#include <nucleus/cpu.h>
+
+void early_page_fault_handler(struct InterruptFrame *frame, uint32_t err);
+
+#endif