From ab29c2c442db487c1f613c9bbc30d006da5eb16c Mon Sep 17 00:00:00 2001
From: Aaron Lindsay <aaron@aclindsay.com>
Date: Fri, 21 Sep 2012 11:52:24 -0400
Subject: [PATCH] mmu: Add initial implementation of identity mapping

---
 boot/start.S          | 44 ++++++++++++++++++++++++++++++++++++++-----
 include/mmu.h         |  6 ++++++
 kernel/Makefile.inc   |  1 +
 kernel/mmu.c          | 29 ++++++++++++++++++++++++++++
 kernel/start_kernel.c |  5 +++++
 link.ld               |  4 ++--
 6 files changed, 82 insertions(+), 7 deletions(-)
 create mode 100644 include/mmu.h
 create mode 100644 kernel/mmu.c

diff --git a/boot/start.S b/boot/start.S
index 018e1f3..946171f 100644
--- a/boot/start.S
+++ b/boot/start.S
@@ -9,9 +9,43 @@ interrupt_vector_table:
     b .
 
 .comm stack, 0x10000 @ Reserve 64k stack in the BSS
-_start:
-    .globl _start
-    ldr sp, =stack+0x10000 @ Set up the stack
-    bl main @ Jump to the main function
+.globl start
+
+start:
+	ldr r0, tt_base
+	mcr p15, 0, r0, c2, c0, 0 /* TTBR0 */
+
+/* Setup page table entries for the page table and kernel (domain 0) */
+	ldr r0, tt_tt_addr
+	ldr r1, tt_tt_val
+	str r1, [r0]
+
+	ldr r0, kernel_tt_addr
+	ldr r1, kernel_tt_val
+	str r1, [r0]
+
+/* Set access permissions for domain 0 to "Manager" */
+	mov r0, #0x3
+	mcr p15, 0, r0, c3, c0, 0 /* DACR */
+
+/* Enable the MMU */
+	mrc p15, 0, r0, c1, c0, 0 /* SCTLR */
+	orr r0, r0, #0x1
+	mcr p15, 0, r0, c1, c0, 0 /* SCTLR */
+
+	ldr sp, =stack+0x10000 @ Set up the stack
+	bl main @ Jump to the main function
+
 1: 
-    b 1b @ Halt
+	b 1b @ Halt
+
+tt_base:
+	.word 0x80000000
+tt_tt_addr:
+	.word 0x80002000
+tt_tt_val:
+	.word 0x80000c02 /* ...c02 means read/write at any priviledge level, and that it's a section w/o PXN bit set */
+kernel_tt_addr:
+	.word 0x80002004
+kernel_tt_val:
+	.word 0x80100c02
diff --git a/include/mmu.h b/include/mmu.h
new file mode 100644
index 0000000..3269761
--- /dev/null
+++ b/include/mmu.h
@@ -0,0 +1,6 @@
+#ifndef MMU_H
+#define MMU_H
+
+void mmu_reinit();
+
+#endif /* MMU_H */
diff --git a/kernel/Makefile.inc b/kernel/Makefile.inc
index 6ec7562..c4401b3 100644
--- a/kernel/Makefile.inc
+++ b/kernel/Makefile.inc
@@ -5,5 +5,6 @@ KOBJS += $(KERNEL_PREFIX)/font.o
 KOBJS += $(KERNEL_PREFIX)/framebuffer.o
 KOBJS += $(KERNEL_PREFIX)/list.o
 KOBJS += $(KERNEL_PREFIX)/mm.o
+KOBJS += $(KERNEL_PREFIX)/mmu.o
 KOBJS += $(KERNEL_PREFIX)/print.o
 KOBJS += $(KERNEL_PREFIX)/start_kernel.o
diff --git a/kernel/mmu.c b/kernel/mmu.c
new file mode 100644
index 0000000..4a59f1b
--- /dev/null
+++ b/kernel/mmu.c
@@ -0,0 +1,29 @@
+#include <print.h>
+
+#define SCTLR 15,0,1,0,0
+#define TTBR0 15,0,2,0,0
+#define TTBR1 15,0,2,0,1
+#define TTBCR 15,0,2,0,2
+
+#define _cp_read(var, cp, opc1, CRn, CRm, opc2) asm("mrc p" #cp ", " #opc1 ", %0, c" #CRn ", c" #CRm ", " #opc2 ";" : "=r"(var) : )
+#define _cp_write(var, cp, opc1, CRn, CRm, opc2) asm("mcr p" #cp ", " #opc1 ", %0, c" #CRn ", c" #CRm ", " #opc2 ";" : : "r"(var) )
+#define cp_read(var, ...)  _cp_read(var, __VA_ARGS__)
+#define cp_write(var, ...)  _cp_write(var, __VA_ARGS__)
+
+void mmu_reinit() {
+	unsigned int *curr_tt_entry; 
+	unsigned int curr_addr;
+
+	//get the current translation table base address
+	cp_read(curr_tt_entry, TTBR0);
+
+	//do first loop iteration outside the loop, because we have to check against wrapping back around to know we're done
+	*curr_tt_entry = 0xc02;
+	curr_tt_entry++;
+
+	//create identity mapping for entire address space using sections
+	for (curr_addr = 0x00100000; curr_addr != 0; curr_addr += 0x00100000) {
+		*curr_tt_entry = curr_addr | 0xc02;
+		curr_tt_entry++;
+	}
+}
diff --git a/kernel/start_kernel.c b/kernel/start_kernel.c
index 32b7080..47f59ac 100644
--- a/kernel/start_kernel.c
+++ b/kernel/start_kernel.c
@@ -1,3 +1,4 @@
+#include <mmu.h>
 #include <mm.h>
 #include <print.h>
 #include <devices/pl011.h>
@@ -54,6 +55,10 @@ void test_memory() {
  
 int main(void) {
 	char *lower;
+
+	//setup MMU
+	mmu_reinit();
+
 	print_init(&pl011_putc); //initialize the serial console
 
 	//setup memory
diff --git a/link.ld b/link.ld
index 584f7f5..05dea3e 100644
--- a/link.ld
+++ b/link.ld
@@ -1,8 +1,8 @@
-ENTRY (_start)
+ENTRY (start)
 
 SECTIONS
 {
-    . = 0x80000000;
+    . = 0x80100000;
     .text : { *(.text*) *(.rodata*) }
     .data : { *(.data*) }
     .bss : { *(.bss*) *(COMMON*) }