From 0cadbe0f70898eae94e35cd498f5a6d5c3d16941 Mon Sep 17 00:00:00 2001
From: zilmar <zilmar@pj64-emu.com>
Date: Thu, 7 Sep 2023 11:31:31 +0930
Subject: [PATCH] RSP: Add clamp16

---
 .../Project64-rsp-core.vcxproj                |  2 ++
 .../Project64-rsp-core.vcxproj.filters        |  6 +++++
 .../cpu/RSPInterpreterOps.cpp                 |  1 +
 Source/Project64-rsp-core/cpu/RspClamp.cpp    | 24 +++++++++++++++++++
 Source/Project64-rsp-core/cpu/RspClamp.h      |  5 ++++
 5 files changed, 38 insertions(+)
 create mode 100644 Source/Project64-rsp-core/cpu/RspClamp.cpp
 create mode 100644 Source/Project64-rsp-core/cpu/RspClamp.h

diff --git a/Source/Project64-rsp-core/Project64-rsp-core.vcxproj b/Source/Project64-rsp-core/Project64-rsp-core.vcxproj
index cacd34d144..dad4c70a4e 100644
--- a/Source/Project64-rsp-core/Project64-rsp-core.vcxproj
+++ b/Source/Project64-rsp-core/Project64-rsp-core.vcxproj
@@ -40,6 +40,7 @@
     </ClCompile>
   </ItemDefinitionGroup>
   <ItemGroup>
+    <ClCompile Include="cpu\RspClamp.cpp" />
     <ClCompile Include="cpu\RSPCpu.cpp" />
     <ClCompile Include="cpu\RspDma.cpp" />
     <ClCompile Include="cpu\RSPiInstruction.cpp" />
@@ -62,6 +63,7 @@
     <ClCompile Include="Settings\RspSettings.cpp" />
   </ItemGroup>
   <ItemGroup>
+    <ClInclude Include="cpu\RspClamp.h" />
     <ClInclude Include="cpu\RSPCpu.h" />
     <ClInclude Include="cpu\RspDma.h" />
     <ClInclude Include="cpu\RSPInstruction.h" />
diff --git a/Source/Project64-rsp-core/Project64-rsp-core.vcxproj.filters b/Source/Project64-rsp-core/Project64-rsp-core.vcxproj.filters
index 43ad36692c..7851056c28 100644
--- a/Source/Project64-rsp-core/Project64-rsp-core.vcxproj.filters
+++ b/Source/Project64-rsp-core/Project64-rsp-core.vcxproj.filters
@@ -93,6 +93,9 @@
     <ClCompile Include="cpu\RspDma.cpp">
       <Filter>Source Files\cpu</Filter>
     </ClCompile>
+    <ClCompile Include="cpu\RspClamp.cpp">
+      <Filter>Source Files\cpu</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="cpu\RSPInstruction.h">
@@ -149,5 +152,8 @@
     <ClInclude Include="cpu\RspDma.h">
       <Filter>Header Files\cpu</Filter>
     </ClInclude>
+    <ClInclude Include="cpu\RspClamp.h">
+      <Filter>Header Files\cpu</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>
\ No newline at end of file
diff --git a/Source/Project64-rsp-core/cpu/RSPInterpreterOps.cpp b/Source/Project64-rsp-core/cpu/RSPInterpreterOps.cpp
index 7486cc2998..3cfc6ad12d 100644
--- a/Source/Project64-rsp-core/cpu/RSPInterpreterOps.cpp
+++ b/Source/Project64-rsp-core/cpu/RSPInterpreterOps.cpp
@@ -6,6 +6,7 @@
 #include <Project64-rsp-core/RSPDebugger.h>
 #include <Project64-rsp-core/RSPInfo.h>
 #include <Project64-rsp-core/Settings/RspSettings.h>
+#include <Project64-rsp-core/cpu/RspClamp.h>
 #include <Project64-rsp-core/cpu/RspDma.h>
 #include <Settings/Settings.h>
 #include <algorithm>
diff --git a/Source/Project64-rsp-core/cpu/RspClamp.cpp b/Source/Project64-rsp-core/cpu/RspClamp.cpp
new file mode 100644
index 0000000000..c8a4d0cf95
--- /dev/null
+++ b/Source/Project64-rsp-core/cpu/RspClamp.cpp
@@ -0,0 +1,24 @@
+#include "RspClamp.h"
+
+uint16_t clamp16(int32_t Value)
+{
+    if (Value > 0x7FFF)
+    {
+        return 0x7FFF;
+    }
+    if (Value < (int32_t)0xffff8000)
+    {
+        return 0x8000;
+    }
+    return (uint16_t)Value;
+}
+
+int64_t clip48(uint64_t Value)
+{
+    enum : uint64_t
+    {
+        b = 1ull << (48 - 1),
+        m = b * 2 - 1 
+    };
+    return ((Value & m) ^ b) - b;
+}
\ No newline at end of file
diff --git a/Source/Project64-rsp-core/cpu/RspClamp.h b/Source/Project64-rsp-core/cpu/RspClamp.h
new file mode 100644
index 0000000000..2a3fc7dfdd
--- /dev/null
+++ b/Source/Project64-rsp-core/cpu/RspClamp.h
@@ -0,0 +1,5 @@
+#pragma once
+#include <stdint.h>
+
+uint16_t clamp16(int32_t Value);
+int64_t clip48(uint64_t);
\ No newline at end of file