From 87263bcbae4b418276f28ac3408dc7cf516f5857 Mon Sep 17 00:00:00 2001
From: Thomas <thomas@thomasw.dev>
Date: Sun, 17 Dec 2023 13:32:06 +0100
Subject: [PATCH] 65816: Cache instruction fetch results in LRU cache

---
 Cargo.lock                        |  7 ++++
 Cargo.toml                        |  1 +
 src/snes/cpu_65816/cpu.rs         | 63 ++++++++++++++++++++++++++-----
 src/snes/cpu_65816/instruction.rs |  1 +
 4 files changed, 62 insertions(+), 10 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index c25ff34..1d8589c 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -323,6 +323,12 @@ version = "0.4.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "969488b55f8ac402214f3f5fd243ebb7206cf82de60d3172994707a4bcc2b829"
 
+[[package]]
+name = "lrumap"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1251ce8e8f9909600e127dcbe74ac50d8464e6685cf5953d37df7a741dbf9e9d"
+
 [[package]]
 name = "memchr"
 version = "2.6.4"
@@ -570,6 +576,7 @@ dependencies = [
  "dbg_hex",
  "hex-literal",
  "itertools",
+ "lrumap",
  "num",
  "num-derive",
  "num-traits",
diff --git a/Cargo.toml b/Cargo.toml
index 188653c..8f13b20 100644
--- a/Cargo.toml
+++ b/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"
diff --git a/src/snes/cpu_65816/cpu.rs b/src/snes/cpu_65816/cpu.rs
index a401893..3cea0c4 100644
--- a/src/snes/cpu_65816/cpu.rs
+++ b/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,6 +11,14 @@ 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>> {
@@ -17,6 +26,9 @@ pub struct Cpu65816<TBus: Bus<Address>> {
     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();
     }
diff --git a/src/snes/cpu_65816/instruction.rs b/src/snes/cpu_65816/instruction.rs
index e6e4d5c..c9143ec 100644
--- a/src/snes/cpu_65816/instruction.rs
+++ b/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,