From 4e9a692449ba64e0bd2de1e52dee58d47fd8a3a4 Mon Sep 17 00:00:00 2001
From: zilmar <zilmar@pj64-emu.com>
Date: Thu, 7 Sep 2023 11:41:17 +0930
Subject: [PATCH] RSP: Add RSP_Vector_VRNDP

---
 .../cpu/RSPInterpreterCPU.cpp                 |  4 +-
 .../cpu/RSPInterpreterOps.cpp                 | 42 +++++++++++++++++++
 .../cpu/RSPInterpreterOps.h                   |  2 +
 Source/Project64-rsp-core/cpu/RSPRegister.cpp | 14 ++++++-
 Source/Project64-rsp-core/cpu/RSPRegisters.h  |  2 +
 5 files changed, 61 insertions(+), 3 deletions(-)

diff --git a/Source/Project64-rsp-core/cpu/RSPInterpreterCPU.cpp b/Source/Project64-rsp-core/cpu/RSPInterpreterCPU.cpp
index 6ca6f4983..b6ff689e9 100644
--- a/Source/Project64-rsp-core/cpu/RSPInterpreterCPU.cpp
+++ b/Source/Project64-rsp-core/cpu/RSPInterpreterCPU.cpp
@@ -241,7 +241,7 @@ 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;
@@ -249,7 +249,7 @@ void BuildInterpreterCPU(void)
     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
index 3cfc6ad12..1ff79eef4 100644
--- 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
index 9bdfc774b..e25cc5742 100644
--- 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
index dc3484a23..8da84b8e3 100644
--- 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",
-};
\ No newline at end of file
+};
+
+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
index e8423b364..30600ff0b 100644
--- 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];