Continue integrating n64 core

external/CMakeLists.txt

@@ -52,7 +52,9 @@ target_include_directories(parallel-rdp PUBLIC
   parallel-rdp-standalone/spirv-cross
   parallel-rdp-standalone/vulkan
   parallel-rdp-standalone/vulkan-headers/include
-  parallel-rdp-standalone/util)
+  parallel-rdp-standalone/util
+  ../src/core/n64
+  )
 
 if(WIN32)
   target_compile_definitions(parallel-rdp PUBLIC VK_USE_PLATFORM_WIN32_KHR)

src/core/n64/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(fmt REQUIRED)
 add_subdirectory(core)
 
 set(PARALLELRDP_INCLUDES
+  ../../frontend
   ../../../external/parallel-rdp-standalone/vulkan
   ../../../external/parallel-rdp-standalone/util
   ../../../external/parallel-rdp-standalone/parallel-rdp
@@ -33,6 +34,6 @@ add_library(n64
   memory_regions.hpp
   ../BaseCore.cpp
   ../BaseCore.hpp)
-target_include_directories(n64 PRIVATE . .. ../../frontend/sdl ${PARALLELRDP_INCLUDES})
+target_include_directories(n64 PRIVATE . .. ${PARALLELRDP_INCLUDES})
 target_include_directories(n64 PUBLIC ${mio_SOURCE_DIR}/include ${toml11_SOURCE_DIR}/include)
 target_link_libraries(n64 PRIVATE mio::mio toml11::toml11 fmt::fmt n64-core)

src/core/n64/Core.cpp

@@ -3,9 +3,9 @@
 #include <ParallelRDPWrapper.hpp>
 
 namespace natsukashii::n64::core {
-Core::Core(const std::string& rom) {
+Core::Core(Platform platform, const std::string& rom) {
   mem.LoadROM(rom);
-  LoadParallelRDP(mem.GetRDRAM());
+  LoadParallelRDP(platform, mem.GetRDRAM());
 }
 
 void Core::Run() {

src/core/n64/Core.hpp

@@ -4,11 +4,13 @@
 #include <n64/core/Mem.hpp>
 #include <string>
 
+enum class Platform : bool;
+
 namespace natsukashii::n64::core {
-using namespace natsukashii::core;
+
-struct Core : BaseCore {
+struct Core : natsukashii::core::BaseCore {
   ~Core() override = default;
-  explicit Core(const std::string&);
+  explicit Core(Platform platform, const std::string&);
   void Run() override;
   void PollInputs(u32) override;
 private:

src/core/n64/core/CMakeLists.txt

@@ -11,7 +11,7 @@ add_library(n64-core
   RDP.cpp
   RDP.hpp
   mmio/VI.cpp
-  mmio/VI.hpp mmio/Interrupt.hpp mmio/MI.cpp mmio/MI.hpp mmio/Interrupt.cpp)
+  mmio/VI.hpp mmio/Interrupt.hpp mmio/MI.cpp mmio/MI.hpp mmio/Interrupt.cpp MMIO.cpp MMIO.hpp RSP.cpp RSP.hpp rsp/decode.cpp rsp/instructions.cpp)
 
 target_include_directories(n64-core PRIVATE . .. ../../ mmio)
 target_link_libraries(n64-core PUBLIC n64-cpu)

src/core/n64/core/Cpu.cpp

@@ -1 +1,102 @@
 #include <Cpu.hpp>
+#include <MI.hpp>
+#include <Interrupt.hpp>
+#include <util.hpp>
+
+namespace natsukashii::n64::core {
+inline bool ShouldServiceInterrupt(Registers& regs) {
+  bool interrupts_pending = (regs.cop0.status.im & regs.cop0.cause.interruptPending) != 0;
+  bool interrupts_enabled = regs.cop0.status.ie == 1;
+  bool currently_handling_exception = regs.cop0.status.exl == 1;
+  bool currently_handling_error = regs.cop0.status.erl == 1;
+
+  return interrupts_pending && interrupts_enabled &&
+         !currently_handling_exception && !currently_handling_error;
+}
+
+inline void CheckCompareInterrupt(MI& mi, Registers& regs) {
+  regs.cop0.count++;
+  regs.cop0.count &= 0x1FFFFFFFF;
+  if(regs.cop0.count == (u64)regs.cop0.compare << 1) {
+    regs.cop0.cause.ip7 = 1;
+    UpdateInterrupt(mi, regs);
+  }
+}
+
+inline bool Is64BitAddressing(Cop0& cp0, u64 addr) {
+  u8 region = (addr >> 62) & 3;
+  switch(region) {
+    case 0b00: return cp0.status.ux;
+    case 0b01: return cp0.status.sx;
+    case 0b11: return cp0.status.kx;
+    default: return false;
+  }
+}
+
+void FireException(Registers& regs, ExceptionCode code, int cop, s64 pc) {
+  bool old_exl = regs.cop0.status.exl;
+
+  if(!regs.cop0.status.exl) {
+    if(regs.prevDelaySlot) { // TODO: cached value of delay_slot should be used, but Namco Museum breaks!
+      regs.cop0.cause.branchDelay = true;
+      pc -= 4;
+    } else {
+      regs.cop0.cause.branchDelay = false;
+    }
+
+    regs.cop0.EPC = pc;
+  }
+
+  regs.cop0.status.exl = true;
+  regs.cop0.cause.copError = cop;
+  regs.cop0.cause.exceptionCode = code;
+
+  if(regs.cop0.status.bev) {
+    util::panic("BEV bit set!\n");
+  } else {
+    switch(code) {
+      case Interrupt: case TLBModification:
+      case AddressErrorLoad: case AddressErrorStore:
+      case InstructionBusError: case DataBusError:
+      case Syscall: case Breakpoint:
+      case ReservedInstruction: case CoprocessorUnusable:
+      case Overflow: case Trap:
+      case FloatingPointError: case Watch:
+        regs.SetPC((s64)((s32)0x80000180));
+        break;
+      case TLBLoad: case TLBStore:
+        if(old_exl || regs.cop0.tlbError == INVALID) {
+          regs.SetPC((s64)((s32)0x80000180));
+        } else if(Is64BitAddressing(regs.cop0, regs.cop0.badVaddr)) {
+          regs.SetPC((s64)((s32)0x80000080));
+        } else {
+          regs.SetPC((s64)((s32)0x80000000));
+        }
+        break;
+      default: util::panic("Unhandled exception! {}\n", code);
+    }
+  }
+}
+
+inline void HandleInterrupt(Registers& regs) {
+  if(ShouldServiceInterrupt(regs)) {
+    FireException(regs, Interrupt, 0, regs.pc);
+  }
+}
+
+void Cpu::Step(Mem& mem) {
+  regs.gpr[0] = 0;
+  regs.prevDelaySlot = regs.delaySlot;
+  regs.delaySlot = false;
+
+  CheckCompareInterrupt(mem.mmio.mi);
+
+  u32 instruction = mem.Read(regs, regs.pc, regs.pc);
+
+  HandleInterrupt(regs);
+
+  regs.oldPC = regs.pc;
+  regs.pc = regs.nextPC;
+  regs.nextPC += 4;
+}
+}

src/core/n64/core/Cpu.hpp

@@ -1,8 +1,32 @@
 #pragma once
 #include <n64/core/cpu/Registers.hpp>
+#include <n64/core/Mem.hpp>
 
 namespace natsukashii::n64::core {
 struct Cpu {
+  Cpu() = default;
+  void Step(Mem&);
   Registers regs;
 };
+
+enum class ExceptionCode : u8 {
+  Interrupt,
+  TLBModification,
+  TLBLoad,
+  TLBStore,
+  AddressErrorLoad,
+  AddressErrorStore,
+  InstructionBusError,
+  DataBusError,
+  Syscall,
+  Breakpoint,
+  ReservedInstruction,
+  CoprocessorUnusable,
+  Overflow,
+  Trap,
+  FloatingPointError = 15,
+  Watch = 23
+};
+
+void FireException(Registers& regs, ExceptionCode code, int cop, s64 pc);
 }

src/core/n64/core/MMIO.cpp

@@ -0,0 +1,34 @@
+#include <MMIO.hpp>
+#include <util.hpp>
+#include <n64/core/Mem.hpp>
+#include <n64/core/cpu/Registers.hpp>
+
+namespace natsukashii::n64::core {
+u32 MMIO::Read(u32 addr) {
+  switch (addr) {
+    case 0x04040000 ... 0x040FFFFF: return rsp.Read(addr);
+    case 0x04100000 ... 0x041FFFFF: return rdp.Read(addr);
+    case 0x04300000 ... 0x043FFFFF: return mi.Read(addr);
+    case 0x04400000 ...	0x044FFFFF: return vi.Read(addr);
+    case 0x04500000 ... 0x045FFFFF: return ai.Read(addr);
+    case 0x04600000 ... 0x046FFFFF: return pi.Read(mi, addr);
+    case 0x04700000 ... 0x047FFFFF: return ri.Read(addr);
+    case 0x04800000 ... 0x048FFFFF: return si.Read(mi, addr);
+    default: util::panic("Unhandled mmio read at addr {:08X}\n", addr);
+  }
+}
+
+void MMIO::Write(Mem& mem, Registers& regs, u32 addr, u32 val) {
+  switch (addr) {
+    case 0x04040000 ... 0x040FFFFF: rsp.Write(mem, regs, addr, val); break;
+    case 0x04100000 ... 0x041FFFFF: rdp.Write(addr, val); break;
+    case 0x04300000 ... 0x043FFFFF: mi.Write(regs, addr, val); break;
+    case 0x04400000 ...	0x044FFFFF: vi.Write(mi, regs, addr, val); break;
+    case 0x04500000 ... 0x045FFFFF: ai.Write(mem, regs, addr, val); break;
+    case 0x04600000 ... 0x046FFFFF: pi.Write(mem, regs, addr, val); break;
+    case 0x04700000 ... 0x047FFFFF: ri.Write(addr, val); break;
+    case 0x04800000 ... 0x048FFFFF: si.Write(mem, regs, addr, val); break;
+    default: util::panic("Unhandled mmio write at addr {:08X} with val {:08X}\n", addr, val);
+  }
+}
+}

src/core/n64/core/MMIO.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include <mmio/VI.hpp>
+#include <mmio/MI.hpp>
+
+namespace natsukashii::n64::core {
+struct Mem;
+struct Registers;
+
+struct MMIO {
+  MMIO() = default;
+  VI vi;
+  MI mi;
+
+  u32 Read(u32);
+  void Write(Mem&, Registers& regs, u32, u32);
+};
+}

src/core/n64/core/Mem.cpp

@@ -1,6 +1,9 @@
 #include <Mem.hpp>
 #include <fstream>
 #include <util.hpp>
+#include <n64/core/cpu/Registers.hpp>
+#include <Cop0.hpp>
+#include <n64/core/Cpu.hpp>
 
 namespace natsukashii::n64::core {
 Mem::Mem() {
@@ -30,4 +33,87 @@ void Mem::LoadROM(const std::string& filename) {
   util::SwapN64Rom(size, rom.data());
   memcpy(dmem, rom.data(), 0x1000);
 }
+
+template <bool tlb>
+inline bool MapVAddr(Registers& regs, TLBAccessType accessType, u32 vaddr, u32& paddr) {
+  paddr = vaddr & 0x1FFFFFFF;
+  if constexpr(!tlb) return true;
+
+  switch(vaddr >> 29) {
+    case 0 ... 3: case 7:
+      return ProbeTLB(regs, accessType, s64(s32(vaddr)), paddr, nullptr);
+    case 4 ... 5: return true;
+    case 6: util::panic("Unimplemented virtual mapping in KSSEG! ({:08X})\n", vaddr);
+    default:
+      util::panic("Should never end up in default case in map_vaddr! ({:08X})\n", vaddr);
+  }
+}
+
+template <class T, bool tlb>
+T Mem::Read(Registers& regs, u32 vaddr, s64 pc) {
+  u32 paddr = vaddr;
+  if(!MapVAddr<tlb>(regs, LOAD, vaddr, paddr)) {
+    HandleTLBException(regs, vaddr);
+    FireException(regs, GetTLBExceptionCode(regs.cop0.tlbError, LOAD), 0, 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 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 0x00800000 ... 0x03FFFFFF: case 0x04002000 ... 0x0403FFFF:
+    case 0x04200000 ... 0x042FFFFF:
+    case 0x04900000 ... 0x07FFFFFF: case 0x08000000 ... 0x0FFFFFFF:
+    case 0x80000000 ... 0xFFFFFFFF: case 0x1FC00800 ... 0x7FFFFFFF: return 0;
+    default: util::panic("Unimplemented {}-bit read at address {:08X} (PC = {:016X})\n", sizeof(T) * 8, paddr, regs.pc);
+  }
+}
+
+template u8 Mem::Read<u8, true>(Registers& regs, u32 vaddr, s64 pc);
+template u16 Mem::Read<u16, true>(Registers& regs, u32 vaddr, s64 pc);
+template u32 Mem::Read<u32, true>(Registers& regs, u32 vaddr, s64 pc);
+template u64 Mem::Read<u64, true>(Registers& regs, u32 vaddr, s64 pc);
+template u8 Mem::Read<u8, false>(Registers& regs, u32 vaddr, s64 pc);
+template u16 Mem::Read<u16, false>(Registers& regs, u32 vaddr, s64 pc);
+template u32 Mem::Read<u32, false>(Registers& regs, u32 vaddr, s64 pc);
+template u64 Mem::Read<u64, false>(Registers& regs, u32 vaddr, s64 pc);
+
+template <class T, bool tlb>
+void Mem::Write(Registers& regs, u32 vaddr, T val, s64 pc) {
+  u32 paddr = vaddr;
+  if(!MapVAddr<tlb>(regs, STORE, vaddr, paddr)) {
+    HandleTLBException(regs, vaddr);
+    FireException(regs, GetTLBExceptionCode(regs.cop0.tlbError, STORE), 0, 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 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
+    case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF: mmio.Read(paddr); break;
+    case 0x10000000 ... 0x1FBFFFFF: util::WriteAccess<T>(cart.data(), paddr & romMask, val); break;
+    case 0x1FC00000 ... 0x1FC007BF: util::WriteAccess<T>(pifBootrom, paddr & PIF_BOOTROM_DSIZE, val); break;
+    case 0x1FC007C0 ... 0x1FC007FF: 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:
+    case 0x80000000 ... 0xFFFFFFFF: case 0x1FC00800 ... 0x7FFFFFFF: break;
+    default: util::panic("Unimplemented {}-bit write at address {:08X} with value {:0X} (PC = {:016X})\n", sizeof(T) * 8, paddr, val, regs.pc);
+  }
+}
+
+template void Mem::Write<u8, true>(Registers& regs, u32 vaddr, u8 val, s64 pc);
+template void Mem::Write<u16, true>(Registers& regs, u32 vaddr, u16 val, s64 pc);
+template void Mem::Write<u32, true>(Registers& regs, u32 vaddr, u32 val, s64 pc);
+template void Mem::Write<u64, true>(Registers& regs, u32 vaddr, u64 val, s64 pc);
+template void Mem::Write<u8, false>(Registers& regs, u32 vaddr, u8 val, s64 pc);
+template void Mem::Write<u16, false>(Registers& regs, u32 vaddr, u16 val, s64 pc);
+template void Mem::Write<u32, false>(Registers& regs, u32 vaddr, u32 val, s64 pc);
+template void Mem::Write<u64, false>(Registers& regs, u32 vaddr, u64 val, s64 pc);
 }

src/core/n64/core/Mem.hpp

@@ -1,17 +1,25 @@
 #pragma once
 #include <common.hpp>
 #include <n64/memory_regions.hpp>
+#include <n64/core/MMIO.hpp>
 #include <vector>
 
 namespace natsukashii::n64::core {
+struct Registers;
 struct Mem {
   ~Mem() = default;
   Mem();
   void LoadROM(const std::string&);
-  [[nodiscard]] auto GetRDRAM() const -> const u8* {
+  [[nodiscard]] auto GetRDRAM() -> u8* {
     return rdram.data();
   }
+  template <class T, bool tlb>
+  T Read(Registers&, u32, s64);
+  template <class T, bool tlb>
+  void Write(Registers&, u32, T, s64);
 private:
+  friend struct RSP;
+  MMIO mmio;
   std::vector<u8> cart, rdram, sram;
   u8 dmem[DMEM_SIZE]{}, imem[IMEM_SIZE]{}, pifRam[PIF_RAM_SIZE]{};
   u8 pifBootrom[PIF_BOOTROM_SIZE]{};

src/core/n64/core/RDP.cpp

@@ -1 +1,113 @@
 #include <RDP.hpp>
+#include <util.hpp>
+#include <RSP.hpp>
+#include <../../../frontend/ParallelRDPWrapper.hpp>
+#include <Interrupt.hpp>
+
+namespace natsukashii::n64::core {
+static const int cmd_lens[64] = {
+  2, 2, 2, 2, 2, 2, 2, 2, 8, 12, 24, 28, 24, 28, 40, 44,
+  2, 2, 2, 2, 2, 2, 2, 2, 2, 2,  2,  2,  2,  2,  2,  2,
+  2, 2, 2, 2, 4, 4, 2, 2, 2, 2,  2,  2,  2,  2,  2,  2,
+  2, 2, 2, 2, 2, 2, 2, 2, 2, 2,  2,  2,  2,  2,  2,  2
+};
+
+u32 RDP::Read(u32 addr) {
+  switch(addr) {
+    case 0x0410000C: return dpc.status.raw;
+    default: util::panic("Unhandled DP Command Registers read (addr: {:08X})\n", addr);
+  }
+}
+
+void RDP::Write(u32 addr, u32 val) {
+  switch(addr) {
+    case 0x0410000C: StatusWrite(val); break;
+    default: util::panic("Unhandled DP Command Registers read (addr: {:08X}, val: {:08X})\n", addr, val);
+  }
+}
+
+void RDP::StatusWrite(u32 val) {
+  DPCStatusWrite temp{};
+  temp.raw = val;
+  CLEAR_SET(dpc.status.xbusDmemDma, temp.clearXbusDmemDma, temp.setXbusDmemDma);
+  CLEAR_SET(dpc.status.freeze, temp.clearFreeze, false); // Setting it seems to break games? Avoid for now (TODO)
+  CLEAR_SET(dpc.status.flush, temp.clearFlush, temp.setFlush);
+}
+
+void RDP::RunCommand(MI& mi, Registers& regs, RSP& rsp) {
+  static int remaining_cmds = 0;
+  dpc.status.freeze = true;
+
+  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.xbusDmemDma) {
+      for(int i = 0; i < len; i += 4) {
+        u32 cmd = util::ReadAccess<u32>(rsp.dmem, current + i);
+        cmd_buf[remaining_cmds + (i >> 2)] = cmd;
+      }
+    } else {
+      if(end > 0x7FFFFF || current > 0x7FFFFF) {
+        return;
+      }
+      for(int i = 0; i < len; i += 4) {
+        u32 cmd = util::ReadAccess<u32>(rsp.dmem, current + i);
+        cmd_buf[remaining_cmds + (i >> 2)] = cmd;
+      }
+    }
+
+    int word_len = (len >> 2) + remaining_cmds;
+    int buf_index = 0;
+
+    bool processed_all = true;
+
+    while(buf_index < word_len) {
+      u8 cmd = (cmd_buf[buf_index] >> 24) & 0x3F;
+
+      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];
+        }
+
+        for(int i = 0; i < remaining_cmds; i++) {
+          cmd_buf[buf_index + i] = tmp[i];
+        }
+
+        processed_all = false;
+        break;
+      }
+
+      if(cmd >= 8) {
+        ParallelRdpEnqueueCommand(cmd_len, &cmd_buf[buf_index]);
+      }
+
+      if (cmd == 0x29) {
+        OnFullSync();
+        InterruptRaise(mi, regs, InterruptType::DP);
+      }
+
+      buf_index += cmd_len;
+    }
+
+    if(processed_all) {
+      remaining_cmds = 0;
+    }
+
+    dpc.current = end;
+    dpc.status.freeze = false;
+    dpc.status.cbufReady = true;
+  }
+}
+
+void RDP::OnFullSync() {
+  ParallelRdpOnFullSync();
+}
+}

src/core/n64/core/RDP.hpp

@@ -1,8 +1,62 @@
 #pragma once
+#include <common.hpp>
+
 namespace natsukashii::n64::core {
 
-struct RDP {
+struct RSP;
+struct MI;
+struct Registers;
 
+union DPCStatusWrite {
+  u32 raw;
+  struct {
+    unsigned clearXbusDmemDma:1;
+    unsigned setXbusDmemDma:1;
+    unsigned clearFreeze:1;
+    unsigned setFreeze:1;
+    unsigned clearFlush:1;
+    unsigned setFlush:1;
+    unsigned clearTmem:1;
+    unsigned clearPipe:1;
+    unsigned clearCmd:1;
+    unsigned clearClock:1;
+  };
 };
 
+union DPCStatus {
+  struct {
+    unsigned xbusDmemDma;
+    unsigned freeze;
+    unsigned flush;
+    unsigned startGclk;
+    unsigned tmemBusy;
+    unsigned pipeBusy;
+    unsigned cmdBusy;
+    unsigned cbufReady;
+    unsigned dmaBusy;
+    unsigned endValid;
+    unsigned startValid;
+  };
+  u32 raw;
+};
+
+struct DPC {
+  DPCStatus status;
+  u32 start;
+  u32 current;
+  u32 end;
+};
+
+struct RDP {
+  DPC dpc{.status{.raw = 0x80}};
+  u32 cmd_buf[0xFFFFF]{};
+
+  RDP() = default;
+
+  u32 Read(u32 addr);
+  void Write(u32 addr, u32 val);
+  void StatusWrite(u32 val);
+  void RunCommand(MI& mi, Registers& regs, RSP& rsp);
+  void OnFullSync();
+};
 } // natsukashii

src/core/n64/core/RSP.cpp

@@ -0,0 +1,103 @@
+#include <RSP.hpp>
+#include <util.hpp>
+#include <n64/core/Mem.hpp>
+#include "Interrupt.hpp"
+
+namespace natsukashii::n64::core {
+void RSP::StepRSP(MI& mi, Registers& regs, RDP& rdp) {
+  if(!spStatus.halt) {
+    gpr[0] = 0;
+    u32 instr = util::ReadAccess<u32>(imem, pc & IMEM_DSIZE);
+    oldPC = pc & 0xFFF;
+    pc = nextPC & 0xFFF;
+    nextPC += 4;
+    Exec(mi, regs, rdp, instr);
+  }
+}
+
+u32 RSP::Read(u32 addr) {
+  switch (addr) {
+    case 0x04040000: return spDMASPAddr.raw & 0xFFFFF8;
+    case 0x04040004: return spDMADRAMAddr.raw & 0x1FF8;
+    case 0x04040008: return spDMARDLen.raw;
+    case 0x0404000C: return spDMAWRLen.raw;
+    case 0x04040010: return spStatus.raw;
+    case 0x04040018: return 0;
+    case 0x04080000: return pc & 0xFFF;
+    default: util::panic("Unimplemented SP register read %08X\n", addr);
+  }
+}
+
+template <bool isDRAMdest>
+inline void DMA(SPDMALen len, RSP& rsp, u8* dst, u8* src) {
+  u32 length = len.len + 1;
+
+  length = (length + 0x7) & ~0x7;
+
+  u32 last_addr = rsp.spDMASPAddr.address + length;
+  if (last_addr > 0x1000) {
+    u32 overshoot = last_addr - 0x1000;
+    length -= overshoot;
+  }
+
+  u32 dram_address = rsp.spDMADRAMAddr.address & 0xFFFFF8;
+  u32 mem_address = rsp.spDMASPAddr.address & 0x1FF8;
+
+  for (int i = 0; i < len.count + 1; i++) {
+    if(isDRAMdest) {
+      memcpy(&dst[dram_address], &src[mem_address], length);
+    } else {
+      memcpy(&dst[mem_address], &src[dram_address], length);
+    }
+
+    int skip = i == len.count ? 0 : len.skip;
+
+    dram_address += (length + skip) & 0xFFFFF8;
+    mem_address += length;
+  }
+}
+
+void RSP::Write(Mem& mem, Registers& regs, u32 addr, u32 value) {
+  MI& mi = mem.mmio.mi;
+  switch (addr) {
+    case 0x04040000: spDMASPAddr.raw = value & 0x1FF8; break;
+    case 0x04040004: spDMADRAMAddr.raw = value & 0xFFFFF8; break;
+    case 0x04040008: {
+      spDMARDLen.raw = value;
+      DMA<false>(spDMARDLen, *this, spDMASPAddr.bank ? imem : dmem, mem.GetRDRAM());
+      spDMARDLen.raw = 0xFF8 | (spDMARDLen.skip << 20);
+    } break;
+    case 0x0404000C: {
+      spDMAWRLen.raw = value;
+      DMA<true>(spDMAWRLen, *this, mem.GetRDRAM(), spDMASPAddr.bank ? imem : dmem);
+      spDMAWRLen.raw = 0xFF8 | (spDMAWRLen.skip << 20);
+    } break;
+    case 0x04040010: {
+      SPStatusWrite write;
+      write.raw = value;
+      CLEAR_SET(spStatus.halt, write.clearHalt, write.setHalt);
+      CLEAR_SET(spStatus.broke, write.clearBroke, false);
+      if(write.clearIntr) InterruptLower(mi, regs, InterruptType::SP);
+      if(write.setIntr) InterruptRaise(mi, regs, InterruptType::SP);
+      CLEAR_SET(spStatus.singleStep, write.clearSstep, write.setSstep);
+      CLEAR_SET(spStatus.interruptOnBreak, write.clearIntrOnBreak, write.setIntrOnBreak);
+      CLEAR_SET(spStatus.signal0Set, write.clearSignal0, write.setSignal0);
+      CLEAR_SET(spStatus.signal1Set, write.clearSignal1, write.setSignal1);
+      CLEAR_SET(spStatus.signal2Set, write.clearSignal2, write.setSignal2);
+      CLEAR_SET(spStatus.signal3Set, write.clearSignal3, write.setSignal3);
+      CLEAR_SET(spStatus.signal4Set, write.clearSignal4, write.setSignal4);
+      CLEAR_SET(spStatus.signal5Set, write.clearSignal5, write.setSignal5);
+      CLEAR_SET(spStatus.signal6Set, write.clearSignal6, write.setSignal6);
+      CLEAR_SET(spStatus.signal7Set, write.clearSignal7, write.setSignal7);
+    } break;
+    case 0x04080000:
+      if(spStatus.halt) {
+        oldPC = pc;
+        pc = nextPC;
+        nextPC = value & 0xFFF;
+      } break;
+    default: util::panic("Unimplemented SP register write {:08X}, val: {:08X}\n", addr, value);
+  }
+}
+
+}

src/core/n64/core/RSP.hpp

@@ -0,0 +1,196 @@
+#pragma once
+#include <mmio/MI.hpp>
+#include <RDP.hpp>
+#include <n64/memory_regions.hpp>
+
+namespace natsukashii::n64::core {
+union SPStatus {
+  u32 raw;
+  struct {
+    unsigned halt:1;
+    unsigned broke:1;
+    unsigned dmaBusy:1;
+    unsigned dmaFull:1;
+    unsigned ioFull:1;
+    unsigned singleStep:1;
+    unsigned interruptOnBreak:1;
+    unsigned signal0Set:1;
+    unsigned signal1Set:1;
+    unsigned signal2Set:1;
+    unsigned signal3Set:1;
+    unsigned signal4Set:1;
+    unsigned signal5Set:1;
+    unsigned signal6Set:1;
+    unsigned signal7Set:1;
+    unsigned:17;
+  };
+};
+
+union SPStatusWrite {
+  u32 raw;
+  struct {
+    unsigned clearHalt:1;
+    unsigned setHalt:1;
+    unsigned clearBroke:1;
+    unsigned clearIntr:1;
+    unsigned setIntr:1;
+    unsigned clearSstep:1;
+    unsigned setSstep:1;
+    unsigned clearIntrOnBreak:1;
+    unsigned setIntrOnBreak:1;
+    unsigned clearSignal0:1;
+    unsigned setSignal0:1;
+    unsigned clearSignal1:1;
+    unsigned setSignal1:1;
+    unsigned clearSignal2:1;
+    unsigned setSignal2:1;
+    unsigned clearSignal3:1;
+    unsigned setSignal3:1;
+    unsigned clearSignal4:1;
+    unsigned setSignal4:1;
+    unsigned clearSignal5:1;
+    unsigned setSignal5:1;
+    unsigned clearSignal6:1;
+    unsigned setSignal6:1;
+    unsigned clearSignal7:1;
+    unsigned setSignal7:1;
+    unsigned:7;
+  };
+};
+
+union SPDMALen {
+  struct {
+    unsigned len:12;
+    unsigned count:8;
+    unsigned skip:12;
+  };
+  u32 raw;
+};
+
+union SPDMASPAddr {
+  struct {
+    unsigned address:12;
+    unsigned bank:1;
+    unsigned: 19;
+  };
+  u32 raw;
+};
+
+union SPDMADRAMAddr {
+  struct {
+    unsigned address:24;
+    unsigned:8;
+  };
+  u32 raw;
+};
+
+union VPR {
+  s16 selement[8];
+  u16 element[8];
+  u8 byte[8];
+  u32 word[4];
+};
+
+struct Mem;
+struct Registers;
+
+#define VT(x) (((x) >> 16) & 0x1F)
+#define VS(x) (((x) >> 11) & 0x1F)
+#define VD(x) (((x) >> 6) & 0x1F)
+#define E(x) (((x) >> 21) & 0x1F)
+#define DE(x) (((x) >> 11) & 0x1F)
+#define CLEAR_SET(val, clear, set) do { \
+  if(clear) (val) = 0; \
+  if(set) (val) = 1; \
+} while(0)
+
+struct RSP {
+  RSP() = default;
+  void StepRSP(MI& mi, Registers& regs, RDP& rdp);
+  u32 Read(u32 addr);
+  void Write(Mem& mem, Registers& regs, u32 addr, u32 value);
+  void Exec(MI& mi, Registers& regs, RDP& rdp, u32 instr);
+  SPStatus spStatus{.raw = 1};
+  u16 oldPC{}, pc{}, nextPC = 4;
+  SPDMASPAddr spDMASPAddr{};
+  SPDMADRAMAddr spDMADRAMAddr{};
+  SPDMALen spDMARDLen{}, spDMAWRLen{};
+  u8 dmem[DMEM_SIZE]{}, imem[IMEM_SIZE]{};
+  VPR vpr[32]{};
+  u32 gpr[32]{};
+  u8 vce{};
+
+  struct {
+    VPR h{}, m{}, l{};
+  } acc;
+
+  struct {
+    VPR l{}, h{};
+  } vcc, vco;
+
+  bool semaphore = false;
+
+  inline void SetPC(u16 val) {
+    pc = val;
+    nextPC = val += 4;
+  }
+
+  inline u16 VCOasU16() {
+    u16 val = 0;
+    for(int i = 0; i < 8; i++) {
+      bool h = vco.h.element[7 - i] != 0;
+      bool l = vco.l.element[7 - i] != 0;
+      u32 mask = (l << i) | (h << (i + 8));
+      val |= mask;
+    }
+    return val;
+  }
+
+  inline u16 VCCasU16() {
+    u16 val = 0;
+    for(int i = 0; i < 8; i++) {
+      bool h = vcc.h.element[7 - i] != 0;
+      bool l = vcc.l.element[7 - i] != 0;
+      u32 mask = (l << i) | (h << (i + 8));
+      val |= mask;
+    }
+    return val;
+  }
+private:
+  void add(u32 instr);
+  void addi(u32 instr);
+  void and_(u32 instr);
+  void andi(u32 instr);
+  void cfc2(u32 instr);
+  void b(u32 instr, bool cond);
+  void lh(u32 instr);
+  void lw(u32 instr);
+  void lui(u32 instr);
+  void lqv(u32 instr);
+  void j(u32 instr);
+  void jal(u32 instr);
+  void jr(u32 instr);
+  void nor(u32 instr);
+  void or_(u32 instr);
+  void ori(u32 instr);
+  void sb(u32 instr);
+  void sh(u32 instr);
+  void sw(u32 instr);
+  void sqv(u32 instr);
+  void sllv(u32 instr);
+  void sll(u32 instr);
+  void vabs(u32 instr);
+  void vmov(u32 instr);
+  void veq(u32 instr);
+  void vne(u32 instr);
+  void vsar(u32 instr);
+  void mfc0(RDP& rdp, u32 instr);
+  void mtc0(MI& mi, Registers& regs, RDP& rdp, u32 instr);
+
+  inline void branch(u16 address, bool cond) {
+    if(cond) {
+      nextPC = address & 0xFFF;
+    }
+  }
+};
+}

src/core/n64/core/cpu/Registers.cpp

@@ -1 +1,23 @@
 #include <Registers.hpp>
+
+namespace natsukashii::n64::core {
+Registers::Registers() {
+  delaySlot = false;
+  prevDelaySlot = false;
+  memset(gpr, 0, 32*sizeof(s64));
+  oldPC = (s64)0xFFFFFFFFA4000040;
+  pc = oldPC;
+  nextPC = pc + 4;
+  lo = 0;
+  hi = 0;
+  gpr[11] = (s64)0xFFFFFFFFA4000040;
+  gpr[20] = 0x0000000000000001;
+  gpr[22] = 0x000000000000003F;
+  gpr[29] = (s64)0xFFFFFFFFA4001FF0;
+}
+
+void Registers::SetPC(s64 val) {
+  pc = val;
+  nextPC = pc + 4;
+}
+}

src/core/n64/core/cpu/Registers.hpp

@@ -3,7 +3,21 @@
 
 namespace natsukashii::n64::core {
 struct Registers {
+  Registers();
+  void SetPC(s64);
   s64 gpr[32];
   Cop0 cop0;
+  s64 oldPC, pc, nextPC;
+  s64 hi, lo;
+  bool LLBit;
+  bool prevDelaySlot, delaySlot;
 };
+
+#define RD(x) (((x) >> 11) & 0x1F)
+#define RT(x) (((x) >> 16) & 0x1F)
+#define RS(x) (((x) >> 21) & 0x1F)
+#define FD(x) (((x) >>  6) & 0x1F)
+#define FT(x) RT(x)
+#define FS(x) RD(x)
+#define BASE(x) RS(x)
 }

src/core/n64/core/cpu/registers/Cop0.cpp

@@ -1,17 +1,195 @@
 #include <Cop0.hpp>
+#include <util.hpp>
+#include <n64/core/cpu/Registers.hpp>
+#include <n64/core/Cpu.hpp>
 
 namespace natsukashii::n64::core {
 Cop0::Cop0() {
-
+  cause.raw = 0xB000007C;
+  random = 0x0000001F;
+  status.raw = 0x241000E0;
+  wired = 64;
+  index = 64;
+  PRId = 0x00000B00;
+  Config = 0x7006E463;
+  EPC = 0xFFFFFFFFFFFFFFFF;
+  ErrorEPC = 0xFFFFFFFFFFFFFFFF;
 }
 
 template<class T>
-T Cop0::GetReg(u8 index) {
+T Cop0::GetReg(u8 addr) {
-
+  switch(addr) {
+    case 0: return index & 0x8000001F;
+    case 1: return random & 0x1F;
+    case 2: return entryLo0.raw & 0x3FFFFFFF;
+    case 3: return entryLo1.raw & 0x3FFFFFFF;
+    case 4: return context.raw;
+    case 5: return pageMask.raw;
+    case 6: return wired & 0x3F;
+    case 7: return r7;
+    case 8: return badVaddr;
+    case 9: return count >> 1;
+    case 10: return entryHi.raw & 0xFFFFFFFFFFFFE0FF;
+    case 11: return compare;
+    case 12: return status.raw & STATUS_MASK;
+    case 13: return cause.raw;
+    case 14: return EPC;
+    case 15: return PRId & 0xFFFF;
+    case 16: return Config;
+    case 17: return LLAddr;
+    case 18: return WatchLo;
+    case 19: return WatchHi;
+    case 20: return xcontext.raw & 0xFFFFFFF0;
+    case 21: return r21;
+    case 22: return r22;
+    case 23: return r23;
+    case 24: return r24;
+    case 25: return r25;
+    case 26: return ParityError;
+    case 27: return CacheError;
+    case 28: return TagLo & 0xFFFFFFF;
+    case 29: return TagHi;
+    case 30: return ErrorEPC;
+    case 31: return r31;
+    default:
+      util::panic("Unsupported word read from COP0 register {}\n", index);
+  }
 }
 
 template<class T>
-void Cop0::SetReg(u8 index, T val) {
+void Cop0::SetReg(u8 addr, T value) {
+  switch(addr) {
+    case 0: index = value & 0x8000001F; break;
+    case 1: random = value & 0x1F; break;
+    case 2: entryLo0.raw = value & 0x3FFFFFFF; break;
+    case 3: entryLo1.raw = value & 0x3FFFFFFF; break;
+    case 4: context.raw = value; break;
+    case 5: pageMask.raw = value; break;
+    case 6: wired = value & 0x3F; break;
+    case 7: r7 = value; break;
+    case 9: count = value << 1; break;
+    case 10: entryHi.raw = value & 0xFFFFFFFFFFFFE0FF; break;
+    case 11: {
+      cause.ip7 = 0;
+      compare = value;
+    } break;
+    case 12: status.raw = value & STATUS_MASK; break;
+    case 13: {
+      Cop0Cause tmp{};
+      tmp.raw = value;
+      cause.ip0 = tmp.ip0;
+      cause.ip1 = tmp.ip1;
+    } break;
+    case 14: EPC = value; break;
+    case 15: PRId = value & 0xFFFF; break;
+    case 16: Config = value; break;
+    case 17: LLAddr = value; break;
+    case 18: WatchLo = value; break;
+    case 19: WatchHi = value; break;
+    case 21: r21 = value; break;
+    case 22: r22 = value; break;
+    case 23: r23 = value; break;
+    case 24: r24 = value; break;
+    case 25: r25 = value; break;
+    case 26: ParityError = value; break;
+    case 27: CacheError = value; break;
+    case 28: TagLo = value & 0xFFFFFFF; break;
+    case 29: TagHi = value; break;
+    case 30: ErrorEPC = value; break;
+    case 31: r31 = value; break;
+    default:
+      util::panic("Unsupported word write to COP0 register {}\n", index);
+  }
+}
 
+#define vpn(addr, PageMask) (((((addr) & 0xFFFFFFFFFF) | (((addr) >> 22) & 0x30000000000)) & ~((PageMask) | 0x1FFF)))
+
+TLBEntry* TLBTryMatch(Registers& regs, u32 vaddr, int* match) {
+  for(int i = 0; i < 32; i++) {
+    TLBEntry *entry = &regs.cop0.tlb[i];
+    u64 entry_vpn = vpn(entry->entryHi.raw, entry->pageMask.raw);
+    u64 vaddr_vpn = vpn(vaddr, entry->pageMask.raw);
+
+    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;
+      }
+      return entry;
+    }
+  }
+
+  return nullptr;
+}
+
+bool ProbeTLB(Registers& regs, TLBAccessType access_type, u32 vaddr, u32& paddr, int* match) {
+  TLBEntry* entry = TLBTryMatch(regs, vaddr, match);
+  if(!entry) {
+    regs.cop0.tlbError = MISS;
+    return false;
+  }
+
+  u32 mask = (entry->pageMask.mask << 12) | 0xFFF;
+  u32 odd = vaddr & (mask + 1);
+  u32 pfn;
+
+  if(!odd) {
+    if(!(entry->entryLo0.v)) {
+      regs.cop0.tlbError = INVALID;
+      return false;
+    }
+
+    if(access_type == STORE && !(entry->entryLo0.d)) {
+      regs.cop0.tlbError = MODIFICATION;
+      return false;
+    }
+
+    pfn = entry->entryLo0.pfn;
+  } else {
+    if(!(entry->entryLo1.v)) {
+      regs.cop0.tlbError = INVALID;
+      return false;
+    }
+
+    if(access_type == STORE && !(entry->entryLo1.d)) {
+      regs.cop0.tlbError = MODIFICATION;
+      return false;
+    }
+
+    pfn = entry->entryLo1.pfn;
+  }
+
+  paddr = (pfn << 12) | (vaddr & mask);
+
+  return true;
+}
+
+void HandleTLBException(Registers& regs, u64 vaddr) {
+  u64 vpn2 = (vaddr >> 13) & 0x7FFFF;
+  u64 xvpn2 = (vaddr >> 13) & 0x7FFFFFF;
+  regs.cop0.badVaddr = vaddr;
+  regs.cop0.context.badvpn2 = vpn2;
+  regs.cop0.xcontext.badvpn2 = xvpn2;
+  regs.cop0.xcontext.r = (vaddr >> 62) & 3;
+  regs.cop0.entryHi.vpn2 = xvpn2;
+  regs.cop0.entryHi.r = (vaddr >> 62) & 3;
+}
+
+ExceptionCode GetTLBExceptionCode(TLBError error, TLBAccessType accessType) {
+  switch(error) {
+    case NONE: util::panic("Getting TLB exception with error NONE\n");
+    case INVALID: case MISS:
+      return accessType == LOAD ?
+        ExceptionCode::TLBLoad : ExceptionCode::TLBStore;
+    case MODIFICATION:
+      return ExceptionCode::TLBModification;
+    case DISALLOWED_ADDRESS:
+      return accessType == LOAD ?
+        ExceptionCode::AddressErrorLoad : ExceptionCode::AddressErrorStore;
+    default:
+      util::panic("Getting TLB exception for unknown error code! ({})\n", error);
+  }
 }
 }

src/core/n64/core/cpu/registers/Cop0.hpp

@@ -71,23 +71,23 @@ union Cop0Status {
 union EntryLo {
   u32 raw;
   struct {
-    unsigned g: 1;
+    unsigned g:1;
-    unsigned v: 1;
+    unsigned v:1;
-    unsigned d: 1;
+    unsigned d:1;
-    unsigned c: 3;
+    unsigned c:3;
-    unsigned pfn: 20;
+    unsigned pfn:20;
-    unsigned: 6;
+    unsigned:6;
   };
 };
 
 union EntryHi {
   u64 raw;
   struct {
-    u64 asid: 8;
+    u64 asid:8;
-    u64: 5;
+    u64:5;
-    u64 vpn2: 27;
+    u64 vpn2:27;
-    u64 fill: 22;
+    u64 fill:22;
-    u64 r: 2;
+    u64 r:2;
   } __attribute__((__packed__));
 };
 
@@ -101,9 +101,21 @@ union PageMask {
 };
 
 struct TLBEntry {
-  EntryLo entryLo0, entryLo1;
+  union {
+    u32 raw;
+    struct {
+      unsigned:1;
+      unsigned v:1;
+      unsigned d:1;
+      unsigned c:3;
+      unsigned pfn:20;
+      unsigned:6;
+    };
+  } entryLo0, entryLo1;
   EntryHi entryHi;
   PageMask pageMask;
+
+  bool global;
 };
 
 enum TLBError : u8 {
@@ -118,7 +130,7 @@ enum TLBAccessType {
   LOAD, STORE
 };
 
-union Context {
+union Cop0Context {
   u64 raw;
   struct {
     u64: 4;
@@ -127,7 +139,7 @@ union Context {
   };
 };
 
-union XContext {
+union Cop0XContext {
   u64 raw;
   struct {
     u64: 4;
@@ -141,28 +153,36 @@ struct Cop0 {
   Cop0();
 
   template<class T>
-  T GetReg(u8 index);
+  T GetReg(u8);
 
   template<class T>
-  void SetReg(u8 index, T val);
+  void SetReg(u8, T);
 
-  PageMask pageMask;
+  PageMask pageMask{};
-  EntryHi entryHi;
+  EntryHi entryHi{};
-  EntryLo entryLo0, entryLo1;
+  EntryLo entryLo0{}, entryLo1{};
   u32 index, random;
-  Context context;
+  Cop0Context context{};
-  u32 wired, r7;
+  u32 wired, r7{};
-  u64 badVaddr, count;
+  u64 badVaddr{}, count{};
-  u32 compare;
+  u32 compare{};
-  Cop0Status status;
+  Cop0Status status{};
-  Cop0Cause cause;
+  Cop0Cause cause{};
   u64 EPC;
-  u32 PRId, Config, LLAddr, WatchLo, WatchHi;
+  u32 PRId, Config, LLAddr{}, WatchLo{}, WatchHi{};
-  XContext xcontext;
+  Cop0XContext xcontext{};
-  u32 r21, r22, r23, r24, r25, ParityError, CacheError, TagLo, TagHi;
+  u32 r21{}, r22{}, r23{}, r24{}, r25{}, ParityError{}, CacheError{}, TagLo{}, TagHi{};
   u64 ErrorEPC;
-  u32 r31;
+  u32 r31{};
-  TLBEntry tlb[32];
+  TLBEntry tlb[32]{};
-  TLBError tlbError;
+  TLBError tlbError = NONE;
 };
+
+struct Registers;
+enum class ExceptionCode : u8;
+
+TLBEntry* TLBTryMatch(Registers& regs, u32 vaddr, int* match);
+bool ProbeTLB(Registers& regs, TLBAccessType access_type, u32 vaddr, u32& paddr, int* match);
+void HandleTLBException(Registers& regs, u64 vaddr);
+ExceptionCode GetTLBExceptionCode(TLBError error, TLBAccessType access_type);
 }

src/core/n64/core/rsp/decode.cpp

@@ -0,0 +1,105 @@
+#include <RSP.hpp>
+#include <util.hpp>
+
+namespace natsukashii::n64::core {
+inline void special(RSP& rsp, u32 instr) {
+  u8 mask = instr & 0x3f;
+  switch(mask) {
+    case 0x00: rsp_sll(rsp, instr); break;
+    case 0x04: rsp_sllv(rsp, instr); break;
+    case 0x08: rsp_jr(rsp, instr); break;
+    case 0x0C:
+    case 0x0D:
+      rsp.spStatus.halt = true;
+      rsp.spStatus.broke = true;
+      break;
+    case 0x20: case 0x21:
+      rsp_add(rsp, instr);
+      break;
+    case 0x24: rsp_and_(rsp, instr); break;
+    case 0x25: rsp_or_(rsp, instr); break;
+    case 0x27: rsp_nor(rsp, instr); break;
+    default: util::panic("Unhandled RSP special instruction %d %d\n", (mask >> 3) & 7, mask & 7);
+  }
+}
+
+inline void regimm(RSP& rsp, u32 instr) {
+  u8 mask = ((instr >> 16) & 0x1F);
+  switch(mask) {
+    case 0x00: rsp_b(rsp, instr, (s32)rsp.gpr[RS(instr)] < 0); break;
+    case 0x01: rsp_b(rsp, instr, (s32)rsp.gpr[RS(instr)] >= 0); break;
+    default: util::panic("Unhandled RSP regimm instruction %d %d\n", (mask >> 3) & 3, mask & 7);
+  }
+}
+
+inline void lwc2(RSP& rsp, u32 instr) {
+  u8 mask = (instr >> 11) & 0x1F;
+  switch(mask) {
+    case 0x04: rsp_lqv(rsp, instr); break;
+    default: util::panic("Unhandled RSP LWC2 %d %d\n", (mask >> 3) & 3, mask & 7);
+  }
+}
+
+inline void swc2(RSP& rsp, u32 instr) {
+  u8 mask = (instr >> 11) & 0x1F;
+  switch(mask) {
+    case 0x04: rsp_sqv(rsp, instr); break;
+    default: util::panic("Unhandled RSP SWC2 %d %d\n", (mask >> 3) & 3, mask & 7);
+  }
+}
+
+inline void cop2(RSP& rsp, u32 instr) {
+  u8 mask = instr & 0x3F;
+  u8 mask_sub = (instr >> 21) & 0x1F;
+  switch(mask) {
+    case 0x00:
+      switch(mask_sub) {
+        case 0x02: rsp_cfc2(rsp, instr); break;
+        default: logfatal("Unhandled RSP COP2 sub %d %d\n", (mask_sub >> 3) & 3, mask_sub & 3);
+      }
+      break;
+    case 0x13: rsp_vabs(rsp, instr); break;
+    case 0x1D: rsp_vsar(rsp, instr); break;
+    case 0x21: rsp_veq(rsp, instr); break;
+    case 0x22: rsp_vne(rsp, instr); break;
+    case 0x33: rsp_vmov(rsp, instr); break;
+    default: util::panic("Unhandled RSP COP2 %d %d\n", (mask >> 3) & 7, mask & 7);
+  }
+}
+
+inline void cop0(MI& mi, Registers& regs, RSP& rsp, RDP& rdp, u32 instr) {
+  u8 mask = (instr >> 21) & 0x1F;
+  switch(mask) {
+    case 0x00: rsp_mfc0(rsp, rdp, instr); break;
+    case 0x04: rsp_mtc0(mi, regs, rsp, rdp, instr); break;
+    default: util::panic("Unhandled RSP COP0 %d %d\n", (mask >> 3) & 3, mask & 7);
+  }
+}
+
+void RSP::Exec(MI &mi, Registers &regs, RDP &rdp, u32 instr) {
+  u8 mask = (instr >> 26) & 0x3F;
+  switch(mask) {
+    case 0x00: special(*this, instr); break;
+    case 0x01: regimm(*this, instr); break;
+    case 0x02: rsp_j(instr); break;
+    case 0x03: rsp_jal(instr); break;
+    case 0x04: rsp_b(instr, gpr[RT(instr)] == gpr[RS(instr)]); break;
+    case 0x05: rsp_b(instr, gpr[RT(instr)] != gpr[RS(instr)]); break;
+    case 0x07: rsp_b(instr, gpr[RS(instr)] > 0); break;
+    case 0x08: case 0x09: rsp_addi(instr); break;
+    case 0x0C: rsp_andi(instr); break;
+    case 0x0D: rsp_ori(instr); break;
+    case 0x0F: rsp_lui(instr); break;
+    case 0x10: cop0(mi, regs, *this, rdp, instr); break;
+    case 0x12: cop2(*this, instr); break;
+    case 0x21: rsp_lh(instr); break;
+    case 0x23: rsp_lw(instr); break;
+    case 0x28: rsp_sb(instr); break;
+    case 0x29: rsp_sh(instr); break;
+    case 0x2B: rsp_sw(instr); break;
+    case 0x32: lwc2(*this, instr); break;
+    case 0x3A: swc2(*this, instr); break;
+    default: util::panic("Unhandled RSP instruction %d %d\n", (mask >> 3) & 7, mask & 7);
+  }
+}
+}

src/core/n64/core/rsp/instructions.cpp

@@ -0,0 +1,209 @@
+#include <RSP.hpp>
+#include <util.hpp>
+#include <n64/core/cpu/Registers.hpp>
+
+namespace natsukashii::n64::core {
+
+#define ELEMENT_INDEX(i) (7 - (i))
+#define BYTE_INDEX(i) (15 - (i))
+
+inline bool AcquireSemaphore(RSP& rsp) {
+  if(rsp.semaphore) {
+    return true;
+  } else {
+    rsp.semaphore = true;
+    return false;
+  }
+}
+
+inline void ReleaseSemaphore(RSP& rsp) {
+  rsp.semaphore = false;
+}
+
+inline u32 GetCop0Reg(RSP& rsp, RDP& rdp, u8 index) {
+  switch(index) {
+    case 4: return rsp.spStatus.raw;
+    case 5: return rsp.spStatus.dmaFull;
+    case 6: return 0;
+    case 7: return AcquireSemaphore(rsp);
+    case 11: return rdp.dpc.status.raw;
+    default: util::panic("Unhandled RSP COP0 register read at index {}\n", index);
+  }
+}
+
+inline void SetCop0Reg(MI& mi, Registers& regs, RSP& rsp, RDP& rdp, u8 index, u32 val) {
+  switch(index) {
+    case 0: rsp.spDMASPAddr.raw = val; break;
+    case 1: rsp.spDMADRAMAddr.raw = val; break;
+    case 2: rsp.spDMARDLen.raw = val; break;
+    case 3: rsp.spDMAWRLen.raw = val; break;
+    case 4: rsp.spStatus.raw = val; break;
+    case 7:
+      if(val == 0) {
+        ReleaseSemaphore(rsp);
+      }
+      break;
+    case 8:
+      rdp.dpc.start = val & 0xFFFFF8;
+      rdp.dpc.current = rdp.dpc.start;
+      break;
+    case 9:
+      rdp.dpc.end = val & 0xFFFFF8;
+      rdp.RunCommand(mi, regs, rdp, rsp);
+      break;
+    case 11: rdp.StatusWrite(val); break;
+    default: util::panic("Unhandled RSP COP0 register write at index {}\n", index);
+  }
+}
+
+inline VPR Broadcast(VPR vt, int l0, int l1, int l2, int l3, int l4, int l5, int l6, int l7) {
+  VPR vte{};
+  vte.element[ELEMENT_INDEX(0)] = vt.element[l0];
+  vte.element[ELEMENT_INDEX(1)] = vt.element[l1];
+  vte.element[ELEMENT_INDEX(2)] = vt.element[l2];
+  vte.element[ELEMENT_INDEX(3)] = vt.element[l3];
+  vte.element[ELEMENT_INDEX(4)] = vt.element[l4];
+  vte.element[ELEMENT_INDEX(5)] = vt.element[l5];
+  vte.element[ELEMENT_INDEX(6)] = vt.element[l6];
+  vte.element[ELEMENT_INDEX(7)] = vt.element[l7];
+  return vte;
+}
+
+inline VPR GetVTE(VPR vt, u8 e) {
+  VPR vte{};
+  switch(e & 0xf) {
+    case 0 ... 1: return vt;
+    case 2 ... 3:
+      vte = Broadcast(vt, e - 2, e - 2, e, e, e + 2, e + 2, e + 4, e + 4);
+      break;
+    case 4 ... 7:
+      vte = Broadcast(vt, e - 4, e - 4, e - 4, e - 4, e, e, e, e);
+      break;
+    case 8 ... 15: {
+      int index = e - 8;
+      for (u16& i : vte.element) {
+        i = vt.element[index];
+      }
+    } break;
+  }
+  return vte;
+}
+
+void RSP::add(u32 instr) {
+
+}
+
+void RSP::addi(u32 instr) {
+
+}
+
+void RSP::and_(u32 instr) {
+
+}
+
+void RSP::andi(u32 instr) {
+
+}
+
+void RSP::cfc2(u32 instr) {
+
+}
+
+void RSP::b(u32 instr, bool cond) {
+  s32 address = ((s32)((s16)(instr & 0xFFFF) << 2)) + pc;
+  branch(address, cond);
+}
+
+void RSP::lh(u32 instr) {
+
+}
+
+void RSP::lw(u32 instr) {
+
+}
+
+void RSP::lui(u32 instr) {
+
+}
+
+void RSP::lqv(u32 instr) {
+
+}
+
+void RSP::j(u32 instr) {
+
+}
+
+void RSP::jal(u32 instr) {
+
+}
+
+void RSP::jr(u32 instr) {
+
+}
+
+void RSP::nor(u32 instr) {
+
+}
+
+void RSP::or_(u32 instr) {
+
+}
+
+void RSP::ori(u32 instr) {
+
+}
+
+void RSP::sb(u32 instr) {
+
+}
+
+void RSP::sh(u32 instr) {
+
+}
+
+void RSP::sw(u32 instr) {
+
+}
+
+void RSP::sqv(u32 instr) {
+
+}
+
+void RSP::sllv(u32 instr) {
+  u8 sa = gpr[RS(instr)] & 0x1F;
+  gpr[RD(instr)] = gpr[RT(instr)] << sa;
+}
+
+void RSP::sll(u32 instr) {
+
+}
+
+void RSP::vabs(u32 instr) {
+
+}
+
+void RSP::vmov(u32 instr) {
+
+}
+
+void RSP::veq(u32 instr) {
+
+}
+
+void RSP::vne(u32 instr) {
+
+}
+
+void RSP::vsar(u32 instr) {
+
+}
+
+void RSP::mfc0(RDP& rdp, u32 instr) {
+  gpr[RT(instr)] = GetCop0Reg(*this, rdp, RD(instr));
+}
+
+void RSP::mtc0(MI& mi, Registers& regs, RDP& rdp, u32 instr) {
+  SetCop0Reg(mi, regs, *this, rdp, RD(instr), gpr[RT(instr)]);
+}
+}

src/core/util.hpp

@@ -59,7 +59,7 @@ auto GetSwapFunc(T num) -> T {
 
 template <typename T>
 inline T ReadAccess(u8* data, u32 index) {
-  static_assert(sizeof(T) != 2 && sizeof(T) != 4 && sizeof(T) != 8);
+  static_assert(sizeof(T) != 2 || sizeof(T) != 4 || sizeof(T) != 8);
   T result = 0;
   memcpy(&result, &data[index], sizeof(T));
   return GetSwapFunc<T>(result);
@@ -67,7 +67,7 @@ inline T ReadAccess(u8* data, u32 index) {
 
 template <typename T>
 inline void WriteAccess(u8* data, u32 index, T val) {
-  static_assert(sizeof(T) != 2 && sizeof(T) != 4 && sizeof(T) != 8);
+  static_assert(sizeof(T) != 2 || sizeof(T) != 4 || sizeof(T) != 8);
   T temp = GetSwapFunc<T, true>(val);
   memcpy(&data[index], &temp, sizeof(T));
 }

src/frontend/ParallelRDPWrapper.cpp

@@ -1,13 +1,14 @@
-#include <ParallelRDPWrapper.hpp>
+#include "ParallelRDPWrapper.hpp"
-#include <RDP.hpp>
+#include "n64/core/RDP.hpp"
 #include <memory>
-#include <rdp_device.hpp>
+#include "parallel-rdp-standalone/parallel-rdp/rdp_device.hpp"
 #include <SDL2/SDL_video.h>
 #include <SDL2/SDL_vulkan.h>
-#include <util.hpp>
+#include "util.hpp"
 
-using namespace natsukashii;
 using namespace natsukashii::n64;
+using namespace natsukashii::core;
+using namespace natsukashii::util;
 using namespace Vulkan;
 using namespace RDP;
 using std::unique_ptr;
@@ -64,7 +65,7 @@ void SetFramerateUnlocked(bool unlocked) {
   }
 }
 
-class SDLWSIPlatform : public Vulkan::WSIPlatform {
+class SDLWSIPlatform final : public Vulkan::WSIPlatform {
 public:
   SDLWSIPlatform() = default;
 
@@ -73,7 +74,7 @@ public:
     unsigned int num_extensions = 64;
 
     if (!SDL_Vulkan_GetInstanceExtensions(window, &num_extensions, extensions)) {
-      util::panic("SDL_Vulkan_GetInstanceExtensions failed: %s", SDL_GetError());
+      panic("SDL_Vulkan_GetInstanceExtensions failed: %s", SDL_GetError());
     }
     auto vec = std::vector<const char*>();
 
@@ -87,7 +88,7 @@ public:
   VkSurfaceKHR create_surface(VkInstance instance, VkPhysicalDevice gpu) override {
     VkSurfaceKHR vk_surface;
     if (!SDL_Vulkan_CreateSurface(window, instance, &vk_surface)) {
-      util::panic("Failed to create Vulkan window surface: %s", SDL_GetError());
+      panic("Failed to create Vulkan window surface: %s", SDL_GetError());
     }
     return vk_surface;
   }
@@ -105,26 +106,27 @@ public:
   }
 
   void poll_input() override {
-    SDL_Event e;
+    g_Core->PollInputs(windowID);
-    while(SDL_PollEvent(&e)) {
-
-    }
   }
 
   void event_frame_tick(double frame, double elapsed) override {
-    //n64_render_screen();
+
   }
 };
 
 Program* fullscreen_quad_program;
 
-void LoadParallelRDP(const u8* rdram) {
+void LoadParallelRDP(Platform platform, const u8* rdram) {
   wsi = new WSI();
   wsi->set_backbuffer_srgb(false);
-  wsi->set_platform(new SDLWSIPlatform());
+  if (platform == Platform::SDL) {
+    wsi->set_platform(new SDLWSIPlatform());
+  } else {
+    panic("WSI Qt platform not yet implemented!\n");
+  }
   Context::SystemHandles handles;
   if (!wsi->init(1, handles)) {
-    util::panic("Failed to initialize WSI!");
+    panic("Failed to initialize WSI!");
   }
 
   ResourceLayout vertLayout;
@@ -142,9 +144,9 @@ void LoadParallelRDP(const u8* rdram) {
   fragLayout.sets[0].fp_mask = 1;
   fragLayout.sets[0].array_size[0] = 1;
 
-  u32* fullscreenQuadVert, *fullscreenQuadFrag;
+  u32* fullscreenQuadVert = nullptr, *fullscreenQuadFrag = nullptr;
-  util::ReadFileBinary("external/vert.spv", fullscreenQuadVert);
+  ReadFileBinary("external/vert.spv", fullscreenQuadVert);
-  util::ReadFileBinary("external/frag.spv", fullscreenQuadFrag);
+  ReadFileBinary("external/frag.spv", fullscreenQuadFrag);
 
   fullscreen_quad_program = wsi->get_device().request_program(fullscreenQuadVert, sizeof(fullscreenQuadVert), fullscreenQuadFrag, sizeof(fullscreenQuadFrag), &vertLayout, &fragLayout);
 
@@ -164,7 +166,7 @@ void LoadParallelRDP(const u8* rdram) {
                                            offset, 8 * 1024 * 1024, 4 * 1024 * 1024, flags);
 
   if (!command_processor->device_is_supported()) {
-    util::panic("This device probably does not support 8/16-bit storage. Make sure you're using up-to-date drivers!");
+    panic("This device probably does not support 8/16-bit storage. Make sure you're using up-to-date drivers!");
   }
 }
 
@@ -241,7 +243,7 @@ void UpdateScreen(Util::IntrusivePtr<Image> image) {
   wsi->end_frame();
 }
 
-void UpdateScreenParallelRdp(const n64::core::VI& vi) {
+void UpdateScreenParallelRdp(const core::VI& vi) {
   command_processor->set_vi_register(VIRegister::Control,      vi.status.raw);
   command_processor->set_vi_register(VIRegister::Origin,       vi.origin);
   command_processor->set_vi_register(VIRegister::Width,        vi.width);

src/frontend/ParallelRDPWrapper.hpp

@@ -1,10 +1,19 @@
 #pragma once
-#include <n64/Core.hpp>
+#include "n64/Core.hpp"
-#include <wsi.hpp>
+#include "parallel-rdp-standalone/vulkan/wsi.hpp"
 #include <SDL2/SDL.h>
-#include <n64/core/mmio/VI.hpp>
+#include "n64/core/mmio/VI.hpp"
+#include "BaseCore.hpp"
+
+enum class Platform : bool {
+  SDL, Qt
+};
+
+using namespace natsukashii::n64;
 
 static SDL_Window* window;
+static u32 windowID;
+static std::unique_ptr<natsukashii::core::BaseCore> g_Core;
 VkQueue GetGraphicsQueue();
 VkInstance GetVkInstance();
 VkPhysicalDevice GetVkPhysicalDevice();
@@ -13,8 +22,8 @@ uint32_t GetVkGraphicsQueueFamily();
 VkFormat GetVkFormat();
 VkCommandBuffer GetVkCommandBuffer();
 void SubmitRequestedVkCommandBuffer();
-void LoadParallelRDP(const u8* rdram);
+void LoadParallelRDP(Platform platform, const u8* rdram);
-void UpdateScreenParallelRdp(natsukashii::n64::core::VI& vi);
+void UpdateScreenParallelRdp(core::VI& vi);
 void ParallelRdpEnqueueCommand(int command_length, u32* buffer);
 void ParallelRdpOnFullSync();
 void UpdateScreenParallelRdpNoGame();

src/frontend/qt/CMakeLists.txt

@@ -9,7 +9,7 @@ set(CMAKE_AUTOUIC ON)
 
 find_package(Qt5 COMPONENTS Widgets REQUIRED)
 
-add_executable(natsukashii-qt Frontend.hpp Frontend.cpp main.cpp)
+add_executable(natsukashii-qt Frontend.hpp Frontend.cpp main.cpp ../ParallelRDPWrapper.cpp ../ParallelRDPWrapper.hpp)
 
 target_include_directories(natsukashii-qt PRIVATE . ../../core ../../core/gb ../../core/n64)
 target_link_libraries(natsukashii-qt PRIVATE cores Qt5::Widgets)

src/frontend/sdl/CMakeLists.txt

@@ -5,7 +5,7 @@ set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 find_package(SDL2 REQUIRED)
 
-add_executable(natsukashii-sdl Frontend.cpp Frontend.hpp main.cpp ParallelRDPWrapper.cpp ParallelRDPWrapper.hpp)
+add_executable(natsukashii-sdl Frontend.cpp Frontend.hpp main.cpp ../ParallelRDPWrapper.cpp ../ParallelRDPWrapper.hpp)
 
 include(FetchContent)
 FetchContent_Declare(

src/frontend/sdl/Frontend.cpp

@@ -2,7 +2,7 @@
 #include <gb/Core.hpp>
 #include <n64/Core.hpp>
 #include <volk.h>
-#include <ParallelRDPWrapper.hpp>
+#include "../ParallelRDPWrapper.hpp"
 
 namespace natsukashii::frontend {
 using namespace natsukashii;
@@ -19,24 +19,23 @@ App::App(const std::string& rom, const std::string& selectedCore) {
     window = SDL_CreateWindow("natukashii", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, 800, 600, SDL_WINDOW_RESIZABLE);
     renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_ACCELERATED);
     SDL_RenderSetLogicalSize(renderer, 160, 144);
-    id = SDL_GetWindowID(window);
+    windowID = SDL_GetWindowID(window);
-    core = std::make_unique<gb::core::Core>(rom);
+    g_Core = std::make_unique<gb::core::Core>(rom);
   } else if(selectedCore == "n64") {
     window = SDL_CreateWindow("natukashii", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, 800, 600, SDL_WINDOW_RESIZABLE | SDL_WINDOW_VULKAN);
-    id = SDL_GetWindowID(window);
+    windowID = SDL_GetWindowID(window);
     if(volkInitialize() != VK_SUCCESS) {
       util::panic("Failed to initialize Volk\n");
     }
-    core = std::make_unique<n64::core::Core>(rom);
+    g_Core = std::make_unique<n64::core::Core>(Platform::SDL, rom);
   } else {
     util::panic("Unimplemented core!");
   }
 }
 
 void App::Run() {
-  while(!core->ShouldQuit()) {
+  while(!g_Core->ShouldQuit()) {
-    core->Run();
+    g_Core->Run();
-    core->PollInputs(id);
   }
 }
 }

src/frontend/sdl/Frontend.hpp

@@ -16,7 +16,5 @@ struct App {
   void Run();
 private:
   SDL_Renderer *renderer = nullptr;
-  u32 id;
-  std::unique_ptr<BaseCore> core;
 };
 }