From 932275ab329772099ebdd51228f6eaaa2506913b Mon Sep 17 00:00:00 2001
From: CocoSimone <simococo03@gmail.com>
Date: Sun, 16 Oct 2022 21:37:48 +0200
Subject: [PATCH] Couple RDP fixes and some other stuff i forgor :skull:

---
 src/frontend/imgui/Window.cpp                 |  12 +-
 src/n64/core/Cpu.cpp                          |  16 +--
 src/n64/core/Mem.cpp                          |  16 +--
 src/n64/core/Mem.hpp                          |  10 ++
 src/n64/core/RDP.cpp                          | 130 +++++++++---------
 src/n64/core/RSP.cpp                          |  18 +--
 src/n64/core/RSP.hpp                          |   8 +-
 src/n64/core/cpu/Registers.cpp                |   1 +
 src/n64/core/cpu/registers/Cop0.cpp           |  18 +--
 src/n64/core/cpu/registers/Cop0.hpp           |   1 +
 .../core/cpu/registers/cop0instructions.cpp   |   1 +
 src/util.hpp                                  |   2 +-
 12 files changed, 131 insertions(+), 102 deletions(-)

diff --git a/src/frontend/imgui/Window.cpp b/src/frontend/imgui/Window.cpp
index f3be8ebc..927415fa 100644
--- a/src/frontend/imgui/Window.cpp
+++ b/src/frontend/imgui/Window.cpp
@@ -138,6 +138,7 @@ void Window::InitImgui(const n64::Core& core) {
 
   memoryEditor.ReadFn = readHandler;
   memoryEditor.WriteFn = writeHandler;
+  memoryEditor.Cols = 32;
 }
 
 Window::~Window() {
@@ -194,8 +195,13 @@ void Window::Render(n64::Core& core) {
     windowTitle = shadowWindowTitle;
   }
   static bool showSettings = false;
+  static bool showMemEditor = false;
   bool showMainMenuBar = windowID == SDL_GetWindowID(SDL_GetMouseFocus());
-  memoryEditor.DrawWindow("Memory viewer", &core, 0x80000000);
+  if(showMemEditor) {
+    ImGui::PushFont(codeFont);
+    memoryEditor.DrawWindow("Memory viewer", &core, 0x80000000);
+    ImGui::PopFont();
+  }
   if(showMainMenuBar) {
     ImGui::BeginMainMenuBar();
     if (ImGui::BeginMenu("File")) {
@@ -214,6 +220,9 @@ void Window::Render(n64::Core& core) {
       if (ImGui::MenuItem("Dump IMEM")) {
         core.mem.DumpIMEM();
       }
+      if (ImGui::MenuItem("Dump DMEM")) {
+        core.mem.DumpDMEM();
+      }
       if (ImGui::MenuItem("Exit")) {
         core.done = true;
       }
@@ -241,6 +250,7 @@ void Window::Render(n64::Core& core) {
       if (ImGui::MenuItem("Settings")) {
         showSettings = true;
       }
+      ImGui::Checkbox("Show memory editor", &showMemEditor);
       ImGui::EndMenu();
     }
     ImGui::EndMainMenuBar();
diff --git a/src/n64/core/Cpu.cpp b/src/n64/core/Cpu.cpp
index c36a2141..3ed8c9c6 100644
--- a/src/n64/core/Cpu.cpp
+++ b/src/n64/core/Cpu.cpp
@@ -28,9 +28,7 @@ inline void CheckCompareInterrupt(MI& mi, Registers& regs) {
 }
 
 inline void HandleInterrupt(Registers& regs) {
-  if(ShouldServiceInterrupt(regs)) {
-    FireException(regs, ExceptionCode::Interrupt, 0, regs.pc);
-  }
+
 }
 
 inline void Cpu::disassembly(u32 instr) {
@@ -57,19 +55,21 @@ void Cpu::Step(Mem& mem) {
   regs.gpr[0] = 0;
 
   CheckCompareInterrupt(mem.mmio.mi, regs);
-  HandleInterrupt(regs);
+
+  regs.prevDelaySlot = regs.delaySlot;
+  regs.delaySlot = false;
 
   u32 instruction = mem.Read32(regs, regs.pc, regs.pc);
 
-  //disassembly(instruction);
+  if(ShouldServiceInterrupt(regs)) {
+    FireException(regs, ExceptionCode::Interrupt, 0, regs.pc);
+    return;
+  }
 
   regs.oldPC = regs.pc;
   regs.pc = regs.nextPC;
   regs.nextPC += 4;
 
   Exec(mem, instruction);
-
-  regs.prevDelaySlot = regs.delaySlot;
-  regs.delaySlot = false;
 }
 }
\ No newline at end of file
diff --git a/src/n64/core/Mem.cpp b/src/n64/core/Mem.cpp
index df3a1f0f..7d511b98 100644
--- a/src/n64/core/Mem.cpp
+++ b/src/n64/core/Mem.cpp
@@ -80,7 +80,7 @@ u8 Mem::Read8(n64::Registers &regs, u64 vaddr, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      return mmio.rdp.dram[BYTE_ADDRESS(paddr) & RDRAM_DSIZE];
+      return mmio.rdp.dram[BYTE_ADDRESS(paddr)];
     case 0x04000000 ... 0x0403FFFF:
       if((paddr >> 12) & 1)
         return mmio.rsp.imem[BYTE_ADDRESS(paddr) & IMEM_DSIZE];
@@ -116,7 +116,7 @@ u16 Mem::Read16(n64::Registers &regs, u64 vaddr, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      return util::ReadAccess<u16>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr) & RDRAM_DSIZE);
+      return util::ReadAccess<u16>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr));
     case 0x04000000 ... 0x0403FFFF:
       if((paddr >> 12) & 1)
         return util::ReadAccess<u16>(mmio.rsp.imem, HALF_ADDRESS(paddr) & IMEM_DSIZE);
@@ -148,7 +148,7 @@ u32 Mem::Read32(n64::Registers &regs, u64 vaddr, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      return util::ReadAccess<u32>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE);
+      return util::ReadAccess<u32>(mmio.rdp.dram.data(), paddr);
     case 0x04000000 ... 0x0403FFFF:
       if((paddr >> 12) & 1)
         return util::ReadAccess<u32>(mmio.rsp.imem, paddr & IMEM_DSIZE);
@@ -180,7 +180,7 @@ u64 Mem::Read64(n64::Registers &regs, u64 vaddr, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      return util::ReadAccess<u64>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE);
+      return util::ReadAccess<u64>(mmio.rdp.dram.data(), paddr);
     case 0x04000000 ... 0x0403FFFF:
       if((paddr >> 12) & 1)
         return util::ReadAccess<u64>(mmio.rsp.imem, paddr & IMEM_DSIZE);
@@ -222,7 +222,7 @@ void Mem::Write8(Registers& regs, u64 vaddr, u32 val, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      mmio.rdp.dram[BYTE_ADDRESS(paddr) & RDRAM_DSIZE] = val;
+      mmio.rdp.dram[BYTE_ADDRESS(paddr)] = val;
       break;
     case 0x04000000 ... 0x0403FFFF:
       val = val << (8 * (3 - (paddr & 3)));
@@ -261,7 +261,7 @@ void Mem::Write16(Registers& regs, u64 vaddr, u32 val, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      util::WriteAccess<u16>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr) & RDRAM_DSIZE, val);
+      util::WriteAccess<u16>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr), val);
       break;
     case 0x04000000 ... 0x0403FFFF:
       val = val << (16 * !(paddr & 2));
@@ -297,7 +297,7 @@ void Mem::Write32(Registers& regs, u64 vaddr, u32 val, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      util::WriteAccess<u32>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE, val);
+      util::WriteAccess<u32>(mmio.rdp.dram.data(), paddr, val);
       break;
     case 0x04000000 ... 0x0403FFFF:
       if((paddr >> 12) & 1)
@@ -340,7 +340,7 @@ void Mem::Write64(Registers& regs, u64 vaddr, u64 val, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      util::WriteAccess<u64>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE, val);
+      util::WriteAccess<u64>(mmio.rdp.dram.data(), paddr, val);
       break;
     case 0x04000000 ... 0x0403FFFF:
       val >>= 32;
diff --git a/src/n64/core/Mem.hpp b/src/n64/core/Mem.hpp
index fd795a03..48b34c58 100644
--- a/src/n64/core/Mem.hpp
+++ b/src/n64/core/Mem.hpp
@@ -62,6 +62,16 @@ struct Mem {
     free(temp);
     fclose(fp);
   }
+
+  inline void DumpDMEM() const {
+    FILE *fp = fopen("dmem.dump", "wb");
+    u8 *temp = (u8*)calloc(DMEM_SIZE, 1);
+    memcpy(temp, mmio.rsp.dmem, DMEM_SIZE);
+    util::SwapBuffer32(DMEM_SIZE, temp);
+    fwrite(temp, 1, DMEM_SIZE, fp);
+    free(temp);
+    fclose(fp);
+  }
 private:
   friend struct SI;
   friend struct PI;
diff --git a/src/n64/core/RDP.cpp b/src/n64/core/RDP.cpp
index d86ebfbd..15b65218 100644
--- a/src/n64/core/RDP.cpp
+++ b/src/n64/core/RDP.cpp
@@ -24,12 +24,13 @@ static const int cmd_lens[64] = {
 };
 
 template <bool crashOnUnimplemented>
-auto RDP::Read(u32 addr) const -> u32{
+auto RDP::Read(u32 addr) const -> u32 {
   switch(addr) {
     case 0x04100000: return dpc.start;
     case 0x04100004: return dpc.end;
     case 0x04100008: return dpc.current;
-    case 0x0410000C: return dpc.status.raw;
+    case 0x0410000C:
+      return dpc.status.raw;
     case 0x04100010: return dpc.clock;
     case 0x04100014: return dpc.status.cmdBusy;
     case 0x04100018: return dpc.status.pipeBusy;
@@ -62,15 +63,12 @@ template void RDP::Write<true>(MI&, Registers&, RSP&, u32, u32);
 template void RDP::Write<false>(MI&, Registers&, RSP&, u32, u32);
 
 void RDP::WriteStatus(MI& mi, Registers& regs, RSP& rsp, u32 val) {
-  bool rdpUnfrozen = false;
-
   DPCStatusWrite temp{};
   temp.raw = val;
 
   CLEAR_SET(dpc.status.xbusDmemDma, temp.clearXbusDmemDma, temp.setXbusDmemDma);
   if(temp.clearFreeze) {
     dpc.status.freeze = false;
-    rdpUnfrozen = true;
   }
 
   if(temp.setFreeze) {
@@ -82,7 +80,7 @@ void RDP::WriteStatus(MI& mi, Registers& regs, RSP& rsp, u32 val) {
   CLEAR_SET(dpc.status.pipeBusy, temp.clearPipe, false);
   CLEAR_SET(dpc.status.tmemBusy, temp.clearTmem, false);
 
-  if(rdpUnfrozen) {
+  if(!dpc.status.freeze) {
     RunCommand(mi, regs, rsp);
   }
 }
@@ -128,85 +126,89 @@ inline void logCommand(u8 cmd) {
 }
 
 void RDP::RunCommand(MI& mi, Registers& regs, RSP& rsp) {
-  //if (dpc.status.freeze) {
-  //  return;
-  //}
-  dpc.status.pipeBusy = true;
-  dpc.status.startGclk = true;
-  dpc.status.freeze = true;
-
-  static int remaining_cmds = 0;
-
-  const u32 current = dpc.current & 0xFFFFF8;
-  const u32 end = dpc.end & 0xFFFFF8;
-
-  int len = end - current;
-  if(len <= 0) return;
-
-  if(len + (remaining_cmds * 4) > 0xFFFFF) {
+  if (dpc.status.freeze) {
     return;
   }
+  dpc.status.pipeBusy = true;
+  dpc.status.startGclk = true;
+  if(dpc.end > dpc.current) {
+    dpc.status.freeze = true;
 
-  if(dpc.status.xbusDmemDma) {
-    for(int i = 0; i < len; i += 4) {
-      u32 cmd = util::ReadAccess<u32>(rsp.dmem, (current + i) & 0xFFF);
-      cmd_buf[remaining_cmds + (i >> 2)] = cmd;
-    }
-  } else {
-    if(end > RDRAM_DSIZE || current > RDRAM_DSIZE) {
+    static int remaining_cmds = 0;
+
+    const u32 current = dpc.current & 0xFFFFF8;
+    const u32 end = dpc.end & 0xFFFFF8;
+
+    int len = end - current;
+    if (len <= 0) return;
+
+    if (len + (remaining_cmds * 4) > 0xFFFFF) {
+      util::panic("Too many RDP commands\n");
       return;
     }
-    for(int i = 0; i < len; i += 4) {
-      u32 cmd = util::ReadAccess<u32>(dram.data(), current + i);
-      cmd_buf[remaining_cmds + (i >> 2)] = cmd;
+
+    if (dpc.status.xbusDmemDma) {
+      for (int i = 0; i < len; i += 4) {
+        u32 cmd = util::ReadAccess<u32>(rsp.dmem, (current + i) & 0xFFF);
+        cmd_buf[remaining_cmds + (i >> 2)] = cmd;
+      }
+    } else {
+      if (end > 0x7FFFFFF || current > 0x7FFFFFF) {
+        util::panic("Not running RDP commands past end of RDRAM\n");
+        return;
+      }
+      for (int i = 0; i < len; i += 4) {
+        u32 cmd = util::ReadAccess<u32>(dram.data(), current + i);
+        cmd_buf[remaining_cmds + (i >> 2)] = cmd;
+      }
     }
-  }
 
-  int word_len = (len >> 2) + remaining_cmds;
-  int buf_index = 0;
+    int word_len = (len >> 2) + remaining_cmds;
+    int buf_index = 0;
 
-  bool processed_all = true;
+    bool processed_all = true;
 
-  while(buf_index < word_len) {
-    u8 cmd = (cmd_buf[buf_index] >> 24) & 0x3F;
-   // logCommand(cmd);
+    while (buf_index < word_len) {
+      u8 cmd = (cmd_buf[buf_index] >> 24) & 0x3F;
+      logCommand(cmd);
 
-    int cmd_len = cmd_lens[cmd];
-    if((buf_index + cmd_len) * 4 > len + (remaining_cmds * 4)) {
-      remaining_cmds = word_len - buf_index;
+      int cmd_len = cmd_lens[cmd];
+      if ((buf_index + cmd_len) * 4 > len + (remaining_cmds * 4)) {
+        remaining_cmds = word_len - buf_index;
 
-      u32 tmp[remaining_cmds];
-      for(int i = 0; i < remaining_cmds; i++) {
-        tmp[i] = cmd_buf[buf_index + i];
+        u32 tmp[remaining_cmds];
+        for (int i = 0; i < remaining_cmds; i++) {
+          tmp[i] = cmd_buf[buf_index + i];
+        }
+
+        for (int i = 0; i < remaining_cmds; i++) {
+          cmd_buf[i] = tmp[i];
+        }
+
+        processed_all = false;
+        break;
       }
 
-      for(int i = 0; i < remaining_cmds; i++) {
-        cmd_buf[buf_index + i] = tmp[i];
+      if (cmd >= 8) {
+        ParallelRdpEnqueueCommand(cmd_len, &cmd_buf[buf_index]);
       }
 
-      processed_all = false;
-      break;
+      if (cmd == 0x29) {
+        OnFullSync(mi, regs);
+      }
+
+      buf_index += cmd_len;
     }
 
-    if(cmd >= 8) {
-      ParallelRdpEnqueueCommand(cmd_len, &cmd_buf[buf_index]);
+    if (processed_all) {
+      remaining_cmds = 0;
     }
 
-    if (cmd == 0x29) {
-      OnFullSync(mi, regs);
-    }
-
-    buf_index += cmd_len;
+    dpc.current = end;
+    dpc.end = end;
+    dpc.status.freeze = false;
   }
-
-  if(processed_all) {
-    remaining_cmds = 0;
-  }
-
-  dpc.current = end;
-  dpc.end = end;
   dpc.status.cbufReady = true;
-  dpc.status.freeze = false;
 }
 
 void RDP::OnFullSync(MI& mi, Registers& regs) {
diff --git a/src/n64/core/RSP.cpp b/src/n64/core/RSP.cpp
index a7b29bff..5e34c176 100644
--- a/src/n64/core/RSP.cpp
+++ b/src/n64/core/RSP.cpp
@@ -56,11 +56,9 @@ inline void logRSP(const RSP& rsp, const u32 instr) {
   for (int e = 0; e < 8; e++) {
     util::print("{:04X}", rsp.acc.l.element[e]);
   }
-  util::print(" ");
 
-  util::print("{:04X} {:04X} {:02X}", rsp.GetVCC(), rsp.GetVCO(), rsp.GetVCE());
-
-  util::print("\n");
+  util::print(" {:04X} {:04X} {:02X}\n", rsp.GetVCC(), rsp.GetVCO(), rsp.GetVCE());
+  util::print("DMEM: {:02X}{:02X}\n", rsp.dmem[0x3c4], rsp.dmem[0x3c5]);
 }
 
 void RSP::Step(Registers& regs, Mem& mem) {
@@ -69,12 +67,10 @@ void RSP::Step(Registers& regs, Mem& mem) {
   oldPC = pc & 0xFFC;
   pc = nextPC & 0xFFC;
   nextPC += 4;
-  if(oldPC == 0x00E4 && gpr[1] == 0x18) {
-    printf("\n");
-  }
+
   Exec(regs, mem, instr);
 
-  // logRSP(*this, instr);
+  //logRSP(*this, instr);
 }
 
 template <bool crashOnUnimplemented>
@@ -87,7 +83,11 @@ auto RSP::Read(u32 addr) -> u32{
     case 0x04040010: return spStatus.raw;
     case 0x04040014: return spStatus.dmaFull;
     case 0x04040018: return 0;
-    case 0x0404001C: return AcquireSemaphore();
+    case 0x0404001C:
+      if constexpr (crashOnUnimplemented) {
+        return semaphore;
+      }
+      return AcquireSemaphore();
     case 0x04080000: return pc & 0xFFC;
     default:
       if constexpr (crashOnUnimplemented) {
diff --git a/src/n64/core/RSP.hpp b/src/n64/core/RSP.hpp
index 44f09a49..781f66bf 100644
--- a/src/n64/core/RSP.hpp
+++ b/src/n64/core/RSP.hpp
@@ -390,13 +390,15 @@ struct RSP {
 
       int skip = i == len.count ? 0 : len.skip;
 
-      dram_address += (length + skip) & 0xFFFFF8;
+      dram_address += (length + skip);
+      dram_address &= 0xFFFFF8;
       mem_address += length;
+      mem_address &= 0xFF8;
     }
 
-    rsp.lastSuccessfulSPAddr.address = mem_address & 0xFF8;
+    rsp.lastSuccessfulSPAddr.address = mem_address;
     rsp.lastSuccessfulSPAddr.bank = bank;
-    rsp.lastSuccessfulDRAMAddr.address = dram_address & 0xFFFFF8;
+    rsp.lastSuccessfulDRAMAddr.address = dram_address;
     rsp.spDMALen.raw = 0xFF8 | (rsp.spDMALen.skip << 20);
   }
 private:
diff --git a/src/n64/core/cpu/Registers.cpp b/src/n64/core/cpu/Registers.cpp
index 83a49f76..a38b5c81 100644
--- a/src/n64/core/cpu/Registers.cpp
+++ b/src/n64/core/cpu/Registers.cpp
@@ -15,6 +15,7 @@ void Registers::Reset() {
 }
 
 void Registers::SetPC(s64 val) {
+  oldPC = pc;
   pc = val;
   nextPC = pc + 4;
 }
diff --git a/src/n64/core/cpu/registers/Cop0.cpp b/src/n64/core/cpu/registers/Cop0.cpp
index a909f889..3ad79ba3 100644
--- a/src/n64/core/cpu/registers/Cop0.cpp
+++ b/src/n64/core/cpu/registers/Cop0.cpp
@@ -173,17 +173,19 @@ u64 getVPN(u64 addr, u64 pageMask) {
 TLBEntry* TLBTryMatch(Registers& regs, u64 vaddr, int* match) {
   for(int i = 0; i < 32; i++) {
     TLBEntry *entry = &regs.cop0.tlb[i];
-    u64 entry_vpn = getVPN(entry->entryHi.raw, entry->pageMask.raw);
-    u64 vaddr_vpn = getVPN(vaddr, entry->pageMask.raw);
+    if(entry->initialized) {
+      u64 entry_vpn = getVPN(entry->entryHi.raw, entry->pageMask.raw);
+      u64 vaddr_vpn = getVPN(vaddr, entry->pageMask.raw);
 
-    bool vpn_match = entry_vpn == vaddr_vpn;
-    bool asid_match = entry->global || (regs.cop0.entryHi.asid == entry->entryHi.asid);
+      bool vpn_match = entry_vpn == vaddr_vpn;
+      bool asid_match = entry->global || (regs.cop0.entryHi.asid == entry->entryHi.asid);
 
-    if(vpn_match && asid_match) {
-      if(match) {
-        *match = i;
+      if (vpn_match && asid_match) {
+        if (match) {
+          *match = i;
+        }
+        return entry;
       }
-      return entry;
     }
   }
 
diff --git a/src/n64/core/cpu/registers/Cop0.hpp b/src/n64/core/cpu/registers/Cop0.hpp
index 583a48d9..7c400e46 100644
--- a/src/n64/core/cpu/registers/Cop0.hpp
+++ b/src/n64/core/cpu/registers/Cop0.hpp
@@ -132,6 +132,7 @@ union PageMask {
 };
 
 struct TLBEntry {
+  bool initialized;
   union {
     u32 raw;
     struct {
diff --git a/src/n64/core/cpu/registers/cop0instructions.cpp b/src/n64/core/cpu/registers/cop0instructions.cpp
index 49dd5a1f..60360e7e 100644
--- a/src/n64/core/cpu/registers/cop0instructions.cpp
+++ b/src/n64/core/cpu/registers/cop0instructions.cpp
@@ -67,6 +67,7 @@ void Cop0::tlbwi(Registers& regs) {
   tlb[Index].pageMask.raw = page_mask.raw;
 
   tlb[Index].global = entryLo0.g && entryLo1.g;
+  tlb[Index].initialized = true;
 }
 
 void Cop0::tlbp(Registers& regs) {
diff --git a/src/util.hpp b/src/util.hpp
index 0dabbe9b..07a1a9c1 100644
--- a/src/util.hpp
+++ b/src/util.hpp
@@ -253,7 +253,7 @@ inline auto ReadFileBinary(const std::string& path, u32** buf) {
   std::ifstream file(path, std::ios::binary);
   file.unsetf(std::ios::skipws);
   if(!file.is_open()) {
-    panic("Could not load file!\n");
+    panic("Could not load file '{}'!\n", path);
   }
 
   file.seekg(0, std::ios::end);