RSP: Add RSP_Vector_VRNDP

project64 · Sep 7, 2023 · 4e9a692 · 4e9a692
1 parent 0cadbe0
commit 4e9a692
Show file tree

Hide file tree

Showing 5 changed files with 61 additions and 3 deletions.
diff --git a/Source/Project64-rsp-core/cpu/RSPInterpreterCPU.cpp b/Source/Project64-rsp-core/cpu/RSPInterpreterCPU.cpp
@@ -241,15 +241,15 @@ void BuildInterpreterCPU(void)
 
     RSP_Vector[0] = RSP_Vector_VMULF;
     RSP_Vector[1] = RSP_Vector_VMULU;
-    RSP_Vector[2] = rsp_UnknownOpcode;
+    RSP_Vector[2] = RSP_Vector_VRNDP;
     RSP_Vector[3] = RSP_Vector_VMULQ;
     RSP_Vector[4] = RSP_Vector_VMUDL;
     RSP_Vector[5] = RSP_Vector_VMUDM;
     RSP_Vector[6] = RSP_Vector_VMUDN;
     RSP_Vector[7] = RSP_Vector_VMUDH;
     RSP_Vector[8] = RSP_Vector_VMACF;
     RSP_Vector[9] = RSP_Vector_VMACU;
-    RSP_Vector[10] = rsp_UnknownOpcode;
+    RSP_Vector[10] = RSP_Vector_VRNDN;
     RSP_Vector[11] = RSP_Vector_VMACQ;
     RSP_Vector[12] = RSP_Vector_VMADL;
     RSP_Vector[13] = RSP_Vector_VMADM;

diff --git a/Source/Project64-rsp-core/cpu/RSPInterpreterOps.cpp b/Source/Project64-rsp-core/cpu/RSPInterpreterOps.cpp
@@ -698,6 +698,27 @@ void RSP_Vector_VMULU(void)
     RSP_Vect[RSPOpC.vd] = Result;
 }
 
+void RSP_Vector_VRNDP(void)
+{
+    RSPVector Result;
+    for (uint8_t el = 0; el < 8; el++)
+    {
+        int32_t Value = RSP_Vect[RSPOpC.vt].se(el, RSPOpC.e);
+        if (RSPOpC.vs & 1)
+        {
+            Value <<= 16;
+        }
+        int64_t Accum = AccumulatorGet(el);
+        if (Accum >= 0)
+        {
+            Accum = clip48(Accum + Value);
+        }
+        AccumulatorSet(el, Accum);
+        Result.s16(el) = clamp16((int32_t)(Accum >> 16));
+    }
+    RSP_Vect[RSPOpC.vd] = Result;
+}
+
 void RSP_Vector_VMUDL(void)
 {
     uint8_t el, del;
@@ -1007,6 +1028,27 @@ void RSP_Vector_VMACQ(void)
     RSP_Vect[RSPOpC.vd] = Result;
 }
 
+void RSP_Vector_VRNDN(void)
+{
+    RSPVector Result;
+    for (uint8_t el = 0; el < 8; el++)
+    {
+        int32_t Value = RSP_Vect[RSPOpC.vt].se(el, RSPOpC.e);
+        if (RSPOpC.vs & 1)
+        {
+            Value <<= 16;
+        }
+        int64_t Accum = AccumulatorGet(el);
+        if (Accum < 0)
+        {
+            Accum = clip48(Accum + Value);
+        }
+        AccumulatorSet(el, Accum);
+        Result.s16(el) = clamp16((int32_t)(Accum >> 16));
+    }
+    RSP_Vect[RSPOpC.vd] = Result;
+}
+
 void RSP_Vector_VMADL(void)
 {
     uint8_t el, del;

diff --git a/Source/Project64-rsp-core/cpu/RSPInterpreterOps.h b/Source/Project64-rsp-core/cpu/RSPInterpreterOps.h
@@ -76,6 +76,7 @@ void RSP_COP2_VECTOR(void);
 
 void RSP_Vector_VMULF(void);
 void RSP_Vector_VMULU(void);
+void RSP_Vector_VRNDP(void);
 void RSP_Vector_VMULQ(void);
 void RSP_Vector_VMUDL(void);
 void RSP_Vector_VMUDM(void);
@@ -84,6 +85,7 @@ void RSP_Vector_VMUDH(void);
 void RSP_Vector_VMACF(void);
 void RSP_Vector_VMACU(void);
 void RSP_Vector_VMACQ(void);
+void RSP_Vector_VRNDN(void);
 void RSP_Vector_VMADL(void);
 void RSP_Vector_VMADM(void);
 void RSP_Vector_VMADN(void);

diff --git a/Source/Project64-rsp-core/cpu/RSPRegister.cpp b/Source/Project64-rsp-core/cpu/RSPRegister.cpp
@@ -42,4 +42,16 @@ char * GPR_Strings[32] = {
     "SP",
     "S8",
     "RA",
-};
+};
+
+int64_t AccumulatorGet(uint8_t el)
+{
+    return (((int64_t)RSP_ACCUM[el].HW[3]) << 32) | (((int64_t)RSP_ACCUM[el].UHW[2]) << 16) | RSP_ACCUM[el].UHW[1];
+}
+
+void AccumulatorSet(uint8_t el, int64_t Accumulator)
+{
+    RSP_ACCUM[el].HW[3] = (int16_t)(Accumulator >> 32);
+    RSP_ACCUM[el].HW[2] = (int16_t)(Accumulator >> 16);
+    RSP_ACCUM[el].HW[1] = (int16_t)(Accumulator);
+}
diff --git a/Source/Project64-rsp-core/cpu/RSPRegisters.h b/Source/Project64-rsp-core/cpu/RSPRegisters.h
@@ -99,6 +99,8 @@ void Enter_RSP_Register_Window(void);
 void InitilizeRSPRegisters(void);
 void UpdateRSPRegistersScreen(void);
 
+int64_t AccumulatorGet(uint8_t el);
+void AccumulatorSet(uint8_t el, int64_t Accumulator);
 // RSP registers
 extern UWORD32 RSP_GPR[32], RSP_Flags[4];
 extern UDWORD RSP_ACCUM[8];