1 files changed, 93 insertions, 0 deletions
diff --git a/src/util/mem/paging.rs b/src/util/mem/paging.rs
new file mode 100644
index 0000000..8a21bda
--- /dev/null
+++ b/src/util/mem/paging.rs
@@ -0,0 +1,93 @@
+//! # MMU Functions
+
+pub mod MMU {
+    pub const CACHABLE: u32 = 1 << 3;
+    pub const BUFFERABLE: u32 = 1 << 2;
+    pub const NO_PERMISSIONS_REQUIRED: u32 = 0b11 << 10;
+    pub const PERMISSIONS_REQUIRED: u32 = 0b01 << 10;
+    pub const BASE: u32 = 0x0004000;
+    pub const MASK: u32 = 0x1005;
+}
+
+/// # Start
+pub fn mmu_start(init: u32, mask: u32) {
+    unsafe {
+        core::arch::asm!("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
+    dsb
+
+    // 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", in("r0") init, in("r1") mask);
+    }
+}
+
+/// # Stop
+pub fn mmu_stop() {
+    unsafe {
+        core::arch::asm!(
+            "mrc p15,0,r2,c1,c0,0
+    bic r2,#0x1000
+    bic r2,#0x0004
+    bic r2,#0x0001
+    mcr p15,0,r2,c1,c0,0"
+        );
+    }
+}
+
+pub fn tlb_invalidate() {
+    unsafe {
+        core::arch::asm!(
+            "mov r2, #0
+    // Invalidate Entries
+    mcr p15, 0, r2, c8, c7, 0
+    // DSB
+    mcr p15, 0, r2, c7, c10, 4"
+        );
+    }
+}
+
+pub fn mmu_section(virt: u32, phys: u32, flags: u32) {
+    use crate::cpu::store32;
+    let offset: u32 = virt >> 20;
+    let entry: u32 = MMU::BASE | (offset << 2);
+    let physv: u32 = (phys & 0xFFF00000) | (flags & 0x7FFC) | 0x00C02;
+    store32(entry, physv)
+}
+
+pub fn mmu_init() {
+    let mut addr: u32 = 0;
+    loop {
+        mmu_section(
+            addr,
+            addr,
+            MMU::CACHABLE | MMU::BUFFERABLE | MMU::NO_PERMISSIONS_REQUIRED,
+        );
+        if addr == 0xFFF00000 {
+            break;
+        }
+        addr += 0x00100000;
+    }
+    mmu_section(
+        0x3F200000,
+        0x3F200000,
+        MMU::CACHABLE | MMU::BUFFERABLE | MMU::PERMISSIONS_REQUIRED,
+    );
+    mmu_start(MMU::BASE, MMU::MASK);
+}