From 5c69bbdad667c261910ddf2096cb81102ed75ff8 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Mon, 22 Jun 2020 02:02:11 +0200
Subject: [PATCH] mmu: fix L1Table.update() flush

---
 libcortex_a9/src/cache.rs | 36 ++++++++++++++++++++++++++++++++++++
 libcortex_a9/src/mmu.rs   | 23 ++++++++++++++++-------
 2 files changed, 52 insertions(+), 7 deletions(-)

diff --git a/libcortex_a9/src/cache.rs b/libcortex_a9/src/cache.rs
index c520252..3ea95b4 100644
--- a/libcortex_a9/src/cache.rs
+++ b/libcortex_a9/src/cache.rs
@@ -44,6 +44,15 @@ pub fn dcisw(setway: u32) {
     }
 }
 
+/// Data cache clean by set/way
+#[inline(always)]
+pub fn dccisw(setway: u32) {
+    unsafe {
+        llvm_asm!("mcr p15, 0, $0, c7, c14, 2" :: "r" (setway) :: "volatile");
+    }
+}
+
+
 /// A made-up "instruction": invalidate all of the L1 D-Cache
 #[inline(always)]
 pub fn dciall() {
@@ -71,6 +80,33 @@ pub fn dciall() {
     }
 }
 
+/// A made-up "instruction": flush and invalidate all of the L1 D-Cache
+#[inline(always)]
+pub fn dcciall() {
+    // the cache associativity could be read from a register, but will
+    // always be 4 in L1 data cache of a cortex a9
+    let ways = 4;
+    let bit_pos_of_way = 30; // 32 - log2(ways)
+
+    // the cache sets could be read from a register, but are always
+    // 256 for the cores in the zync-7000; in general, 128 or 512 are
+    // also possible.
+    let sets = 256;
+    let bit_pos_of_set = 5; // for a line size of 8 words = 2^5 bytes
+
+    // select L1 data cache
+    unsafe {
+        llvm_asm!("mcr p15, 2, $0, c0, c0, 0" :: "r" (0) :: "volatile");
+    }
+
+    // Invalidate entire D-Cache by iterating every set and every way
+    for set in 0..sets {
+        for way in 0..ways {
+            dccisw((set << bit_pos_of_set) | (way << bit_pos_of_way));
+        }
+    }
+}
+
 
 const CACHE_LINE: usize = 0x20;
 const CACHE_LINE_MASK: usize = CACHE_LINE - 1;
diff --git a/libcortex_a9/src/mmu.rs b/libcortex_a9/src/mmu.rs
index a744b5f..151386b 100644
--- a/libcortex_a9/src/mmu.rs
+++ b/libcortex_a9/src/mmu.rs
@@ -1,5 +1,5 @@
 use bit_field::BitField;
-use super::{regs::*, asm, cache};
+use super::{regs::*, asm::*, cache::*};
 use libregister::RegisterW;
 
 #[derive(Copy, Clone)]
@@ -368,10 +368,19 @@ impl L1Table {
         let result = f(&mut section);
         entry.set_section(section);
 
-        asm::dmb();
-        cache::tlbiall();
-        asm::dsb();
-        asm::isb();
+        // Flush L1Dcache
+        dcciall();
+        // // TODO: L2?
+
+        // Invalidate TLB
+        tlbiall();
+        // Invalidate all branch predictors
+        bpiall();
+
+        // ensure completion of the BP and TLB invalidation
+        dsb();
+        // synchronize context on this processor
+        isb();
 
         result
     }
@@ -406,9 +415,9 @@ pub fn with_mmu<F: FnMut() -> !>(l1table: &L1Table, mut f: F) -> ! {
 
     // Synchronization barriers
     // Allows MMU to start
-    asm::dsb();
+    dsb();
     // Flushes pre-fetch buffer
-    asm::isb();
+    isb();
 
     f();
 }