Bug in RDP gathering of commands + 2 new vector instructions

src/n64/core/Mem.cpp

@@ -304,7 +304,7 @@ void Mem::Write32(Registers& regs, u64 vaddr, u32 val, s64 pc) {
       if(val < ISVIEWER_SIZE) {
         char* message = (char*)calloc(val + 1, 1);
         memcpy(message, isviewer, val);
-        printf("%s", message);
+        fmt::print("{}", message);
         free(message);
       }
     } break;

src/n64/core/RDP.cpp

@@ -60,10 +60,12 @@ void RDP::Write(MI& mi, Registers& regs, RSP& rsp, u32 addr, u32 val) {
 }
 
 void RDP::StatusWrite(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);
-  bool rdpUnfrozen = false;
   if(temp.clearFreeze) {
     dpc.status.freeze = false;
     rdpUnfrozen = true;
@@ -71,11 +73,10 @@ void RDP::StatusWrite(MI& mi, Registers& regs, RSP& rsp, u32 val) {
 
   if(temp.setFreeze) {
     dpc.status.freeze = true;
-    rdpUnfrozen = false;
   }
   CLEAR_SET(dpc.status.flush, temp.clearFlush, temp.setFlush);
   CLEAR_SET(dpc.status.cmdBusy, temp.clearCmd, false);
-  CLEAR_SET(dpc.clock, temp.clearClock, false);
+  if(temp.clearClock) dpc.clock = 0;
   CLEAR_SET(dpc.status.pipeBusy, temp.clearPipe, false);
   CLEAR_SET(dpc.status.tmemBusy, temp.clearTmem, false);
 
@@ -85,6 +86,9 @@ void RDP::StatusWrite(MI& mi, Registers& regs, RSP& rsp, u32 val) {
 }
 
 void RDP::RunCommand(MI& mi, Registers& regs, RSP& rsp) {
+  dpc.status.pipeBusy = true;
+  dpc.status.startGclk = true;
+
   static int remaining_cmds = 0;
 
   const u32 current = dpc.current & 0xFFFFF8;
@@ -93,12 +97,14 @@ void RDP::RunCommand(MI& mi, Registers& regs, RSP& rsp) {
   int len = end - current;
   if(len <= 0) return;
 
-  dpc.status.freeze = true;
 
-  if(len + (remaining_cmds * 4) <= 0xFFFFF) {
+  if(len + (remaining_cmds * 4) > 0xFFFFF) {
+    return;
+  }
+
   if(dpc.status.xbusDmemDma) {
     for(int i = 0; i < len; i += 4) {
-        u32 cmd = util::ReadAccess<u32>(rsp.dmem, current + i);
+      u32 cmd = util::ReadAccess<u32>(rsp.dmem, (current + i) & 0xFFF);
       cmd_buf[remaining_cmds + (i >> 2)] = cmd;
     }
   } else {
@@ -106,7 +112,7 @@ void RDP::RunCommand(MI& mi, Registers& regs, RSP& rsp) {
       return;
     }
     for(int i = 0; i < len; i += 4) {
-        u32 cmd = util::ReadAccess<u32>(rsp.dmem, current + i);
+      u32 cmd = util::ReadAccess<u32>(dram.data(), current + i);
       cmd_buf[remaining_cmds + (i >> 2)] = cmd;
     }
   }
@@ -141,8 +147,7 @@ void RDP::RunCommand(MI& mi, Registers& regs, RSP& rsp) {
     }
 
     if (cmd == 0x29) {
-        OnFullSync();
+      OnFullSync(mi, regs);
-        InterruptRaise(mi, regs, Interrupt::DP);
     }
 
     buf_index += cmd_len;
@@ -151,14 +156,19 @@ void RDP::RunCommand(MI& mi, Registers& regs, RSP& rsp) {
   if(processed_all) {
     remaining_cmds = 0;
   }
-  }
 
   dpc.current = end;
+  dpc.end = end;
   dpc.status.freeze = false;
   dpc.status.cbufReady = true;
 }
 
-void RDP::OnFullSync() {
+void RDP::OnFullSync(MI& mi, Registers& regs) {
   ParallelRdpOnFullSync();
+
+  dpc.status.pipeBusy = false;
+  dpc.status.startGclk = false;
+  dpc.status.cbufReady = false;
+  InterruptRaise(mi, regs, Interrupt::DP);
 }
 }

src/n64/core/RDP.hpp

@@ -62,6 +62,6 @@ struct RDP {
   void Write(MI& mi, Registers& regs, RSP& rsp, u32 addr, u32 val);
   void StatusWrite(MI& mi, Registers& regs, RSP& rsp, u32 val);
   void RunCommand(MI& mi, Registers& regs, RSP& rsp);
-  void OnFullSync();
+  void OnFullSync(MI& mi, Registers& regs);
 };
 } // natsukashii

src/n64/core/RSP.hpp

@@ -172,7 +172,12 @@ struct RSP {
 
   inline void WriteStatus(MI& mi, Registers& regs, u32 value) {
     auto write = SPStatusWrite{.raw = value};
-    CLEAR_SET(spStatus.halt, write.clearHalt, write.setHalt);
+    if(write.clearHalt && !write.setHalt) {
+      spStatus.halt = false;
+    }
+    if(write.setHalt && !write.clearHalt) {
+      spStatus.halt = true;
+    }
     if(write.clearBroke) spStatus.broke = false;
     if(write.clearIntr && !write.setIntr)
       InterruptLower(mi, regs, Interrupt::SP);
@@ -348,8 +353,9 @@ struct RSP {
   void sh(u32 instr);
   void sw(u32 instr);
   void sub(u32 instr);
-  void sqv(u32 instr);
+  void sbv(u32 instr);
   void sdv(u32 instr);
+  void sqv(u32 instr);
   void ssv(u32 instr);
   void sllv(u32 instr);
   void srlv(u32 instr);
@@ -363,8 +369,10 @@ struct RSP {
   void sltiu(u32 instr);
   void vabs(u32 instr);
   void vadd(u32 instr);
-  void vmov(u32 instr);
   void vmacf(u32 instr);
+  void vmadl(u32 instr);
+  void vmov(u32 instr);
+  void vmudh(u32 instr);
   void veq(u32 instr);
   void vne(u32 instr);
   void vsar(u32 instr);

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

@@ -103,10 +103,10 @@ void Cop0::SetReg32(u8 addr, u32 value) {
       status.raw |= (value & STATUS_MASK);
       break;
     case COP0_REG_CAUSE: {
-      Cop0Cause temp{};
+      Cop0Cause tmp{};
-      temp.raw = value;
+      tmp.raw = value;
-      cause.ip0 = temp.ip0;
+      cause.ip0 = tmp.ip0;
-      cause.ip1 = temp.ip1;
+      cause.ip1 = tmp.ip1;
     } break;
     case COP0_REG_EPC:
       EPC = s64(s32(value));
@@ -260,8 +260,6 @@ void FireException(Registers& regs, ExceptionCode code, int cop, s64 pc) {
   regs.cop0.cause.copError = cop;
   regs.cop0.cause.exceptionCode = static_cast<u8>(code);
 
-  s64 exceptionVector = 0;
-
   if(regs.cop0.status.bev) {
     util::panic("BEV bit set!\n");
   } else {

src/n64/core/mmio/PIF.cpp

@@ -48,7 +48,7 @@ void ProcessPIFCommands(u8* pifRam, Controller& controller, Mem& mem) {
             res[2] = controller.b3;
             res[3] = controller.b4;
             break;
-          case 2: case 3: break;
+          case 2: case 3: res[0] = 0; break;
           default: util::panic("Unimplemented PIF command {}", cmd[2]);
         }
 

src/n64/core/rsp/decode.cpp

@@ -50,7 +50,7 @@ inline void regimm(RSP& rsp, u32 instr) {
     case 0x01: rsp.b(instr, (s32)rsp.gpr[RS(instr)] >= 0); break;
     case 0x10: rsp.bl(instr, (s32)rsp.gpr[RS(instr)] < 0); break;
     case 0x11: rsp.bl(instr, (s32)rsp.gpr[RS(instr)] >= 0); break;
-    default: util::panic("Unhandled RSP regimm instruction ({:06b})\n", mask);
+    default: util::panic("Unhandled RSP regimm instruction ({:05b})\n", mask);
   }
 }
 
@@ -59,17 +59,18 @@ inline void lwc2(RSP& rsp, u32 instr) {
   switch(mask) {
     case 0x03: rsp.ldv(instr); break;
     case 0x04: rsp.lqv(instr); break;
-    default: util::panic("Unhandled RSP LWC2 {:06b}\n", mask);
+    default: util::panic("Unhandled RSP LWC2 {:05b}\n", mask);
   }
 }
 
 inline void swc2(RSP& rsp, u32 instr) {
   u8 mask = (instr >> 11) & 0x1F;
   switch(mask) {
+    case 0x00: rsp.sbv(instr); break;
     case 0x01: rsp.ssv(instr); break;
     case 0x03: rsp.sdv(instr); break;
     case 0x04: rsp.sqv(instr); break;
-    default: util::panic("Unhandled RSP SWC2 {:06b}\n", mask);
+    default: util::panic("Unhandled RSP SWC2 {:05b}\n", mask);
   }
 }
 
@@ -87,7 +88,9 @@ inline void cop2(RSP& rsp, u32 instr) {
         default: util::panic("Unhandled RSP COP2 sub ({:05b})\n", mask_sub);
       }
       break;
+    case 0x07: rsp.vmudh(instr); break;
     case 0x08: rsp.vmacf(instr); break;
+    case 0x0C: rsp.vmadl(instr); break;
     case 0x10: rsp.vadd(instr); break;
     case 0x13: rsp.vabs(instr); break;
     case 0x1D: rsp.vsar(instr); break;
@@ -106,7 +109,7 @@ inline void cop0(Registers& regs, Mem& mem, u32 instr) {
   switch(mask) {
     case 0x00: rsp.mfc0(rdp, instr); break;
     case 0x04: rsp.mtc0(regs, mem, instr); break;
-    default: util::panic("Unhandled RSP COP0 ({:06b})\n", mask);
+    default: util::panic("Unhandled RSP COP0 ({:05b})\n", mask);
   }
 }
 

src/n64/core/rsp/instructions.cpp

@@ -302,6 +302,13 @@ void RSP::sqv(u32 instr) {
   }
 }
 
+void RSP::sbv(u32 instr) {
+  int e = E1(instr);
+  u32 addr = gpr[BASE(instr)] + SignExt7bit(OFFSET(instr), 3);
+
+  WriteByte(addr, vpr[VT(instr)].byte[BYTE_INDEX(e & 0xF)]);
+}
+
 void RSP::sdv(u32 instr) {
   int e = E1(instr);
   u32 addr = gpr[BASE(instr)] + SignExt7bit(OFFSET(instr), 3);
@@ -426,6 +433,57 @@ void RSP::vmov(u32 instr) {
 
 }
 
+inline bool IsSignExtension(s16 hi, s16 lo) {
+  if (hi == 0) {
+    return (lo & 0x8000) == 0;
+  } else if (hi == -1) {
+    return (lo & 0x8000) == 0x8000;
+  }
+  return false;
+}
+
+void RSP::vmudh(u32 instr) {
+  int e = E2(instr);
+  VPR& vs = vpr[VS(instr)];
+  VPR& vd = vpr[VD(instr)];
+  VPR vte = GetVTE(vpr[VT(instr)], e);
+  for(int i = 0; i < 8; i++) {
+    s32 prod = vs.selement[i] * vte.selement[i];
+    s64 accum = prod;
+
+    s16 result = clamp_signed(accum);
+
+    accum <<= 16;
+    SetACC(e, accum);
+
+    vd.element[e] = result;
+  }
+}
+
+void RSP::vmadl(u32 instr) {
+  int e = E2(instr);
+  VPR& vs = vpr[VS(instr)];
+  VPR& vd = vpr[VD(instr)];
+  VPR vte = GetVTE(vpr[VT(instr)], e);
+  for(int i = 0; i < 8; i++) {
+    u64 prod = (u64)vs.selement[i] * (u64)vte.selement[i];
+    u64 accum = (prod >> 16) + GetACC(e);
+
+    SetACC(e, accum);
+
+    u16 result;
+    if(IsSignExtension(acc.h.selement[e], acc.m.selement[e])) {
+      result = acc.l.element[e];
+    } else if (acc.h.selement[e] < 0) {
+      result = 0;
+    } else {
+      result = 0xffff;
+    }
+
+    vd.element[e] = result;
+  }
+}
+
 void RSP::vmacf(u32 instr) {
   VPR& vd = vpr[VD(instr)];
   VPR& vs = vpr[VS(instr)];