2 files changed, 77 insertions, 0 deletions
diff --git a/src/lib/mmu.S b/src/lib/mmu.S
new file mode 100644
index 0000000..faca3cc
--- /dev/null
+++ b/src/lib/mmu.S
@@ -0,0 +1,45 @@
+.section .text
+.globl mmu_start
+mmu_start:
+	mov r2, #0
+	// Invalidate Caches
+	mcr p15,0,r2,c7,c1,6 
+	// Invalidate TLB entries
+	mcr p15,0,r2,c8,c7,0 
+	// Data synchronisation barrier
+	mcr p15,0,r2,c7,c10,4 
+
+	// Set all domains to 0b11
+	mvn r2, #0
+	bic r2, #0xC
+	mcr p15,0,r2,c3,c0,0
+
+	// Set the translation table base address (remember to align 16 KiB!)
+	mcr p15,0,r0,c2,c0,0
+	mcr p15,0,r0,c2,c0,1
+	mov r3, #0
+	mcr p15,0,r3,c2,c0,2
+
+	// Set the bits mentioned above
+	mrc p15,0,r2,c1,c0,0
+	orr r2,r2,r1
+	mcr p15,0,r2,c1,c0,0
+	bx lr
+
+.globl mmu_stop
+mmu_stop:
+	mrc p15,0,r2,c1,c0,0
+	bic r2,#0x1000
+	bic r2,#0x0004
+	bic r2,#0x0001
+	mcr p15,0,r2,c1,c0,0
+	bx lr
+
+.globl tlb_invalidate
+tlb_invalidate:
+	mov r2, #0
+	// Invalidate Entries
+	mcr p15, 0, r2, c8, c7, 0
+	// DSB
+	mcr p15, 0, r2, c7, c10, 4
+	bx lr
diff --git a/src/lib/mmu.c b/src/lib/mmu.c
new file mode 100644
index 0000000..6a947c5
--- /dev/null
+++ b/src/lib/mmu.c
@@ -0,0 +1,32 @@
+#include <lib/mmu.h>
+
+#define CACHABLE 0x08
+#define BUFFERABLE 0x04
+#define NO_PERMISSIONS_REQUIRED 0b11 << 10
+#define MMU_TABLE_BASE 0x00004000
+
+void mmu_start(unsigned long base, unsigned long flags);
+
+void mmu_section(unsigned long virtual, unsigned long physical, unsigned long flags)
+{
+	unsigned long offset = virtual >> 20;
+	unsigned long* entry = (unsigned long*)(MMU_TABLE_BASE | (offset << 2));
+	unsigned long physval = (physical & 0xFFF00000) | (flags & 0x7FFC) | 0x00C02;
+	*entry = physval;
+}
+
+void mmu_init(void)
+{
+	mmu_section(0x00000000, 0x00000000, CACHABLE | BUFFERABLE);
+	for (unsigned long addr =  0x00100000;; addr += 0x00100000) {
+		mmu_section(addr, addr, NO_PERMISSIONS_REQUIRED);
+		if (addr == 0x02000000)
+			mmu_section(addr, addr, CACHABLE | BUFFERABLE | NO_PERMISSIONS_REQUIRED);
+		//else if (addr == 0x3F000000)
+		//else
+		//	mmu_section(addr, addr, NO_PERMISSIONS_REQUIRED);
+		if (addr == 0xFFF00000)
+			break;
+	}
+	mmu_start(MMU_TABLE_BASE,0x00000001|0x1000|0x0004);
+}