65816: Cache instruction fetch results in LRU cache

Cargo.toml

@@ -13,6 +13,7 @@ colored = "2.0.4"
 dbg_hex = "0.1.1"
 hex-literal = "0.4.1"
 itertools = "0.11.0"
+lrumap = "0.1.0"
 num = "0.4.1"
 num-derive = "0.4.0"
 num-traits = "0.2.16"

src/snes/cpu_65816/cpu.rs

@@ -1,5 +1,6 @@
 use anyhow::Result;
 use arrayvec::ArrayVec;
+use lrumap::LruBTreeMap;
 use num_traits::ToPrimitive;
 use serde::{Deserialize, Serialize};
 
@@ -10,13 +11,24 @@ use super::alu;
 use super::instruction::{AddressingMode, Instruction, InstructionType, MAX_INSTRUCTION_LEN};
 use super::regs::{Flag, Register, RegisterFile, RegisterWidth};
 
+type InstrCacheKey = (Address, bool, bool); // Address, M, X
+type InstrCache = LruBTreeMap<InstrCacheKey, Instruction>;
+const INSTRCACHE_SIZE: usize = 100000;
+
+fn _clean_cache() -> InstrCache {
+    InstrCache::new(INSTRCACHE_SIZE)
+}
+
 /// Main SNES CPU (65816)
 #[derive(Serialize, Deserialize)]
 pub struct Cpu65816<TBus: Bus<Address>> {
     pub bus: TBus,
     pub regs: RegisterFile,
     pub cycles: Ticks,
     pub wait_for_int: bool,
+
+    #[serde(skip, default = "_clean_cache")]
+    instr_cache: InstrCache,
 }
 
 impl<TBus> Cpu65816<TBus>
@@ -34,6 +46,7 @@ where
             regs: RegisterFile::new(),
             cycles: 0,
             wait_for_int: false,
+            instr_cache: _clean_cache(),
         };
         cpu.regs.pc = reset_addr;
         cpu.regs.p = (1 << Flag::M.to_u8().unwrap()) | (1 << Flag::X.to_u8().unwrap());
@@ -51,6 +64,22 @@ where
         )
     }
 
+    /// Invalidates an entry in the instruction cache
+    fn instrcache_invalidate(&mut self, addr: Address) {
+        self.instr_cache
+            .entry(&(addr, false, false))
+            .and_then(|e| Some(e.take()));
+        self.instr_cache
+            .entry(&(addr, false, true))
+            .and_then(|e| Some(e.take()));
+        self.instr_cache
+            .entry(&(addr, true, false))
+            .and_then(|e| Some(e.take()));
+        self.instr_cache
+            .entry(&(addr, true, true))
+            .and_then(|e| Some(e.take()));
+    }
+
     /// Fetches and decodes the next instruction at PC
     pub fn peek_next_instr(&self) -> Result<Instruction> {
         let mut busiter = BusIterator::new_from(&self.bus, self.regs.get_full_pc());
@@ -63,21 +92,32 @@ where
 
     /// Fetches and decodes the next instruction at PC
     pub fn fetch_next_instr(&mut self) -> Result<Instruction> {
-        let mut fetched: ArrayVec<u8, MAX_INSTRUCTION_LEN> = ArrayVec::new();
+        let pc = self.regs.get_full_pc();
+        let (m, x) = (self.regs.test_flag(Flag::M), self.regs.test_flag(Flag::X));
+
+        if let Some(instr) = self.instr_cache.get(&(pc, m, x)) {
+            // Instruction cache hit
+            let i = instr.clone();
+            self.tick_bus(i.len)?;
+            return Ok(i);
+        }
 
-        for p in 0.. {
+        // Instruction cache miss
+        let mut fetched: ArrayVec<u8, MAX_INSTRUCTION_LEN> = ArrayVec::new();
+        let mut p = 0;
+        let instr = loop {
             let pc = (self.regs.k as Address) << 16 | self.regs.pc.wrapping_add(p) as Address;
-            match Instruction::decode(
-                &mut fetched.clone().into_iter(),
-                self.regs.test_flag(Flag::M),
-                self.regs.test_flag(Flag::X),
-            ) {
+            p += 1;
+
+            match Instruction::decode(&mut fetched.clone().into_iter(), m, x) {
                 Err(_) => fetched.push(self.read_tick(pc)),
-                Ok(i) => return Ok(i),
+                Ok(i) => break i,
             }
-        }
+        };
+
+        self.instr_cache.push((pc, m, x), instr.clone());
 
-        unreachable!()
+        Ok(instr)
     }
 
     /// Executes one CPU step (one instruction).
@@ -161,6 +201,9 @@ where
     /// Writes a memory location while ticking peripherals
     /// for the access time.
     fn write_tick(&mut self, addr: Address, val: u8) {
+        // Invalidate instruction cache for this address
+        self.instrcache_invalidate(addr);
+
         self.bus.write(addr, val);
         self.tick_bus(1).unwrap();
     }

src/snes/cpu_65816/instruction.rs

@@ -252,6 +252,7 @@ pub struct InstructionDef {
 }
 
 /// A decoded instruction
+#[derive(Clone)]
 pub struct Instruction {
     /// Reference to definition in instruction table
     pub def: &'static InstructionDef,