RSP: Add clamp16

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" />

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>

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>

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;
+}

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);