From a46f972c2179513060fc845faf2cbca418dc2133 Mon Sep 17 00:00:00 2001
From: DesperateProgrammer <tim.seidel@rwth-aachen.de>
Date: Wed, 24 Jan 2024 10:37:17 +0100
Subject: [PATCH] Added Cache Data Write and Read via CP15

---
 src/CP15.cpp | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/src/CP15.cpp b/src/CP15.cpp
index ea3acddf..3de89caa 100644
--- a/src/CP15.cpp
+++ b/src/CP15.cpp
@@ -872,10 +872,22 @@ void ARMv5::CP15Write(u32 id, u32 val)
 
     case 0xF30:
         //printf("cache debug instruction cache %08X\n", val);
+        {
+            uint8_t segment = (CacheDebugRegisterIndex >> (32-ICACHE_SETS_LOG2)) & (ICACHE_SETS-1);
+            uint8_t wordAddress = (CacheDebugRegisterIndex & (ICACHE_LINELENGTH-1)) >> 2;
+            uint8_t index = (CacheDebugRegisterIndex >> ICACHE_LINELENGTH_LOG2) & (ICACHE_LINESPERSET-1);
+            *(u32 *)&ICache[(((index << ICACHE_SETS_LOG2) + segment) << ICACHE_LINELENGTH_LOG2) + wordAddress*4] = val;            
+        }
         return;
 
     case 0xF40:
         //printf("cache debug data cache %08X\n", val);
+        {
+            uint8_t segment = (CacheDebugRegisterIndex >> (32-DCACHE_SETS_LOG2)) & (DCACHE_SETS-1);
+            uint8_t wordAddress = (CacheDebugRegisterIndex & (DCACHE_LINELENGTH-1)) >> 2;
+            uint8_t index = (CacheDebugRegisterIndex >> DCACHE_LINELENGTH_LOG2) & (DCACHE_LINESPERSET-1);
+            *(u32 *)&DCache[((index << DCACHE_SETS_LOG2) + segment) << DCACHE_LINELENGTH_LOG2 + wordAddress*4] = val;            
+        }
         return;
 
     }
@@ -1005,6 +1017,20 @@ u32 ARMv5::CP15Read(u32 id) const
             Log(LogLevel::Debug, "Read DCache Tag %08lx (%u, %02x, %u) -> %08lx\n", CacheDebugRegisterIndex, segment, index, wordAddress, DCacheTags[(index << DCACHE_SETS_LOG2) + segment]);
             return DCacheTags[(index << DCACHE_SETS_LOG2) + segment];
         }
+    case 0xF30:
+        {
+            uint8_t segment = (CacheDebugRegisterIndex >> (32-ICACHE_SETS_LOG2)) & (ICACHE_SETS-1);
+            uint8_t wordAddress = (CacheDebugRegisterIndex & (ICACHE_LINELENGTH-1)) >> 2;
+            uint8_t index = (CacheDebugRegisterIndex >> ICACHE_LINELENGTH_LOG2) & (ICACHE_LINESPERSET-1);
+            return *(u32 *)&ICache[(((index << ICACHE_SETS_LOG2) + segment) << ICACHE_LINELENGTH_LOG2) + wordAddress*4];
+        }
+    case 0xF40:
+        {
+            uint8_t segment = (CacheDebugRegisterIndex >> (32-DCACHE_SETS_LOG2)) & (DCACHE_SETS-1);
+            uint8_t wordAddress = (CacheDebugRegisterIndex & (DCACHE_LINELENGTH-1)) >> 2;
+            uint8_t index = (CacheDebugRegisterIndex >> DCACHE_LINELENGTH_LOG2) & (DCACHE_LINESPERSET-1);
+            return *(u32 *)&DCache[(((index << DCACHE_SETS_LOG2) + segment) << DCACHE_LINELENGTH_LOG2) + wordAddress*4];
+        }
     }
 
     if ((id & 0xF00) == 0xF00) // test/debug shit?