N64 CPU integration

src/core/n64/Core.cpp

@@ -9,7 +9,22 @@ Core::Core(Platform platform, const std::string& rom) {
 }
 
 void Core::Run() {
+  MMIO& mmio = mem.mmio;
+  for(mmio.vi.current = 0; mmio.vi.current < 262; mmio.vi.current++) {
+    for(int i = 0; i < 6000; i++) {
+      cpu.Step(mem);
+      mmio.rsp.Step(mmio.mi, cpu.regs, mmio.rdp);
+      mmio.rsp.Step(mmio.mi, cpu.regs, mmio.rdp);
+      mmio.rsp.Step(mmio.mi, cpu.regs, mmio.rdp);
+      mmio.rsp.Step(mmio.mi, cpu.regs, mmio.rdp);
+    }
 
+    if((mmio.vi.current & 0x3FE) == mmio.vi.intr) {
+      InterruptRaise(mmio.mi, cpu.regs, Interrupt::VI);
+    }
+
+    UpdateScreenParallelRdp(mmio.vi);
+  }
 }
 
 void Core::PollInputs(u32 windowID) {

src/core/n64/core/Mem.hpp

@@ -24,6 +24,7 @@ private:
   friend struct AI;
   friend struct Cpu;
   friend struct RSP;
+  friend struct Core;
   MMIO mmio;
   std::vector<u8> cart, rdram, sram;
   u8 dmem[DMEM_SIZE]{}, imem[IMEM_SIZE]{};

src/core/n64/core/RSP.cpp

@@ -4,7 +4,7 @@
 #include <n64/core/mmio/Interrupt.hpp>
 
 namespace natsukashii::n64::core {
-void RSP::StepRSP(MI& mi, Registers& regs, RDP& rdp) {
+void RSP::Step(MI& mi, Registers& regs, RDP& rdp) {
   if(!spStatus.halt) {
     gpr[0] = 0;
     u32 instr = util::ReadAccess<u32>(imem, pc & IMEM_DSIZE);

src/core/n64/core/RSP.hpp

@@ -106,7 +106,7 @@ struct Registers;
 
 struct RSP {
   RSP() = default;
-  void StepRSP(MI& mi, Registers& regs, RDP& rdp);
+  void Step(MI& mi, Registers& regs, RDP& rdp);
   auto Read(u32 addr) const -> u32;
   void Write(Mem& mem, Registers& regs, u32 addr, u32 value);
   void Exec(MI& mi, Registers& regs, RDP& rdp, u32 instr);

src/core/n64/core/cpu/CMakeLists.txt

@@ -5,7 +5,7 @@ add_library(n64-cpu
   Registers.cpp
   Registers.hpp
   registers/Cop0.cpp
-  registers/Cop0.hpp decode.cpp registers/cop0instructions.cpp registers/Cop1.cpp registers/Cop1.hpp registers/cop1instructions.cpp)
+  registers/Cop0.hpp decode.cpp registers/cop0instructions.cpp registers/Cop1.cpp registers/Cop1.hpp registers/cop1instructions.cpp instructions.cpp)
 
 target_include_directories(n64-cpu PRIVATE . .. ../../ ../../../)
 target_include_directories(n64-cpu PUBLIC registers ../../../../../external)

src/core/n64/core/cpu/decode.cpp

@@ -100,7 +100,7 @@ void Cpu::Exec(Mem& mem, u32 instr) {
     case 0x0E: xori(instr); break;
     case 0x0F: lui(instr); break;
     case 0x10: regs.cop0.decode(regs, mem, instr); break;
-    case 0x11: regs.cop1.decode(regs, instr); break;
+    case 0x11: regs.cop1.decode(*this, instr); break;
     case 0x14: bl(instr, regs.gpr[RS(instr)] == regs.gpr[RT(instr)]); break;
     case 0x15: bl(instr, regs.gpr[RS(instr)] != regs.gpr[RT(instr)]); break;
     case 0x16: bl(instr, regs.gpr[RS(instr)] <= 0); break;
@@ -127,14 +127,14 @@ void Cpu::Exec(Mem& mem, u32 instr) {
     case 0x2E: swr(mem, instr); break;
     case 0x2F: break; // CACHE
     case 0x30: ll(mem, instr); break;
-    case 0x31: lwc1(mem, instr); break;
+    case 0x31: regs.cop1.lwc1(regs, mem, instr); break;
     case 0x34: lld(mem, instr); break;
-    case 0x35: ldc1(mem, instr); break;
+    case 0x35: regs.cop1.ldc1(regs, mem, instr); break;
     case 0x37: ld(mem, instr); break;
     case 0x38: sc(mem, instr); break;
-    case 0x39: swc1(mem, instr); break;
+    case 0x39: regs.cop1.swc1(regs, mem, instr); break;
     case 0x3C: scd(mem, instr); break;
-    case 0x3D: sdc1(mem, instr); break;
+    case 0x3D: regs.cop1.sdc1(regs, mem, instr); break;
     case 0x3F: sd(mem, instr); break;
     default:
       util::panic("Unimplemented instruction {} {}", (mask >> 3) & 7, mask & 7);

src/core/n64/core/cpu/instructions.cpp

@@ -0,0 +1,673 @@
+#include <n64/core/Cpu.hpp>
+#include <util.hpp>
+
+#define se_imm(x) ((s16)((x) & 0xFFFF))
+
+namespace natsukashii::n64::core {
+
+void Cpu::add(u32 instr) {
+  s32 rs = (s32)regs.gpr[RS(instr)];
+  s32 rt = (s32)regs.gpr[RT(instr)];
+  s32 result = rs + rt;
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::addu(u32 instr) {
+  s32 rs = (s32)regs.gpr[RS(instr)];
+  s32 rt = (s32)regs.gpr[RT(instr)];
+  s32 result = rs + rt;
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::addi(u32 instr) {
+  s32 rs = (s32)regs.gpr[RS(instr)];
+  s16 imm = (s16)(instr);
+  s32 result = rs + imm;
+  regs.gpr[RT(instr)] = result;
+}
+
+void Cpu::addiu(u32 instr) {
+  s32 rs = (s32)regs.gpr[RS(instr)];
+  s16 imm = (s16)(instr);
+  s32 result = rs + imm;
+  regs.gpr[RT(instr)] = result;
+}
+
+void Cpu::dadd(u32 instr) {
+  s64 rs = regs.gpr[RS(instr)];
+  s64 rt = regs.gpr[RT(instr)];
+  regs.gpr[RD(instr)] = rs + rt;
+}
+
+void Cpu::daddu(u32 instr) {
+  s64 rs = regs.gpr[RS(instr)];
+  s64 rt = regs.gpr[RT(instr)];
+  regs.gpr[RD(instr)] = rs + rt;
+}
+
+void Cpu::daddi(u32 instr) {
+  s16 imm = (s16)(instr);
+  s64 rs = regs.gpr[RS(instr)];
+  regs.gpr[RT(instr)] = rs + imm;
+}
+
+void Cpu::daddiu(u32 instr) {
+  s16 imm = (s16)(instr);
+  s64 rs = regs.gpr[RS(instr)];
+  regs.gpr[RT(instr)] = rs + imm;
+}
+
+void Cpu::div_(u32 instr) {
+  s64 dividend = (s32)regs.gpr[RS(instr)];
+  s64 divisor = (s32)regs.gpr[RT(instr)];
+
+  if(divisor == 0) {
+    regs.hi = dividend;
+    if(dividend >= 0) {
+      regs.lo = -1;
+    } else {
+      regs.lo = 1;
+    }
+  } else {
+    s64 quotient = dividend / divisor;
+    s64 remainder = dividend % divisor;
+    regs.lo = quotient;
+    regs.hi = remainder;
+  }
+}
+
+void Cpu::divu(u32 instr) {
+  u32 dividend = regs.gpr[RS(instr)];
+  u32 divisor = regs.gpr[RT(instr)];
+  if(divisor == 0) {
+    regs.lo = -1;
+    regs.hi = (s32)dividend;
+  } else {
+    s32 quotient = (s32)(dividend / divisor);
+    s32 remainder = (s32)(dividend % divisor);
+    regs.lo = quotient;
+    regs.hi = remainder;
+  }
+}
+
+void Cpu::ddiv(u32 instr) {
+  s64 dividend = regs.gpr[RS(instr)];
+  s64 divisor = regs.gpr[RT(instr)];
+  if (dividend == 0x8000000000000000 && divisor == 0xFFFFFFFFFFFFFFFF) {
+    regs.lo = dividend;
+    regs.hi = 0;
+  } else if(divisor == 0) {
+    regs.hi = dividend;
+    if(dividend >= 0) {
+      regs.lo = -1;
+    } else {
+      regs.lo = 1;
+    }
+  } else {
+    s64 quotient = dividend / divisor;
+    s64 remainder = dividend % divisor;
+    regs.lo = quotient;
+    regs.hi = remainder;
+  }
+}
+
+void Cpu::ddivu(u32 instr) {
+  u64 dividend = regs.gpr[RS(instr)];
+  u64 divisor = regs.gpr[RT(instr)];
+  if(divisor == 0) {
+    regs.lo = -1;
+    regs.hi = (s64)dividend;
+  } else {
+    u64 quotient = dividend / divisor;
+    u64 remainder = dividend % divisor;
+    regs.lo = (s64)quotient;
+    regs.hi = (s64)remainder;
+  }
+}
+
+void Cpu::branch(bool cond, s64 address) {
+  regs.delaySlot = true;
+  if (cond) {
+    regs.nextPC = address;
+  }
+}
+
+void Cpu::branch_likely(bool cond, s64 address) {
+  regs.delaySlot = true;
+  if (cond) {
+    regs.nextPC = address;
+  } else {
+    regs.SetPC(regs.nextPC);
+  }
+}
+
+void Cpu::b(u32 instr, bool cond) {
+  s64 offset = (s64)se_imm(instr) << 2;
+  s64 address = regs.pc + offset;
+  branch(cond, address);
+}
+
+void Cpu::blink(u32 instr, bool cond) {
+  regs.gpr[31] = regs.nextPC;
+  s64 offset = (s64)se_imm(instr) << 2;
+  s64 address = regs.pc + offset;
+  branch(cond, address);
+}
+
+void Cpu::bl(u32 instr, bool cond) {
+  s64 offset = (s64)se_imm(instr) << 2;
+  s64 address = regs.pc + offset;
+  branch_likely(cond, address);
+}
+
+void Cpu::bllink(u32 instr, bool cond) {
+  regs.gpr[31] = regs.nextPC;
+  s64 offset = (s64)se_imm(instr) << 2;
+  s64 address = regs.pc + offset;
+  branch_likely(cond, address);
+}
+
+void Cpu::lui(u32 instr) {
+  s64 val = (s16)instr;
+  val *= 65536;
+  regs.gpr[RT(instr)] = val;
+}
+
+void Cpu::lb(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  regs.gpr[RT(instr)] = s64(mem.Read<s8>(regs, address, regs.oldPC));
+}
+
+void Cpu::lh(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 1) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
+  }
+
+  regs.gpr[RT(instr)] = (s64)(s16)mem.Read<u16>(regs, address, regs.oldPC);
+}
+
+void Cpu::lw(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 3) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
+  }
+
+  s32 value = mem.Read<s32>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = value;
+}
+
+void Cpu::ll(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 3) != 0) {
+    HandleTLBException(regs, s64(s32(address)));
+    FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
+  }
+
+  regs.LLBit = true;
+  regs.cop0.LLAddr = address;
+
+  s32 value = mem.Read<s32>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = value;
+}
+
+void Cpu::lwl(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  u32 shift = 8 * ((address ^ 0) & 3);
+  u32 mask = 0xFFFFFFFF << shift;
+  u32 data = mem.Read<u32>(regs, address & ~3, regs.oldPC);
+  s64 rt = regs.gpr[RT(instr)];
+  s32 result = (s32)((rt & ~mask) | (data << shift));
+  regs.gpr[RT(instr)] = result;
+}
+
+void Cpu::lwr(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  u32 shift = 8 * ((address ^ 3) & 3);
+  u32 mask = 0xFFFFFFFF >> shift;
+  u32 data = mem.Read<u32>(regs, address & ~3, regs.oldPC);
+  s64 rt = regs.gpr[RT(instr)];
+  s32 result = (s32)((rt & ~mask) | (data >> shift));
+  regs.gpr[RT(instr)] = result;
+}
+
+void Cpu::ld(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 7) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
+  }
+
+  s64 value = mem.Read<s64>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = value;
+}
+
+void Cpu::lld(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 7) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
+  }
+
+  regs.LLBit = true;
+  regs.cop0.LLAddr = address;
+
+  s64 value = mem.Read<s64>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = value;
+}
+
+void Cpu::ldl(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  s32 shift = 8 * ((address ^ 0) & 7);
+  u64 mask = 0xFFFFFFFFFFFFFFFF << shift;
+  u64 data = mem.Read<u64>(regs, address & ~7, regs.oldPC);
+  s64 rt = regs.gpr[RT(instr)];
+  s64 result = (s64)((rt & ~mask) | (data << shift));
+  regs.gpr[RT(instr)] = result;
+}
+
+void Cpu::ldr(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  s32 shift = 8 * ((address ^ 7) & 7);
+  u64 mask = 0xFFFFFFFFFFFFFFFF >> shift;
+  u64 data = mem.Read<u64>(regs, address & ~7, regs.oldPC);
+  s64 rt = regs.gpr[RT(instr)];
+  s64 result = (s64)((rt & ~mask) | (data >> shift));
+  regs.gpr[RT(instr)] = result;
+}
+
+void Cpu::lbu(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  u8 value = mem.Read<u8>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = value;
+}
+
+void Cpu::lhu(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 1) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
+  }
+
+  u16 value = mem.Read<u16>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = value;
+}
+
+void Cpu::lwu(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 3) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
+  }
+
+  u32 value = mem.Read<u32>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = value;
+}
+
+void Cpu::sb(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  mem.Write<u8>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+}
+
+void Cpu::sc(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 3) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
+  }
+
+  if(regs.LLBit) {
+    mem.Write<u32>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+  }
+
+  regs.gpr[RT(instr)] = (s64)((u64)regs.LLBit);
+  regs.LLBit = false;
+}
+
+void Cpu::scd(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 7) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
+  }
+
+  if(regs.LLBit) {
+    mem.Write<u64>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+  }
+
+  regs.gpr[RT(instr)] = (s64)((u64)regs.LLBit);
+  regs.LLBit = false;
+}
+
+void Cpu::sh(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 1) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
+  }
+
+  mem.Write<u16>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+}
+
+void Cpu::sw(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 3) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
+  }
+
+  mem.Write<u32>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+}
+
+void Cpu::sd(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  if ((address & 7) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
+  }
+
+  mem.Write<u64>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+}
+
+void Cpu::sdl(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  s32 shift = 8 * ((address ^ 0) & 7);
+  u64 mask = 0xFFFFFFFFFFFFFFFF >> shift;
+  u64 data = mem.Read<u64>(regs, address & ~7, regs.oldPC);
+  s64 rt = regs.gpr[RT(instr)];
+  mem.Write<u64>(regs, address & ~7, (data & ~mask) | (rt >> shift), regs.oldPC);
+}
+
+void Cpu::sdr(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  s32 shift = 8 * ((address ^ 7) & 7);
+  u64 mask = 0xFFFFFFFFFFFFFFFF << shift;
+  u64 data = mem.Read<u64>(regs, address & ~7, regs.oldPC);
+  s64 rt = regs.gpr[RT(instr)];
+  mem.Write<u64>(regs, address & ~7, (data & ~mask) | (rt << shift), regs.oldPC);
+}
+
+void Cpu::swl(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  u32 shift = 8 * ((address ^ 0) & 3);
+  u32 mask = 0xFFFFFFFF >> shift;
+  u32 data = mem.Read<u32>(regs, address & ~3, regs.oldPC);
+  u32 rt = regs.gpr[RT(instr)];
+  mem.Write<u32>(regs, address & ~3, (data & ~mask) | (rt >> shift), regs.oldPC);
+}
+
+void Cpu::swr(Mem& mem, u32 instr) {
+  u32 address = regs.gpr[RS(instr)] + (s16)instr;
+  u32 shift = 8 * ((address ^ 3) & 3);
+  u32 mask = 0xFFFFFFFF << shift;
+  u32 data = mem.Read<u32>(regs, address & ~3, regs.oldPC);
+  u32 rt = regs.gpr[RT(instr)];
+  mem.Write<u32>(regs, address & ~3, (data & ~mask) | (rt << shift), regs.oldPC);
+}
+
+void Cpu::ori(u32 instr) {
+  s64 imm = (u16)instr;
+  s64 result = imm | regs.gpr[RS(instr)];
+  regs.gpr[RT(instr)] = result;
+}
+
+void Cpu::or_(u32 instr) {
+  regs.gpr[RD(instr)] = regs.gpr[RS(instr)] | regs.gpr[RT(instr)];
+}
+
+void Cpu::nor(u32 instr) {
+  regs.gpr[RD(instr)] = ~(regs.gpr[RS(instr)] | regs.gpr[RT(instr)]);
+}
+
+void Cpu::j(u32 instr) {
+  u32 target = (instr & 0x3ffffff) << 2;
+  u32 address = (regs.oldPC & ~0xfffffff) | target;
+  if ((address & 3) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::DataBusError, 0, regs.oldPC);
+  }
+
+  branch(true, address);
+}
+
+void Cpu::jal(u32 instr) {
+  regs.gpr[31] = regs.nextPC;
+  j(instr);
+}
+
+void Cpu::jalr(u32 instr) {
+  branch(true, regs.gpr[RS(instr)]);
+  regs.gpr[RD(instr)] = regs.pc + 4;
+}
+
+void Cpu::slti(u32 instr) {
+  regs.gpr[RT(instr)] = regs.gpr[RS(instr)] < se_imm((s64)instr);
+}
+
+void Cpu::sltiu(u32 instr) {
+  regs.gpr[RT(instr)] = (u64)regs.gpr[RS(instr)] < se_imm((s64)instr);
+}
+
+void Cpu::slt(u32 instr) {
+  regs.gpr[RD(instr)] = regs.gpr[RS(instr)] < regs.gpr[RT(instr)];
+}
+
+void Cpu::sltu(u32 instr) {
+  regs.gpr[RD(instr)] = (u64)regs.gpr[RS(instr)] < (u64)regs.gpr[RT(instr)];
+}
+
+void Cpu::xori(u32 instr) {
+  s64 imm = (u16)instr;
+  regs.gpr[RT(instr)] = regs.gpr[RS(instr)] ^ imm;
+}
+
+void Cpu::xor_(u32 instr) {
+  regs.gpr[RD(instr)] = regs.gpr[RT(instr)] ^ regs.gpr[RS(instr)];
+}
+
+void Cpu::andi(u32 instr) {
+  s64 imm = (u16)instr;
+  regs.gpr[RT(instr)] = regs.gpr[RS(instr)] & imm;
+}
+
+void Cpu::and_(u32 instr) {
+  regs.gpr[RD(instr)] = regs.gpr[RS(instr)] & regs.gpr[RT(instr)];
+}
+
+void Cpu::sll(u32 instr) {
+  u8 sa = ((instr >> 6) & 0x1f);
+  s32 result = regs.gpr[RT(instr)] << sa;
+  regs.gpr[RD(instr)] = (s64)result;
+}
+
+void Cpu::sllv(u32 instr) {
+  u8 sa = (regs.gpr[RS(instr)]) & 0x1F;
+  u32 rt = regs.gpr[RT(instr)];
+  s32 result = rt << sa;
+  regs.gpr[RD(instr)] = (s64)result;
+}
+
+void Cpu::dsll32(u32 instr) {
+  u8 sa = ((instr >> 6) & 0x1f);
+  s64 result = regs.gpr[RT(instr)] << (sa + 32);
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::dsll(u32 instr) {
+  u8 sa = ((instr >> 6) & 0x1f);
+  s64 result = regs.gpr[RT(instr)] << sa;
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::dsllv(u32 instr) {
+  s64 sa = regs.gpr[RS(instr)] & 63;
+  s64 result = regs.gpr[RT(instr)] << sa;
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::srl(u32 instr) {
+  u32 rt = regs.gpr[RT(instr)];
+  u8 sa = ((instr >> 6) & 0x1f);
+  u32 result = rt >> sa;
+  regs.gpr[RD(instr)] = (s32)result;
+}
+
+void Cpu::srlv(u32 instr) {
+  u8 sa = (regs.gpr[RS(instr)] & 0x1F);
+  u32 rt = regs.gpr[RT(instr)];
+  s32 result = rt >> sa;
+  regs.gpr[RD(instr)] = (s64)result;
+}
+
+void Cpu::dsrl(u32 instr) {
+  u64 rt = regs.gpr[RT(instr)];
+  u8 sa = ((instr >> 6) & 0x1f);
+  u64 result = rt >> sa;
+  regs.gpr[RD(instr)] = s64(result);
+}
+
+void Cpu::dsrlv(u32 instr) {
+  u8 amount = (regs.gpr[RS(instr)] & 63);
+  u64 rt = regs.gpr[RT(instr)];
+  u64 result = rt >> amount;
+  regs.gpr[RD(instr)] = s64(result);
+}
+
+void Cpu::dsrl32(u32 instr) {
+  u64 rt = regs.gpr[RT(instr)];
+  u8 sa = ((instr >> 6) & 0x1f);
+  u64 result = rt >> (sa + 32);
+  regs.gpr[RD(instr)] = s64(result);
+}
+
+void Cpu::sra(u32 instr) {
+  s64 rt = regs.gpr[RT(instr)];
+  u8 sa = ((instr >> 6) & 0x1f);
+  s32 result = rt >> sa;
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::srav(u32 instr) {
+  s64 rt = regs.gpr[RT(instr)];
+  s64 rs = regs.gpr[RS(instr)];
+  u8 sa = rs & 0x1f;
+  s32 result = rt >> sa;
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::dsra(u32 instr) {
+  s64 rt = regs.gpr[RT(instr)];
+  u8 sa = ((instr >> 6) & 0x1f);
+  s64 result = rt >> sa;
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::dsrav(u32 instr) {
+  s64 rt = regs.gpr[RT(instr)];
+  s64 rs = regs.gpr[RS(instr)];
+  s64 sa = rs & 63;
+  s64 result = rt >> sa;
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::dsra32(u32 instr) {
+  s64 rt = regs.gpr[RT(instr)];
+  u8 sa = ((instr >> 6) & 0x1f);
+  s64 result = rt >> (sa + 32);
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::jr(u32 instr) {
+  u32 address = regs.gpr[RS(instr)];
+  if ((address & 3) != 0) {
+    HandleTLBException(regs, (s64)((s32)address));
+    FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
+  }
+
+  branch(true, address);
+}
+
+void Cpu::dsub(u32 instr) {
+  s64 rt = regs.gpr[RT(instr)];
+  s64 rs = regs.gpr[RS(instr)];
+  s64 result = rs - rt;
+  regs.gpr[RD(instr)] = result;
+}
+
+void Cpu::dsubu(u32 instr) {
+  u64 rt = regs.gpr[RT(instr)];
+  u64 rs = regs.gpr[RS(instr)];
+  u64 result = rs - rt;
+  regs.gpr[RD(instr)] = s64(result);
+}
+
+void Cpu::sub(u32 instr) {
+  s32 rt = regs.gpr[RT(instr)];
+  s32 rs = regs.gpr[RS(instr)];
+  s32 result = rs - rt;
+  regs.gpr[RD(instr)] = (s64)result;
+}
+
+void Cpu::subu(u32 instr) {
+  u32 rt = regs.gpr[RT(instr)];
+  u32 rs = regs.gpr[RS(instr)];
+  u32 result = rs - rt;
+  regs.gpr[RD(instr)] = (s64)((s32)result);
+}
+
+void Cpu::dmultu(u32 instr) {
+  u64 rt = regs.gpr[RT(instr)];
+  u64 rs = regs.gpr[RS(instr)];
+  u128 result = (u128)rt * (u128)rs;
+  regs.lo = (s64)(result & 0xFFFFFFFFFFFFFFFF);
+  regs.hi = (s64)(result >> 64);
+}
+
+void Cpu::dmult(u32 instr) {
+  s64 rt = regs.gpr[RT(instr)];
+  s64 rs = regs.gpr[RS(instr)];
+  s128 result = (s128)rt * (s128)rs;
+  regs.lo = result & 0xFFFFFFFFFFFFFFFF;
+  regs.hi = result >> 64;
+}
+
+void Cpu::multu(u32 instr) {
+  u32 rt = regs.gpr[RT(instr)];
+  u32 rs = regs.gpr[RS(instr)];
+  u64 result = (u64)rt * (u64)rs;
+  regs.lo = (s64)((s32)result);
+  regs.hi = (s64)((s32)(result >> 32));
+}
+
+void Cpu::mult(u32 instr) {
+  s32 rt = regs.gpr[RT(instr)];
+  s32 rs = regs.gpr[RS(instr)];
+  s64 result = (s64)rt * (s64)rs;
+  regs.lo = (s64)((s32)result);
+  regs.hi = (s64)((s32)(result >> 32));
+}
+
+void Cpu::mflo(u32 instr) {
+  regs.gpr[RD(instr)] = regs.lo;
+}
+
+void Cpu::mfhi(u32 instr) {
+  regs.gpr[RD(instr)] = regs.hi;
+}
+
+void Cpu::mtlo(u32 instr) {
+  regs.lo = regs.gpr[RS(instr)];
+}
+
+void Cpu::mthi(u32 instr) {
+  regs.hi = regs.gpr[RS(instr)];
+}
+
+void Cpu::trap(bool cond) {
+  if(cond) {
+    FireException(regs, ExceptionCode::Trap, 0, regs.oldPC);
+  }
+}
+}

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

@@ -207,7 +207,7 @@ void Cop0::decode(Registers& regs, Mem& mem, u32 instr) {
         case 0x02: tlbwi(regs); break;
         case 0x08: tlbp(regs); break;
         case 0x18: eret(regs); break;
-        default: util::panic("Unimplemented COP0 function {} {} ({:08X}) ({:016lX})", mask_cop2 >> 3, mask_cop2 & 7, instr, regs->old_pc);
+        default: util::panic("Unimplemented COP0 function {} {} ({:08X}) ({:016lX})", mask_cop2 >> 3, mask_cop2 & 7, instr, regs.oldPC);
       }
       break;
     default: util::panic("Unimplemented COP0 instruction {} {}", mask_cop >> 4, mask_cop & 7);

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

@@ -54,6 +54,7 @@ struct Cop1 {
   FCR31 fcr31;
   FGR fgr[32];
   void decode(Cpu&, u32);
+  friend struct Cpu;
 private:
   template <typename T>
   inline void SetReg(Cop0& cop0, u8 index, T value) {

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

@@ -1,5 +1,8 @@
 #include <n64/core/cpu/registers/Cop1.hpp>
 #include <n64/core/cpu/Registers.hpp>
+#include <n64/core/Mem.hpp>
+#include <util.hpp>
+#include <math.h>
 
 namespace natsukashii::n64::core {
 void Cop1::absd(Registers& regs, u32 instr) {
@@ -13,243 +16,463 @@ void Cop1::abss(Registers& regs, u32 instr) {
 }
 
 void Cop1::absw(Registers& regs, u32 instr) {
-
+  s32 fs = GetReg<s32>(regs.cop0, FS(instr));
+  SetReg<u32>(regs.cop0, FD(instr), abs(fs));
 }
 
 void Cop1::absl(Registers& regs, u32 instr) {
-
+  s64 fs = GetReg<s64>(regs.cop0, FS(instr));
+  SetReg(regs.cop0, FD(instr), labs(fs));
 }
 
 void Cop1::adds(Registers& regs, u32 instr) {
-
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  float ft = GetCop1RegFloat(regs.cop0, FT(instr));
+  float result = fs + ft;
+  SetCop1RegFloat(regs.cop0, FD(instr), result);
 }
 
 void Cop1::addd(Registers& regs, u32 instr) {
-
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
-}
+  double ft = GetCop1RegDouble(regs.cop0, FT(instr));
-
+  double result = fs + ft;
-void Cop1::subs(Registers& regs, u32 instr) {
+  SetCop1RegDouble(regs.cop0, FD(instr), result);
-
-}
-
-void Cop1::subd(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::subw(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::subl(Registers& regs, u32 instr) {
-
 }
 
 void Cop1::ceills(Registers& regs, u32 instr) {
-
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  s64 result = ceilf(fs);
+  SetReg<u64>(regs.cop0, FD(instr), result);
 }
 
 void Cop1::ceilws(Registers& regs, u32 instr) {
-
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  s32 result = ceilf(fs);
+  SetReg<u32>(regs.cop0, FD(instr), result);
 }
 
 void Cop1::ceilld(Registers& regs, u32 instr) {
-
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  s64 result = ceil(fs);
+  SetReg<u64>(regs.cop0, FD(instr), result);
 }
 
 void Cop1::ceilwd(Registers& regs, u32 instr) {
-
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  s32 result = ceil(fs);
+  SetReg<u32>(regs.cop0, FD(instr), result);
 }
 
 void Cop1::cfc1(Registers& regs, u32 instr) {
-
+  u8 fd = FD(instr);
+  s32 val = 0;
+  switch(fd) {
+    case 0: val = (s32)fcr0; break;
+    case 31: val = (s32)fcr31.raw; break;
+    default: util::panic("Undefined CFC1 with rd != 0 or 31\n");
+  }
+  regs.gpr[RT(instr)] = val;
 }
 
 void Cop1::ctc1(Registers& regs, u32 instr) {
-
+  u8 fs = FS(instr);
-}
+  u32 val = regs.gpr[RT(instr)];
-
+  switch(fs) {
-void Cop1::roundls(Registers& regs, u32 instr) {
+    case 0: util::panic("CTC1 attempt to write to FCR0 which is read only!\n");
-
+    case 31: {
-}
+      val &= 0x183ffff;
-
+      fcr31.raw = val;
-void Cop1::roundld(Registers& regs, u32 instr) {
+    } break;
-
+    default: util::panic("Undefined CTC1 with rd != 0 or 31\n");
-}
+  }
-
-void Cop1::roundws(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::roundwd(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::floorls(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::floorld(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::floorws(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::floorwd(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::cvtls(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::cvtws(Registers& regs, u32 instr) {
-
 }
 
 void Cop1::cvtds(Registers& regs, u32 instr) {
-
+  SetCop1RegDouble(
-}
+        regs.cop0,
-
+    FD(instr),
-void Cop1::cvtsw(Registers& regs, u32 instr) {
+    GetCop1RegFloat(
-
+            regs.cop0,
-}
+      FS(instr)
-
+    )
-void Cop1::cvtdw(Registers& regs, u32 instr) {
+  );
-
 }
 
 void Cop1::cvtsd(Registers& regs, u32 instr) {
-
+  SetCop1RegFloat(
+        regs.cop0,
+    FD(instr),
+    GetCop1RegDouble(
+            regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
 void Cop1::cvtwd(Registers& regs, u32 instr) {
-
+  SetReg<u32>(
+        regs.cop0,
+    FD(instr),
+    GetCop1RegDouble(
+            regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
-void Cop1::cvtld(Registers& regs, u32 instr) {
+void Cop1::cvtws(Registers& regs, u32 instr) {
-
+  SetReg<u32>(
+        regs.cop0,
+    FD(instr),
+    GetCop1RegFloat(
+            regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
-void Cop1::cvtdl(Registers& regs, u32 instr) {
+void Cop1::cvtls(Registers& regs, u32 instr) {
-
+  SetReg<u64>(
+        regs.cop0,
+    FD(instr),
+    GetCop1RegFloat(
+            regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
 void Cop1::cvtsl(Registers& regs, u32 instr) {
+  SetCop1RegFloat(
+        regs.cop0,
+    FD(instr),
+    (s64)GetReg<u64>(
+            regs.cop0,
+      FS(instr)
+    )
+  );
+}
 
+void Cop1::cvtdw(Registers& regs, u32 instr) {
+  SetCop1RegDouble(
+    regs.cop0,
+    FD(instr),
+    (s32)GetReg<u32>(
+      regs.cop0,
+      FS(instr)
+    )
+  );
+}
+
+void Cop1::cvtsw(Registers& regs, u32 instr) {
+  SetCop1RegFloat(
+        regs.cop0,
+    FD(instr),
+    (s32)GetReg<u32>(
+            regs.cop0,
+      FS(instr)
+    )
+  );
+}
+
+void Cop1::cvtdl(Registers& regs, u32 instr) {
+  SetCop1RegDouble(
+        regs.cop0,
+    FD(instr),
+    (s64)GetReg<u64>(
+            regs.cop0,
+      FS(instr)
+    )
+  );
+}
+
+void Cop1::cvtld(Registers& regs, u32 instr) {
+  SetReg<u64>(
+        regs.cop0,
+    FD(instr),
+    GetCop1RegDouble(
+            regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
 void Cop1::ccondd(Registers& regs, u32 instr, CompConds cond) {
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  double ft = GetCop1RegDouble(regs.cop0, FT(instr));
 
+  bool less, equal, unordered;
+  if(isnan(fs) || isnan(ft)) {
+    less = false;
+    equal = false;
+    unordered = true;
+  } else {
+    less = fs < ft;
+    equal = fs == ft;
+    unordered = false;
+  }
+
+  bool condition = ((cond >> 2) && less) || ((cond >> 1) && equal) || ((cond & 1) && unordered);
+
+  fcr31.compare = condition;
 }
 
 void Cop1::cconds(Registers& regs, u32 instr, CompConds cond) {
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  float ft = GetCop1RegFloat(regs.cop0, FT(instr));
 
+  bool less, equal, unordered;
+  if(isnan(fs) || isnan(ft)) {
+    less = false;
+    equal = false;
+    unordered = true;
+  } else {
+    less = fs < ft;
+    equal = fs == ft;
+    unordered = false;
+  }
+
+  bool condition = ((cond >> 2) && less) || ((cond >> 1) && equal) || ((cond & 1) && unordered);
+
+  fcr31.compare = condition;
 }
 
-void Cop1::divs(Registers& regs, u32 instr) {
+void Cop1::divs(Registers &regs, u32 instr) {
-
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  float ft = GetCop1RegFloat(regs.cop0, FT(instr));
+  SetCop1RegFloat(regs.cop0, FD(instr), fs / ft);
 }
 
-void Cop1::divd(Registers& regs, u32 instr) {
+void Cop1::divd(Registers &regs, u32 instr) {
-
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  double ft = GetCop1RegDouble(regs.cop0, FT(instr));
+  SetCop1RegDouble(regs.cop0, FD(instr), fs / ft);
 }
 
-void Cop1::muls(Registers& regs, u32 instr) {
+void Cop1::muls(Registers &regs, u32 instr) {
-
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  float ft = GetCop1RegFloat(regs.cop0, FT(instr));
+  SetCop1RegFloat(regs.cop0, FD(instr), fs * ft);
 }
 
 void Cop1::muld(Registers& regs, u32 instr) {
-
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  double ft = GetCop1RegDouble(regs.cop0, FT(instr));
+  SetCop1RegDouble(regs.cop0, FD(instr), fs * ft);
 }
 
-void Cop1::mulw(Registers& regs, u32 instr) {
+void Cop1::mulw(Registers &regs, u32 instr) {
-
+  u32 fs = GetReg<u32>(regs.cop0, FS(instr));
+  u32 ft = GetReg<u32>(regs.cop0, FT(instr));
+  SetReg<u32>(regs.cop0, FD(instr), fs * ft);
 }
 
-void Cop1::mull(Registers& regs, u32 instr) {
+void Cop1::mull(Registers &regs, u32 instr) {
+  u64 fs = GetReg<u64>(regs.cop0, FS(instr));
+  u64 ft = GetReg<u64>(regs.cop0, FT(instr));
+  SetReg<u64>(regs.cop0, FD(instr), fs * ft);
+}
 
+void Cop1::subs(Registers &regs, u32 instr) {
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  float ft = GetCop1RegFloat(regs.cop0, FT(instr));
+  SetCop1RegFloat(regs.cop0, FD(instr), fs - ft);
+}
+
+void Cop1::subd(Registers &regs, u32 instr) {
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  double ft = GetCop1RegDouble(regs.cop0, FT(instr));
+  SetCop1RegDouble(regs.cop0, FD(instr), fs - ft);
+}
+
+void Cop1::subw(Registers &regs, u32 instr) {
+  u32 fs = GetReg<u32>(regs.cop0, FS(instr));
+  u32 ft = GetReg<u32>(regs.cop0, FT(instr));
+  SetReg<u32>(regs.cop0, FD(instr), fs - ft);
+}
+
+void Cop1::subl(Registers &regs, u32 instr) {
+  u64 fs = GetReg<u64>(regs.cop0, FS(instr));
+  u64 ft = GetReg<u64>(regs.cop0, FT(instr));
+  SetReg<u64>(regs.cop0, FD(instr), fs - ft);
 }
 
 void Cop1::movs(Registers& regs, u32 instr) {
-
+  SetCop1RegFloat(
+    regs.cop0,
+    FD(instr),
+    GetCop1RegFloat(
+      regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
 void Cop1::movd(Registers& regs, u32 instr) {
-
+  SetCop1RegDouble(
+    regs.cop0,
+    FD(instr),
+    GetCop1RegDouble(
+      regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
 void Cop1::movw(Registers& regs, u32 instr) {
-
+  SetReg<u32>(
+    regs.cop0,
+    FD(instr),
+    GetReg<u32>(
+      regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
 void Cop1::movl(Registers& regs, u32 instr) {
-
+  SetReg<u64>(
+    regs.cop0,
+    FD(instr),
+    GetReg<u64>(
+      regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
-void Cop1::negs(Registers& regs, u32 instr) {
+void Cop1::negs(Registers &regs, u32 instr) {
-
+  SetCop1RegFloat(
+    regs.cop0,
+    FD(instr),
+    -GetCop1RegFloat(
+      regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
-void Cop1::negd(Registers& regs, u32 instr) {
+void Cop1::negd(Registers &regs, u32 instr) {
-
+  SetCop1RegDouble(
+    regs.cop0,
+    FD(instr),
+    -GetCop1RegDouble(
+      regs.cop0,
+      FS(instr)
+    )
+  );
 }
 
-void Cop1::sqrts(Registers& regs, u32 instr) {
+void Cop1::sqrts(Registers &regs, u32 instr) {
-
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  SetCop1RegFloat(regs.cop0, FD(instr), sqrtf(fs));
 }
 
-void Cop1::sqrtd(Registers& regs, u32 instr) {
+void Cop1::sqrtd(Registers &regs, u32 instr) {
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  SetCop1RegDouble(regs.cop0, FD(instr), sqrt(fs));
+}
 
+void Cop1::roundls(Registers& regs, u32 instr) {
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  SetReg<u64>(regs.cop0, FD(instr), (s32)roundf(fs));
+}
+
+void Cop1::roundld(Registers& regs, u32 instr) {
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  SetReg<u64>(regs.cop0, FD(instr), (s64)round(fs));
+}
+
+void Cop1::roundws(Registers& regs, u32 instr) {
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  SetReg<u32>(regs.cop0, FD(instr), (s32)roundf(fs));
+}
+
+void Cop1::roundwd(Registers& regs, u32 instr) {
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  SetReg<u32>(regs.cop0, FD(instr), (s64)round(fs));
+}
+
+void Cop1::floorls(Registers& regs, u32 instr) {
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  SetReg<u64>(regs.cop0, FD(instr), (s64)floorf(fs));
+}
+
+void Cop1::floorld(Registers& regs, u32 instr) {
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  SetReg<u64>(regs.cop0, FD(instr), (s64)floor(fs));
+}
+
+void Cop1::floorws(Registers& regs, u32 instr) {
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  SetReg<u32>(regs.cop0, FD(instr), (s64)floorf(fs));
+}
+
+void Cop1::floorwd(Registers& regs, u32 instr) {
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  SetReg<u32>(regs.cop0, FD(instr), (s64)floor(fs));
 }
 
 void Cop1::lwc1(Registers& regs, Mem& mem, u32 instr) {
-
+  u32 addr = (s64)(s16)instr + regs.gpr[BASE(instr)];
+  u32 data = mem.Read<u32>(regs, addr, regs.oldPC);
+  SetReg<u32>(regs.cop0, FT(instr), data);
 }
 
 void Cop1::swc1(Registers& regs, Mem& mem, u32 instr) {
-
+  u32 addr = (s64)(s16)instr + regs.gpr[BASE(instr)];
+  mem.Write<u32>(regs, addr, GetReg<u32>(regs.cop0, FT(instr)), regs.oldPC);
 }
 
 void Cop1::ldc1(Registers& regs, Mem& mem, u32 instr) {
-
+  u32 addr = (s64)(s16)instr + regs.gpr[BASE(instr)];
-}
+  u64 data = mem.Read<u64>(regs, addr, regs.oldPC);
-
+  SetReg<u64>(regs.cop0, FT(instr), data);
-void Cop1::mfc1(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::dmfc1(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::mtc1(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::dmtc1(Registers& regs, u32 instr) {
-
-}
-
-void Cop1::sdc1(Registers& regs, Mem&, u32 instr) {
-
 }
 
 void Cop1::truncws(Registers& regs, u32 instr) {
-
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  s32 result = (s32)truncf(fs);
+  SetReg<u32>(regs.cop0, FD(instr), result);
 }
 
 void Cop1::truncwd(Registers& regs, u32 instr) {
-
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  s32 result = (s32)trunc(fs);
+  SetReg<u32>(regs.cop0, FD(instr), result);
 }
 
 void Cop1::truncls(Registers& regs, u32 instr) {
-
+  float fs = GetCop1RegFloat(regs.cop0, FS(instr));
+  s64 result = (s64)truncf(fs);
+  SetReg<u64>(regs.cop0, FD(instr), result);
 }
 
 void Cop1::truncld(Registers& regs, u32 instr) {
+  double fs = GetCop1RegDouble(regs.cop0, FS(instr));
+  s64 result = (s64)trunc(fs);
+  SetReg<u64>(regs.cop0, FD(instr), result);
+}
 
+void Cop1::sdc1(Registers& regs, Mem& mem, u32 instr) {
+  u32 addr = (s64)(s16)instr + regs.gpr[BASE(instr)];
+  mem.Write<u64>(regs, addr, GetReg<u64>(regs.cop0, FT(instr)), regs.oldPC);
+}
+
+void Cop1::mfc1(Registers& regs, u32 instr) {
+  regs.gpr[RT(instr)] = (s32)GetReg<u32>(regs.cop0, FS(instr));
+}
+
+void Cop1::dmfc1(Registers& regs, u32 instr) {
+  regs.gpr[RT(instr)] = (s64)GetReg<u64>(regs.cop0, FS(instr));
+}
+
+void Cop1::mtc1(Registers& regs, u32 instr) {
+  SetReg<u32>(regs.cop0, FS(instr), regs.gpr[RT(instr)]);
+}
+
+void Cop1::dmtc1(Registers& regs, u32 instr) {
+  SetReg<u64>(regs.cop0, FS(instr), regs.gpr[RT(instr)]);
 }
 
 }