From 1b5a1a87aaf4b51094b21579017dc7a8af4dfb77 Mon Sep 17 00:00:00 2001
From: Berkus Decker <berkus+github@metta.systems>
Date: Sun, 20 Jan 2019 17:41:53 +0200
Subject: [PATCH] Add some paging assembly code

* [fixme - replace with cortex-a]
---
 src/arch/aarch64/mod.rs | 58 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 58 insertions(+)

diff --git a/src/arch/aarch64/mod.rs b/src/arch/aarch64/mod.rs
index c735f5d..fe6dd39 100644
--- a/src/arch/aarch64/mod.rs
+++ b/src/arch/aarch64/mod.rs
@@ -53,3 +53,61 @@ pub fn loop_until<F: Fn() -> bool>(f: F) {
         asm::nop();
     }
 }
+
+pub fn read_translation_table_base() -> u64 {
+    let mut base: u64 = 0;
+    unsafe {
+        asm!("mrs $0, ttbr0_el1" : "=r"(base) ::: "volatile");
+    }
+    return base;
+}
+
+pub fn read_translation_control() -> u64 {
+    let mut tcr: u64 = 0;
+    unsafe {
+        asm!("mrs $0, tcr_el1" : "=r"(tcr) ::: "volatile");
+    }
+    return tcr;
+}
+
+pub fn read_mair() -> u64 {
+    let mut mair: u64 = 0;
+    unsafe {
+        asm!("mrs $0, mair_el1" : "=r"(mair) ::: "volatile");
+    }
+    return mair;
+}
+
+pub fn write_translation_table_base(base: usize) {
+    unsafe {
+        asm!("msr ttbr0_el1, $0" :: "r"(base) :: "volatile");
+    }
+}
+
+// Helper function similar to u-boot
+pub fn write_ttbr_tcr_mair(el: u8, base: u64, tcr: u64, attr: u64) {
+    unsafe {
+        asm!("dsb sy" :::: "volatile");
+    }
+    match (el) {
+        1 => unsafe {
+            asm!("msr ttbr0_el1, $0
+                msr tcr_el1, $1
+                msr mair_el1, $2" :: "r"(base), "r"(tcr), "r"(attr) : "memory" : "volatile");
+        },
+        2 => unsafe {
+            asm!("msr ttbr0_el2, $0
+                msr tcr_el2, $1
+                msr mair_el2, $2" :: "r"(base), "r"(tcr), "r"(attr) : "memory" : "volatile");
+        },
+        3 => unsafe {
+            asm!("msr ttbr0_el3, $0
+                msr tcr_el3, $1
+                msr mair_el3, $2" :: "r"(base), "r"(tcr), "r"(attr) : "memory" : "volatile");
+        },
+        _ => loop {},
+    }
+    unsafe {
+        asm!("isb" :::: "volatile");
+    }
+}