2 files changed, 117 insertions, 0 deletions
diff --git a/src/main.rs b/src/main.rs
index 615a965..a4cf598 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -10,6 +10,7 @@ mod globals;
 mod memory_tracker;
 mod shareable_cache;
 mod system;
+mod trapped_cache;
 
 use std::fs::File;
 use std::io::Read;
diff --git a/src/trapped_cache.rs b/src/trapped_cache.rs
new file mode 100644
index 0000000..690a734
--- /dev/null
+++ b/src/trapped_cache.rs
@@ -0,0 +1,116 @@
+use rustv::isa;
+use rustv::memory::{MemoryError, MemoryInterface, Result};
+
+pub const WRITE_TRAP_VALUE: u8 = 0x42;
+pub const AREA_TRAP_VALUE: u8 = 0x43;
+
+pub const WRITE_TRAP_STALL: MemoryError = MemoryError::CacheMiss {
+    stall_cycles: 10000
+};
+
+/// A cache that stalls when a trap is triggered.
+pub struct TrappedCache<T: MemoryInterface> {
+    cache: T,
+    half: isa::Address,
+}
+
+// TODO: move this t
+enum WordOffset {
+    Byte0,
+    Byte1,
+    Byte2,
+    Byte3,
+}
+
+impl<T: MemoryInterface> TrappedCache<T> {
+    pub fn new(cache: T, half: isa::Address) -> TrappedCache<T> {
+        TrappedCache {
+            cache: cache,
+            half: half,
+        }
+    }
+
+    fn clear_trap(&mut self, address: isa::Address, offset: WordOffset) {
+        // Precondition: address is in the cache and can be
+        // read/written without stalling
+        match self.read_word(address) {
+            Ok(value) => {
+                let new_value = match offset {
+                    WordOffset::Byte0 => value & 0xFFFFFF00,
+                    WordOffset::Byte1 => value & 0xFFFF00FF,
+                    WordOffset::Byte2 => value & 0xFF00FFFF,
+                    WordOffset::Byte3 => value & 0x00FFFFFF,
+                };
+
+                match self.write_word(address, new_value) {
+                    Ok(()) => {},
+                    Err(e) => panic!("{:?}", e),
+                }
+            },
+            Err(e) => panic!("{:?}", e),
+        }
+    }
+}
+
+impl<T: MemoryInterface> MemoryInterface for TrappedCache<T> {
+    fn latency(&self) -> u32 {
+        self.cache.latency()
+    }
+
+    fn step(&mut self) {
+        self.cache.step();
+    }
+
+    fn is_address_accessible(&self, address: isa::Address) -> bool {
+        self.cache.is_address_accessible(address)
+    }
+
+    fn read_word(&mut self, address: isa::Address) -> Result<isa::Word> {
+        self.cache.read_word(address)
+    }
+
+    fn write_word(&mut self, address: isa::Address,
+                  value: isa::Word) -> Result<()> {
+        if self.is_address_accessible(address) {
+            // No stall - check for trap
+            let old_value = self.read_word(address);
+
+            match old_value {
+                Ok(old_value) => {
+                    // TODO: helper method on isa::Word (would require
+                    // newtype) for accessing individual bytes?
+                    if ((old_value & 0xFF) as u8) == WRITE_TRAP_VALUE &&
+                        ((value & 0xFF) as u8) != WRITE_TRAP_VALUE {
+                            self.clear_trap(old_value, WordOffset::Byte0);
+                            Err(WRITE_TRAP_STALL)
+                    }
+                    else if (((old_value >> 8) & 0xFF) as u8) == WRITE_TRAP_VALUE &&
+                        (((value >> 8) & 0xFF) as u8) == WRITE_TRAP_VALUE {
+                            self.clear_trap(old_value, WordOffset::Byte1);
+                            Err(WRITE_TRAP_STALL)
+                    }
+                    else if (((old_value >> 16) & 0xFF) as u8) == WRITE_TRAP_VALUE &&
+                        (((value >> 16) & 0xFF) as u8) == WRITE_TRAP_VALUE {
+                            self.clear_trap(old_value, WordOffset::Byte2);
+                            Err(WRITE_TRAP_STALL)
+                    }
+                    else if (((old_value >> 24) & 0xFF) as u8) == WRITE_TRAP_VALUE &&
+                        (((value >> 24) & 0xFF) as u8) == WRITE_TRAP_VALUE {
+                            self.clear_trap(old_value, WordOffset::Byte3);
+                            Err(WRITE_TRAP_STALL)
+                    }
+                    else {
+                        self.cache.write_word(address, value)
+                    }
+                }
+                Err(e) => {
+                    panic!("Could not read accessible value: {:?}", e)
+                }
+            }
+        }
+        else {
+            // Not in cache - defer to fetch
+            self.cache.write_word(address, value)
+        }
+    }
+}