From 0726095f00f9e230890ea9c8c38bc5de51e256b8 Mon Sep 17 00:00:00 2001
From: Connor McLaughlin <mclaughc@outlook.com>
Date: Sat, 14 Sep 2019 13:52:57 +1000
Subject: [PATCH] CPU: Implement fixed dcache/scratchpad

---
 src/pse/cpu_core.h   | 13 ++++++++++-
 src/pse/cpu_core.inl | 53 ++++++++++++++++++++++++++++++++++++++++++--
 2 files changed, 63 insertions(+), 3 deletions(-)

diff --git a/src/pse/cpu_core.h b/src/pse/cpu_core.h
index a5255b171..d5b781875 100644
--- a/src/pse/cpu_core.h
+++ b/src/pse/cpu_core.h
@@ -2,6 +2,7 @@
 #include "common/bitfield.h"
 #include "cpu_types.h"
 #include "types.h"
+#include <array>
 
 class StateWrapper;
 
@@ -12,7 +13,11 @@ namespace CPU {
 class Core
 {
 public:
-  static constexpr VirtualMemoryAddress RESET_VECTOR = 0xbfc00000;
+  static constexpr VirtualMemoryAddress RESET_VECTOR = UINT32_C(0xBFC00000);
+  static constexpr PhysicalMemoryAddress DCACHE_LOCATION = UINT32_C(0x1F800000);
+  static constexpr PhysicalMemoryAddress DCACHE_LOCATION_MASK = UINT32_C(0xFFFFFC00);
+  static constexpr PhysicalMemoryAddress DCACHE_OFFSET_MASK = UINT32_C(0x000003FF);
+  static constexpr PhysicalMemoryAddress DCACHE_SIZE = UINT32_C(0x00000400);
 
   Core();
   ~Core();
@@ -40,6 +45,9 @@ private:
   template<MemoryAccessType type, MemoryAccessSize size, bool is_instruction_fetch, bool raise_exceptions>
   bool DoMemoryAccess(VirtualMemoryAddress address, u32& value);
 
+  template<MemoryAccessType type, MemoryAccessSize size>
+  void DoScratchpadAccess(PhysicalMemoryAddress address, u32& value);
+
   u8 ReadMemoryByte(VirtualMemoryAddress addr);
   u16 ReadMemoryHalfWord(VirtualMemoryAddress addr);
   u32 ReadMemoryWord(VirtualMemoryAddress addr);
@@ -88,6 +96,9 @@ private:
   u32 m_cache_control = 0;
 
   Cop0Registers m_cop0_regs = {};
+
+  // data cache (used as scratchpad)
+  std::array<u8, DCACHE_SIZE> m_dcache = {};
 };
 
 extern bool TRACE_EXECUTION;
diff --git a/src/pse/cpu_core.inl b/src/pse/cpu_core.inl
index 68a1caecf..69cd3d954 100644
--- a/src/pse/cpu_core.inl
+++ b/src/pse/cpu_core.inl
@@ -18,7 +18,14 @@ bool Core::DoMemoryAccess(VirtualMemoryAddress address, u32& value)
           return true;
       }
 
-      if (!m_bus->DispatchAccess<type, size>(address, address & UINT32_C(0x1FFFFFFF), value))
+      const PhysicalMemoryAddress phys_addr = address & UINT32_C(0x1FFFFFFF);
+      if ((phys_addr & DCACHE_LOCATION_MASK) == DCACHE_LOCATION)
+      {
+        DoScratchpadAccess<type, size>(phys_addr, value);
+        return true;
+      }
+
+      if (!m_bus->DispatchAccess<type, size>(address, phys_addr, value))
       {
         Panic("Bus error");
         return false;
@@ -44,6 +51,13 @@ bool Core::DoMemoryAccess(VirtualMemoryAddress address, u32& value)
           return true;
       }
 
+      const PhysicalMemoryAddress phys_addr = address & UINT32_C(0x1FFFFFFF);
+      if ((phys_addr & DCACHE_LOCATION_MASK) == DCACHE_LOCATION)
+      {
+        DoScratchpadAccess<type, size>(phys_addr, value);
+        return true;
+      }
+
       if (!m_bus->DispatchAccess<type, size>(address, address & UINT32_C(0x1FFFFFFF), value))
       {
         Panic("Bus error");
@@ -56,7 +70,8 @@ bool Core::DoMemoryAccess(VirtualMemoryAddress address, u32& value)
 
     case 0x05: // KSEG1 - physical memory uncached
     {
-      if (!m_bus->DispatchAccess<type, size>(address, address & UINT32_C(0x1FFFFFFF), value))
+      const PhysicalMemoryAddress phys_addr = address & UINT32_C(0x1FFFFFFF);
+      if (!m_bus->DispatchAccess<type, size>(address, phys_addr, value))
       {
         Panic("Bus error");
         return false;
@@ -91,4 +106,38 @@ bool Core::DoMemoryAccess(VirtualMemoryAddress address, u32& value)
   }
 }
 
+template<MemoryAccessType type, MemoryAccessSize size>
+void CPU::Core::DoScratchpadAccess(PhysicalMemoryAddress address, u32& value)
+{
+  const PhysicalMemoryAddress cache_offset = address & DCACHE_OFFSET_MASK;
+  if constexpr (size == MemoryAccessSize::Byte)
+  {
+    if constexpr (type == MemoryAccessType::Read)
+      value = ZeroExtend32(m_dcache[cache_offset]);
+    else
+      m_dcache[cache_offset] = Truncate8(value);
+  }
+  else if constexpr (size == MemoryAccessSize::HalfWord)
+  {
+    if constexpr (type == MemoryAccessType::Read)
+    {
+      u16 temp;
+      std::memcpy(&temp, &m_dcache[cache_offset], sizeof(temp));
+      value = ZeroExtend32(temp);
+    }
+    else
+    {
+      u16 temp = Truncate16(value);
+      std::memcpy(&m_dcache[cache_offset], &temp, sizeof(temp));
+    }
+  }
+  else if constexpr (size == MemoryAccessSize::Word)
+  {
+    if constexpr (type == MemoryAccessType::Read)
+      std::memcpy(&value, &m_dcache[cache_offset], sizeof(value));
+    else
+      std::memcpy(&m_dcache[cache_offset], &value, sizeof(value));
+  }
+}
+
 } // namespace CPU
\ No newline at end of file