From 701c0de18a366dc0ca7c83594d6629ab86699970 Mon Sep 17 00:00:00 2001
From: CocoSimone <simococo03@gmail.com>
Date: Tue, 16 Aug 2022 11:48:12 +0200
Subject: [PATCH] Fixed endianness for parallel-rdp

---
 src/common.hpp                |  4 +-
 src/frontend/App.hpp          |  3 ++
 src/frontend/imgui/Window.cpp |  2 +-
 src/main.cpp                  |  3 ++
 src/n64/Core.hpp              |  2 +-
 src/n64/core/Cpu.cpp          |  8 +--
 src/n64/core/Mem.cpp          | 98 ++++++++++++++++++++++++++++++-----
 src/n64/core/Mem.hpp          |  4 +-
 src/n64/core/RDP.cpp          |  2 +
 src/n64/core/RDP.hpp          |  4 +-
 src/n64/core/mmio/PI.cpp      |  8 ++-
 src/n64/core/mmio/SI.cpp      |  5 +-
 src/util.hpp                  | 89 ++++++++++++++++++++-----------
 13 files changed, 172 insertions(+), 60 deletions(-)

diff --git a/src/common.hpp b/src/common.hpp
index 52edf2ad..9d80b082 100644
--- a/src/common.hpp
+++ b/src/common.hpp
@@ -25,4 +25,6 @@ using m128 = __m128i;
 #define MiB ((KiB) * 1024)
 #define GiB ((MiB) * 1024)
 #define N64_CPU_FREQ 93750000
-#define N64_CYCLES_PER_FRAME ((N64_CPU_FREQ) / 60)
\ No newline at end of file
+#define N64_CYCLES_PER_FRAME ((N64_CPU_FREQ) / 60)
+#define HALF_ADDRESS(addr) ((addr) ^ 2)
+#define BYTE_ADDRESS(addr) ((addr) ^ 3)
\ No newline at end of file
diff --git a/src/frontend/App.hpp b/src/frontend/App.hpp
index 6e134ddc..eb2c3774 100644
--- a/src/frontend/App.hpp
+++ b/src/frontend/App.hpp
@@ -4,6 +4,9 @@
 struct App {
   App() : window(core) {};
   void Run();
+  inline void LoadROM(const std::string& path) {
+    core.LoadROM(path);
+  }
 private:
   n64::Core core;
   Window window;
diff --git a/src/frontend/imgui/Window.cpp b/src/frontend/imgui/Window.cpp
index e2c921dc..3bf1a2c9 100644
--- a/src/frontend/imgui/Window.cpp
+++ b/src/frontend/imgui/Window.cpp
@@ -6,7 +6,7 @@
 
 Window::Window(n64::Core& core) {
   InitSDL();
-  InitParallelRDP(core.GetRDRAM(), window);
+  InitParallelRDP(core.mem.GetRDRAM(), window);
   //InitImgui();
   NFD::Init();
 }
diff --git a/src/main.cpp b/src/main.cpp
index 22d3dcea..21dacc5d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -3,6 +3,9 @@
 
 int main(int argc, char* argv[]) {
   App app;
+  if(argc > 1) {
+    app.LoadROM(argv[1]);
+  }
   app.Run();
   return 0;
 }
\ No newline at end of file
diff --git a/src/n64/Core.hpp b/src/n64/Core.hpp
index 9a1db540..0f4db52d 100644
--- a/src/n64/Core.hpp
+++ b/src/n64/Core.hpp
@@ -14,9 +14,9 @@ struct Core {
   void Run(Window&);
   void PollInputs(SDL_Event);
   VI& GetVI() { return mem.mmio.vi; }
-  u8* GetRDRAM() { return mem.rdram.data(); }
   bool romLoaded = false;
 private:
+  friend struct ::Window;
   Mem mem;
   Cpu cpu;
 };
diff --git a/src/n64/core/Cpu.cpp b/src/n64/core/Cpu.cpp
index cf58b720..ff9b3e84 100644
--- a/src/n64/core/Cpu.cpp
+++ b/src/n64/core/Cpu.cpp
@@ -118,14 +118,8 @@ void Cpu::Step(Mem& mem) {
 
   CheckCompareInterrupt(mem.mmio.mi, regs);
 
-  static int count = 0;
-  if(regs.gpr[30] == -1 && count < 1) {
-    util::logdebug("Test passed!\n");
-    count++;
-  }
-
   u32 instruction = mem.Read<u32>(regs, regs.pc, regs.pc);
-  //LogInstruction(instruction);
+  LogInstruction(instruction);
 
   HandleInterrupt(regs);
 
diff --git a/src/n64/core/Mem.cpp b/src/n64/core/Mem.cpp
index c268c03e..ba6c07e9 100644
--- a/src/n64/core/Mem.cpp
+++ b/src/n64/core/Mem.cpp
@@ -11,9 +11,7 @@ Mem::Mem() {
 }
 
 void Mem::Reset() {
-  rdram.resize(RDRAM_SIZE);
   sram.resize(SRAM_SIZE);
-  std::fill(rdram.begin(), rdram.end(), 0);
   std::fill(sram.begin(), sram.end(), 0);
   romMask = 0;
   mmio.Reset();
@@ -38,7 +36,7 @@ void Mem::LoadROM(const std::string& filename) {
   cart.insert(cart.begin(), std::istream_iterator<u8>(file), std::istream_iterator<u8>());
 
   file.close();
-  util::SwapN64Rom(size, cart.data());
+  util::SwapN64Rom(sizeAdjusted, cart.data());
   memcpy(mmio.rsp.dmem, cart.data(), 0x1000);
 }
 
@@ -68,14 +66,56 @@ T Mem::Read(Registers& regs, u32 vaddr, s64 pc) {
   }
 
   switch(paddr) {
-    case 0x00000000 ... 0x007FFFFF: return util::ReadAccess<T>(rdram.data(), paddr & RDRAM_DSIZE);
-    case 0x04000000 ... 0x04000FFF: return util::ReadAccess<T>(mmio.rsp.dmem, paddr & DMEM_DSIZE);
-    case 0x04001000 ... 0x04001FFF: return util::ReadAccess<T>(mmio.rsp.imem, paddr & IMEM_DSIZE);
+    case 0x00000000 ... 0x007FFFFF:
+      if constexpr (sizeof(T) == 1) {
+        return util::ReadAccess<T>(mmio.rdp.dram.data(), BYTE_ADDRESS(paddr) & RDRAM_DSIZE);
+      } else if constexpr (sizeof(T) == 2) {
+        return util::ReadAccess<T>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr) & RDRAM_DSIZE);
+      } else {
+        return util::ReadAccess<T>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE);
+      }
+    case 0x04000000 ... 0x04000FFF:
+      if constexpr (sizeof(T) == 1) {
+        return util::ReadAccess<T>(mmio.rsp.dmem, BYTE_ADDRESS(paddr) & DMEM_DSIZE);
+      } else if constexpr (sizeof(T) == 2) {
+        return util::ReadAccess<T>(mmio.rsp.dmem, HALF_ADDRESS(paddr) & DMEM_DSIZE);
+      } else {
+        return util::ReadAccess<T>(mmio.rsp.dmem, paddr & DMEM_DSIZE);
+      }
+    case 0x04001000 ... 0x04001FFF:
+      if constexpr (sizeof(T) == 1) {
+        return util::ReadAccess<T>(mmio.rsp.imem, BYTE_ADDRESS(paddr) & IMEM_DSIZE);
+      } else if constexpr (sizeof(T) == 2) {
+        return util::ReadAccess<T>(mmio.rsp.imem, HALF_ADDRESS(paddr) & IMEM_DSIZE);
+      } else {
+        return util::ReadAccess<T>(mmio.rsp.imem, paddr & IMEM_DSIZE);
+      }
     case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
     case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF: return mmio.Read(paddr);
-    case 0x10000000 ... 0x1FBFFFFF: return util::ReadAccess<T>(cart.data(), paddr & romMask);
-    case 0x1FC00000 ... 0x1FC007BF: return util::ReadAccess<T>(pifBootrom, paddr & PIF_BOOTROM_DSIZE);
-    case 0x1FC007C0 ... 0x1FC007FF: return util::ReadAccess<T>(pifRam, paddr & PIF_RAM_DSIZE);
+    case 0x10000000 ... 0x1FBFFFFF:
+      if constexpr (sizeof(T) == 1) {
+        return util::ReadAccess<T>(cart.data(), BYTE_ADDRESS(paddr) & romMask);
+      } else if constexpr (sizeof(T) == 2) {
+        return util::ReadAccess<T>(cart.data(), HALF_ADDRESS(paddr) & romMask);
+      } else {
+        return util::ReadAccess<T>(cart.data(), paddr & romMask);
+      }
+    case 0x1FC00000 ... 0x1FC007BF:
+      if constexpr (sizeof(T) == 1) {
+        return util::ReadAccess<T>(pifBootrom, BYTE_ADDRESS(paddr) & PIF_BOOTROM_DSIZE);
+      } else if constexpr (sizeof(T) == 2) {
+        return util::ReadAccess<T>(pifBootrom, HALF_ADDRESS(paddr) & PIF_BOOTROM_DSIZE);
+      } else {
+        return util::ReadAccess<T>(pifBootrom, paddr & PIF_BOOTROM_DSIZE);
+      }
+    case 0x1FC007C0 ... 0x1FC007FF:
+      if constexpr (sizeof(T) == 1) {
+        return util::ReadAccess<T>(pifRam, BYTE_ADDRESS(paddr) & PIF_RAM_DSIZE);
+      } else if constexpr (sizeof(T) == 2) {
+        return util::ReadAccess<T>(pifRam, HALF_ADDRESS(paddr) & PIF_RAM_DSIZE);
+      } else {
+        return util::ReadAccess<T>(pifRam, paddr & PIF_RAM_DSIZE);
+      }
     case 0x00800000 ... 0x03FFFFFF: case 0x04002000 ... 0x0403FFFF:
     case 0x04200000 ... 0x042FFFFF:
     case 0x04900000 ... 0x07FFFFFF: case 0x08000000 ... 0x0FFFFFFF:
@@ -111,12 +151,44 @@ void Mem::Write(Registers& regs, u32 vaddr, T val, s64 pc) {
   }
 
   switch(paddr) {
-    case 0x00000000 ... 0x007FFFFF: util::WriteAccess<T>(rdram.data(), paddr & RDRAM_DSIZE, val); break;
-    case 0x04000000 ... 0x04000FFF: util::WriteAccess<T>(mmio.rsp.dmem, paddr & DMEM_DSIZE, val); break;
-    case 0x04001000 ... 0x04001FFF: util::WriteAccess<T>(mmio.rsp.imem, paddr & IMEM_DSIZE, val); break;
+    case 0x00000000 ... 0x007FFFFF:
+      if constexpr (sizeof(T) == 1) {
+        util::WriteAccess<T>(mmio.rdp.dram.data(), BYTE_ADDRESS(paddr) & RDRAM_DSIZE, val);
+      } else if constexpr (sizeof(T) == 2) {
+        util::WriteAccess<T>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr) & RDRAM_DSIZE, val);
+      } else {
+        util::WriteAccess<T>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE, val);
+      }
+      break;
+    case 0x04000000 ... 0x04000FFF:
+      if constexpr (sizeof(T) == 1) {
+        util::WriteAccess<T>(mmio.rsp.dmem, BYTE_ADDRESS(paddr) & DMEM_DSIZE, val);
+      } else if constexpr (sizeof(T) == 2) {
+        util::WriteAccess<T>(mmio.rsp.dmem, HALF_ADDRESS(paddr) & DMEM_DSIZE, val);
+      } else {
+        util::WriteAccess<T>(mmio.rsp.dmem, paddr & DMEM_DSIZE, val);
+      }
+      break;
+    case 0x04001000 ... 0x04001FFF:
+      if constexpr (sizeof(T) == 1) {
+        util::WriteAccess<T>(mmio.rsp.imem, BYTE_ADDRESS(paddr) & IMEM_DSIZE, val);
+      } else if constexpr (sizeof(T) == 2) {
+        util::WriteAccess<T>(mmio.rsp.imem, HALF_ADDRESS(paddr) & IMEM_DSIZE, val);
+      } else {
+        util::WriteAccess<T>(mmio.rsp.imem, paddr & IMEM_DSIZE, val);
+      }
+      break;
     case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
     case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF: mmio.Write(*this, regs, paddr, val); break;
-    case 0x1FC007C0 ... 0x1FC007FF: util::WriteAccess<T>(pifRam, paddr & PIF_RAM_DSIZE, val); break;
+    case 0x1FC007C0 ... 0x1FC007FF:
+      if constexpr (sizeof(T) == 1) {
+        util::WriteAccess<T>(pifRam, BYTE_ADDRESS(paddr) & PIF_RAM_DSIZE, val);
+      } else if constexpr (sizeof(T) == 2) {
+        util::WriteAccess<T>(pifRam, HALF_ADDRESS(paddr) & PIF_RAM_DSIZE, val);
+      } else {
+        util::WriteAccess<T>(pifRam, paddr & PIF_RAM_DSIZE, val);
+      }
+      break;
     case 0x00800000 ... 0x03FFFFFF: case 0x04002000 ... 0x0403FFFF:
     case 0x04200000 ... 0x042FFFFF:
     case 0x04900000 ... 0x07FFFFFF: case 0x08000000 ... 0x0FFFFFFF:
diff --git a/src/n64/core/Mem.hpp b/src/n64/core/Mem.hpp
index 839d96ec..9f1a638d 100644
--- a/src/n64/core/Mem.hpp
+++ b/src/n64/core/Mem.hpp
@@ -12,7 +12,7 @@ struct Mem {
   void Reset();
   void LoadROM(const std::string&);
   [[nodiscard]] auto GetRDRAM() -> u8* {
-    return rdram.data();
+    return mmio.rdp.dram.data();
   }
   template <class T, bool tlb = true>
   T Read(Registers&, u32, s64);
@@ -27,7 +27,7 @@ private:
   friend struct RSP;
   friend struct Core;
   MMIO mmio;
-  std::vector<u8> cart, rdram, sram;
+  std::vector<u8> cart, sram;
   u8 pifBootrom[PIF_BOOTROM_SIZE]{};
   size_t romMask;
 };
diff --git a/src/n64/core/RDP.cpp b/src/n64/core/RDP.cpp
index a847f5bc..288b6f49 100644
--- a/src/n64/core/RDP.cpp
+++ b/src/n64/core/RDP.cpp
@@ -11,6 +11,8 @@ RDP::RDP() {
 
 void RDP::Reset() {
   dpc.status.raw = 0x80;
+  dram.resize(RDRAM_SIZE);
+  std::fill(dram.begin(), dram.end(), 0);
   memset(cmd_buf, 0, 0x100000);
 }
 
diff --git a/src/n64/core/RDP.hpp b/src/n64/core/RDP.hpp
index c57f608c..f7b22c25 100644
--- a/src/n64/core/RDP.hpp
+++ b/src/n64/core/RDP.hpp
@@ -1,5 +1,6 @@
 #pragma once
 #include <common.hpp>
+#include <vector>
 
 namespace n64 {
 
@@ -54,7 +55,8 @@ struct RDP {
   RDP();
   void Reset();
 
-  auto Read(u32 addr) const -> u32;
+  std::vector<u8> dram;
+  [[nodiscard]] auto Read(u32 addr) const -> u32;
   void Write(u32 addr, u32 val);
   void StatusWrite(u32 val);
   void RunCommand(MI& mi, Registers& regs, RSP& rsp);
diff --git a/src/n64/core/mmio/PI.cpp b/src/n64/core/mmio/PI.cpp
index 6d437b02..667b3e57 100644
--- a/src/n64/core/mmio/PI.cpp
+++ b/src/n64/core/mmio/PI.cpp
@@ -52,7 +52,9 @@ void PI::Write(Mem& mem, Registers& regs, u32 addr, u32 val) {
         len -= dram_addr & 0x7;
       }
       rdLen = len;
-      memcpy(&mem.cart[cart_addr & mem.romMask], &mem.rdram[dram_addr & RDRAM_DSIZE], len);
+      for(int i = 0; i < len; i++) {
+        mem.cart[BYTE_ADDRESS(cart_addr + i) & mem.romMask] = mem.mmio.rdp.dram[BYTE_ADDRESS(dram_addr + i) & RDRAM_DSIZE];
+      }
       dramAddr = dram_addr + len;
       cartAddr = cart_addr + len;
       InterruptRaise(mi, regs, Interrupt::PI);
@@ -67,7 +69,9 @@ void PI::Write(Mem& mem, Registers& regs, u32 addr, u32 val) {
         len -= dram_addr & 0x7;
       }
       wrLen = len;
-      memcpy(&mem.rdram[dram_addr & RDRAM_DSIZE], &mem.cart[cart_addr & mem.romMask], len);
+      for(int i = 0; i < len; i++) {
+        mem.mmio.rdp.dram[BYTE_ADDRESS(dram_addr + i) & RDRAM_DSIZE] = mem.cart[BYTE_ADDRESS(cart_addr + i) & mem.romMask];
+      }
       dramAddr = dram_addr + len;
       cartAddr = cart_addr + len;
       InterruptRaise(mi, regs, Interrupt::PI);
diff --git a/src/n64/core/mmio/SI.cpp b/src/n64/core/mmio/SI.cpp
index a33dec8a..fb123afc 100644
--- a/src/n64/core/mmio/SI.cpp
+++ b/src/n64/core/mmio/SI.cpp
@@ -38,7 +38,7 @@ void SI::Write(Mem& mem, Registers& regs, u32 addr, u32 val) {
       ProcessPIFCommands(mem.pifRam, controller, mem);
 
       u8 pifAddr = (val & 0x7FC) & PIF_RAM_DSIZE;
-      memcpy(&mem.rdram[dramAddr & RDRAM_DSIZE],
+      memcpy(&mem.mmio.rdp.dram[dramAddr & RDRAM_DSIZE],
              &mem.pifRam[pifAddr], 64);
       InterruptRaise(mem.mmio.mi, regs, Interrupt::SI);
       status.intr = 1;
@@ -47,8 +47,7 @@ void SI::Write(Mem& mem, Registers& regs, u32 addr, u32 val) {
       //if(!(status.raw & 3)) {
       u8 pifAddr = (val & 0x7FC) & PIF_RAM_DSIZE;
       memcpy(&mem.pifRam[pifAddr],
-             &mem.rdram[dramAddr & RDRAM_DSIZE], 64);
-
+             &mem.mmio.rdp.dram[dramAddr & RDRAM_DSIZE], 64);
       ProcessPIFCommands(mem.pifRam, controller, mem);
       InterruptRaise(mem.mmio.mi, regs, Interrupt::SI);
       status.intr = 1;
diff --git a/src/util.hpp b/src/util.hpp
index b44ff268..c5a3dc1e 100644
--- a/src/util.hpp
+++ b/src/util.hpp
@@ -5,6 +5,7 @@
 #include <portable_endian_bswap.h>
 #include <fstream>
 #include <vector>
+#include <array>
 
 namespace util {
 enum MessageType : u8 {
@@ -46,7 +47,7 @@ constexpr void logdebug(const std::string& fmt, Args... args) {
 }
 
 template <typename T, bool HToBE = false>
-auto GetSwapFunc(T num) -> T {
+[[maybe_unused]] auto GetSwapFunc(T num) -> T {
   static_assert(sizeof(T) == 2 || sizeof(T) == 4 || sizeof(T) == 8, "GetSwapFunc used with invalid size!");
   if constexpr(sizeof(T) == 2) {
     if constexpr(HToBE) {
@@ -70,11 +71,18 @@ template <typename T>
 inline T ReadAccess(u8* data, u32 index) {
   if constexpr(sizeof(T) == 1) {
     return data[index];
-  } else {
-    static_assert(sizeof(T) == 2 || sizeof(T) == 4 || sizeof(T) == 8);
+  } else if constexpr (sizeof(T) == 2 || sizeof(T) == 4) {
     T result = 0;
     memcpy(&result, &data[index], sizeof(T));
-    return GetSwapFunc<T>(result);
+    return result;
+  } else {
+    static_assert(sizeof(T) == 8);
+    u32 hi = 0;
+    u32 lo = 0;
+    memcpy(&hi, &data[index + 0], sizeof(u32));
+    memcpy(&lo, &data[index + 4], sizeof(u32));
+    T result = ((T)hi << 32) | (T)lo;
+    return result;
   }
 }
 
@@ -83,35 +91,52 @@ inline void WriteAccess(u8* data, u32 index, T val) {
   if constexpr(sizeof(T) == 1) {
     data[index] = val;
     return;
+  } else if constexpr (sizeof(T) == 2 || sizeof(T) == 4){
+    memcpy(&data[index], &val, sizeof(T));
   } else {
-    static_assert(sizeof(T) == 2 || sizeof(T) == 4 || sizeof(T) == 8);
-    T temp = GetSwapFunc<T, true>(val);
-    memcpy(&data[index], &temp, sizeof(T));
+    static_assert(sizeof(T) == 8);
+    u32 hi = val >> 32;
+    u32 lo = val;
+    memcpy(&data[index + 0], &hi, sizeof(u32));
+    memcpy(&data[index + 4], &lo, sizeof(u32));
   }
 }
 
-#define Z64 0x80371240
-#define N64 0x40123780
-#define V64 0x37804012
+inline void SwapBuffer32(size_t size, u8* data) {
+  for(int i = 0; i < size; i += 4) {
+    u32 original = *(u32*)&data[i];
+    *(u32*)&data[i] = bswap_32(original);
+  }
+}
+
+inline void SwapBuffer16(size_t size, u8* data) {
+  for(int i = 0; i < size; i += 2) {
+    u16 original = *(u16*)&data[i];
+    *(u16*)&data[i] = bswap_16(original);
+  }
+}
+
+enum RomTypes {
+  Z64 = 0x80371240,
+  N64 = 0x40123780,
+  V64 = 0x37804012
+};
 
 inline void SwapN64Rom(size_t size, u8* data) {
-  u32 endianness;
+  RomTypes endianness;
   memcpy(&endianness, data, 4);
-  endianness = be32toh(endianness);
+  endianness = static_cast<RomTypes>(be32toh(endianness));
   switch(endianness) {
-    case V64:
-      for(int i = 0; i < size; i += 2) {
-        u16 original = *(u16*)&data[i];
-        *(u16*)&data[i] = bswap_16(original);
+    case V64: {
+      u8* tmp = (u8*)calloc(size, 1);
+      for(int i = 0; i < size; i++) {
+        data[i] = tmp[i ^ 2];
       }
-      break;
-    case N64:
-      for(int i = 0; i < size; i += 4) {
-        u32 original = *(u32*)&data[i];
-        *(u32*)&data[i] = bswap_32(original);
-      }
-      break;
-    case Z64: break;
+      free(tmp);
+    } break;
+    case N64: break;
+    case Z64:
+      SwapBuffer32(size, data); break;
     default:
       panic("Unrecognized rom format! Make sure this is a valid Nintendo 64 ROM dump!\n");
   }
@@ -120,23 +145,29 @@ inline void SwapN64Rom(size_t size, u8* data) {
 template <size_t size, typename T = u8>
 struct CircularBuffer {
   CircularBuffer() : head(0) {
-    memset(raw, 0, size * sizeof(T));
+    memset(raw.data(), 0, size * sizeof(T));
   }
 
   void PushValue(T val) {
-    raw[head & mask] = val;
-    head &= mask;
+    raw[head] = val;
     head++;
+    head &= mask;
   }
 
   T PopValue() {
     head--;
     head &= mask;
-    return raw[head & mask];
+    return raw[head];
   }
+
+  T& operator[](const int index) {
+    return raw[index];
+  }
+  auto begin() { return raw.begin(); }
+  auto end() { return raw.end(); }
   size_t GetHead() { return head; }
 private:
-  T raw[size];
+  std::array<T, size> raw;
   size_t head;
   static constexpr size_t mask = size - 1;
 };