From d520e02324619d655860b32f50924ef9f171420c Mon Sep 17 00:00:00 2001
From: Giovanni Bajo <rasky@develer.com>
Date: Sun, 3 Dec 2023 22:31:58 +0100
Subject: [PATCH] n64: add support for RSP unaligned reads/writes to cache
 coherency checks

---
 ares/n64/rsp/debugger.cpp | 28 ++++++++++++++++++++++++++++
 ares/n64/rsp/rsp.hpp      | 22 ++++++++++++++++------
 2 files changed, 44 insertions(+), 6 deletions(-)

diff --git a/ares/n64/rsp/debugger.cpp b/ares/n64/rsp/debugger.cpp
index 6e6437f60d..6bb0afd11a 100644
--- a/ares/n64/rsp/debugger.cpp
+++ b/ares/n64/rsp/debugger.cpp
@@ -146,6 +146,20 @@ auto RSP::Debugger::dmemReadWord(u12 address, int size, const char *peripheral)
   }
 }
 
+auto RSP::Debugger::dmemReadUnalignedWord(u12 address, int size, const char *peripheral) -> void {
+  if (system.homebrewMode) {
+    u32 addressAlignedStart = address            & ~7;
+    u32 addressAlignedEnd   = address + size - 1 & ~7;
+    if(addressAlignedStart == addressAlignedEnd) {
+      dmemReadWord(address, size, "RSP");
+    } else {
+      int sizeStart = addressAlignedEnd - address;
+      dmemReadWord(address,             sizeStart,        "RSP");
+      dmemReadWord(address + sizeStart, size - sizeStart, "RSP");
+    }
+  }
+}
+
 auto RSP::Debugger::dmemWriteWord(u12 address, int size, u64 value) -> void {
   if (system.homebrewMode) {
     auto& taintWord = taintMask.dmem[address >> 3];
@@ -153,4 +167,18 @@ auto RSP::Debugger::dmemWriteWord(u12 address, int size, u64 value) -> void {
   }
 }
 
+auto RSP::Debugger::dmemWriteUnalignedWord(u12 address, int size, u64 value) -> void {
+  if (system.homebrewMode) {
+    u32 addressAlignedStart = address            & ~7;
+    u32 addressAlignedEnd   = address + size - 1 & ~7;
+    if(addressAlignedStart == addressAlignedEnd) {
+      dmemWriteWord(address, size, value);
+    } else {
+      int sizeStart = addressAlignedEnd - address;
+      dmemWriteWord(address,             sizeStart,        value);
+      dmemWriteWord(address + sizeStart, size - sizeStart, value);
+    }
+  }
+}
+
 #undef rsp
diff --git a/ares/n64/rsp/rsp.hpp b/ares/n64/rsp/rsp.hpp
index a69721a028..cb4f2ea45c 100644
--- a/ares/n64/rsp/rsp.hpp
+++ b/ares/n64/rsp/rsp.hpp
@@ -9,20 +9,28 @@ struct RSP : Thread, Memory::RCP<RSP> {
 
     template<u32 Size>
     auto read(u32 address) -> u64 {
-      if (system.homebrewMode) {
-        self.debugger.dmemReadWord(address, Size, "RSP");
-      }
+      if (system.homebrewMode) self.debugger.dmemReadWord(address, Size, "RSP");
       return Memory::Writable::read<Size>(address);
     }
 
+    template<u32 Size>
+    auto readUnaligned(u32 address) -> u64 {
+      if (system.homebrewMode) self.debugger.dmemReadUnalignedWord(address, Size, "RSP");
+      return Memory::Writable::readUnaligned<Size>(address);
+    }
+
     template<u32 Size>
     auto write(u32 address, u64 value) -> void {
-      if (system.homebrewMode) {
-        self.debugger.dmemWriteWord(address, Size, value);
-      }
+      if (system.homebrewMode) self.debugger.dmemWriteWord(address, Size, value);
       Memory::Writable::write<Size>(address, value);
     }
     
+    template<u32 Size>
+    auto writeUnaligned(u32 address, u64 value) -> void {
+      if (system.homebrewMode) self.debugger.dmemWriteUnalignedWord(address, Size, value);
+      Memory::Writable::writeUnaligned<Size>(address, value);
+    }
+
   } dmem{*this};
   Memory::Writable imem;
 
@@ -38,6 +46,8 @@ struct RSP : Thread, Memory::RCP<RSP> {
     auto dmaReadWord(u32 rdramAddress, u32 pbusRegion, u32 pbusAddress) -> void;
     auto dmemReadWord(u12 address, int size, const char *peripheral) -> void;
     auto dmemWriteWord(u12 address, int size, u64 value) -> void;
+    auto dmemReadUnalignedWord(u12 address, int size, const char *peripheral) -> void;
+    auto dmemWriteUnalignedWord(u12 address, int size, u64 value) -> void;
 
     struct TaintMask {
       struct TaintWord {