From 1033648c3ebb605612dd0cf7bf1a9d53883232e8 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Thu, 23 May 2019 19:05:06 +0200
Subject: [PATCH] add l1_cache_init()

---
 src/cortex_a9/asm.rs  | 18 ++++++++++++++++++
 src/cortex_a9/regs.rs | 36 ++++++++++++++++++++++++++++++++++++
 src/main.rs           | 27 +++++++++++++++++++++++++--
 3 files changed, 79 insertions(+), 2 deletions(-)

diff --git a/src/cortex_a9/asm.rs b/src/cortex_a9/asm.rs
index f69b197..022530e 100644
--- a/src/cortex_a9/asm.rs
+++ b/src/cortex_a9/asm.rs
@@ -9,3 +9,21 @@ pub fn nop() {
 pub fn wfe() {
     unsafe { asm!("wfe" :::: "volatile") }
 }
+
+/// Data Memory Barrier
+#[inline]
+pub fn dmb() {
+    unsafe { asm!("dmb" :::: "volatile") }
+}
+
+/// Data Synchronization Barrier
+#[inline]
+pub fn dsb() {
+    unsafe { asm!("dsb" :::: "volatile") }
+}
+
+/// Instruction Synchronization Barrier
+#[inline]
+pub fn isb() {
+    unsafe { asm!("isb" :::: "volatile") }
+}
diff --git a/src/cortex_a9/regs.rs b/src/cortex_a9/regs.rs
index 498e087..203d328 100644
--- a/src/cortex_a9/regs.rs
+++ b/src/cortex_a9/regs.rs
@@ -36,3 +36,39 @@ def_reg_set!(SP, u32, "mov sp, $0");
 
 pub struct MPIDR;
 def_reg_get!(MPIDR, u32, "mrc p15, 0, $0, c0, c0, 5");
+
+/// Invalidate TLBs
+pub fn tlbiall() {
+    unsafe {
+        asm!("mcr p15, 0, $0, c8, c7, 0" :: "r" (0) :: "volatile");
+    }
+}
+
+/// Invalidate I-Cache
+pub fn iciallu() {
+    unsafe {
+        asm!("mcr p15, 0, $0, c7, c5, 0" :: "r" (0) :: "volatile");
+    }
+}
+
+/// Invalidate Branch Predictor Array
+pub fn bpiall() {
+    unsafe {
+        asm!("mcr p15, 0, $0, c7, c5, 6" :: "r" (0) :: "volatile");
+    }
+}
+
+/// Invalidate D-Cache
+pub fn dccisw() {
+    // TODO: $0 is r11 at what value?
+    unsafe {
+        asm!("mcr p15, 0, $0, c7, c5, 6" :: "r" (0) :: "volatile");
+    }
+}
+
+/// Enable I-Cache and D-Cache
+pub fn sctlr() {
+    unsafe {
+        asm!("mcr p15, 0, $0, c1, c0, 0" :: "r" (0x1004) :: "volatile");
+    }
+}
diff --git a/src/main.rs b/src/main.rs
index b873ccf..7339ccb 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -15,6 +15,8 @@ mod uart;
 use uart::Uart;
 mod eth;
 
+use crate::cortex_a9::{asm, regs::*};
+
 extern "C" {
     static mut __bss_start: u32;
     static mut __bss_end: u32;
@@ -25,8 +27,6 @@ extern "C" {
 #[no_mangle]
 #[naked]
 pub unsafe extern "C" fn _boot_cores() -> ! {
-    use cortex_a9::{asm, regs::*};
-
     const CORE_MASK: u32 = 0x3;
     let stack_start = __end + 4096;
 
@@ -43,11 +43,34 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
 }
 
 unsafe fn boot_core0() -> ! {
+    l1_cache_init();
     zero_bss(&mut __bss_start, &mut __bss_end);
     main();
     panic!("return from main");
 }
 
+fn l1_cache_init() {
+    // Invalidate TLBs
+    tlbiall();
+    // Invalidate I-Cache
+    iciallu();
+    // Invalidate Branch Predictor Array
+    bpiall();
+    // Invalidate D-Cache
+    dccisw();
+
+    // (Initialize MMU)
+
+    // Enable I-Cache and D-Cache
+    sctlr();
+
+    // Synchronization barriers
+    // Allows MMU to start
+    asm::dsb();
+    // Flushes pre-fetch buffer
+    asm::isb();
+}
+
 fn main() {
     let mut uart = Uart::uart1(115_200);
     loop {