From 83a7d9e3f2216fae7548ef903c40047dfe54b15e Mon Sep 17 00:00:00 2001
From: zilmar <zilmar@pj64-emu.com>
Date: Mon, 30 Jan 2023 20:36:58 +1030
Subject: [PATCH] Core: Start to improve the accuracy of R4300iOp::COP1_S_ADD

---
 .../N64System/Interpreter/InterpreterOps.cpp  | 99 ++++++++++++++++++-
 .../N64System/Interpreter/InterpreterOps.h    |  2 +
 2 files changed, 100 insertions(+), 1 deletion(-)

diff --git a/Source/Project64-core/N64System/Interpreter/InterpreterOps.cpp b/Source/Project64-core/N64System/Interpreter/InterpreterOps.cpp
index 6812f6066..fae7eea39 100644
--- a/Source/Project64-core/N64System/Interpreter/InterpreterOps.cpp
+++ b/Source/Project64-core/N64System/Interpreter/InterpreterOps.cpp
@@ -2105,8 +2105,15 @@ void R4300iOp::COP1_S_ADD()
     {
         return;
     }
+    _FPCR[31] &= ~0x0003F000;
     fesetround(*_RoundingModel);
-    *(float *)_FPR_S[m_Opcode.fd] = (*(float *)_FPR_S[m_Opcode.fs] + *(float *)_FPR_S[m_Opcode.ft]);
+    feclearexcept(FE_ALL_EXCEPT);
+    float Result = (*(float *)_FPR_S[m_Opcode.fs] + *(float *)_FPR_S[m_Opcode.ft]);
+    if (CheckFPUException() || CheckFPUResult32(Result))
+    {
+        return;
+    }
+    *(uint32_t *)_FPR_S[m_Opcode.fd] = *(uint32_t *)&Result;
 }
 
 void R4300iOp::COP1_S_SUB()
@@ -2786,3 +2793,93 @@ bool R4300iOp::TestCop1UsableException(void)
     }
     return false;
 }
+
+bool R4300iOp::CheckFPUResult32(float & Result)
+{
+    int fptype = fpclassify(Result);
+    if (fptype == FP_NAN)
+    {
+        *((uint32_t *)&Result) = 0x7fbfffff;
+    }
+    return false;
+
+}
+
+bool R4300iOp::CheckFPUException(void)
+{
+    int Except = fetestexcept(FE_ALL_EXCEPT);
+    if (Except == 0)
+    {
+        return false;
+    }
+
+    bool Res = false;
+    FPStatusReg & StatusReg = (FPStatusReg &)_FPCR[31];
+    if ((Except & FE_INEXACT) != 0)
+    {
+        StatusReg.Cause.Inexact = 1;
+        if (StatusReg.Enable.Inexact)
+        {
+            Res = true;
+        }
+        else
+        {
+            StatusReg.Flags.Inexact = 1;
+        }
+    }
+    if ((Except & FE_UNDERFLOW) != 0)
+    {
+        StatusReg.Cause.Underflow = 1;
+        if (StatusReg.Enable.Underflow)
+        {
+            Res = true;
+        }
+        else
+        {
+            StatusReg.Flags.Underflow = 1;
+        }
+    }
+    if ((Except & FE_OVERFLOW) != 0)
+    {
+        StatusReg.Cause.Overflow = 1;
+        if (StatusReg.Enable.Overflow)
+        {
+            Res = true;
+        }
+        else
+        {
+            StatusReg.Flags.Overflow = 1;
+        }
+    }
+    if ((Except & FE_DIVBYZERO) != 0)
+    {
+        StatusReg.Cause.DivisionByZero = 1;
+        if (StatusReg.Enable.DivisionByZero)
+        {
+            Res = true;
+        }
+        else
+        {
+            StatusReg.Flags.DivisionByZero = 1;
+        }
+    }
+    if ((Except & FE_INVALID) != 0)
+    {
+        StatusReg.Cause.InvalidOperation = 1;
+        if (StatusReg.Enable.InvalidOperation)
+        {
+            Res = true;
+        }
+        else
+        {
+            StatusReg.Flags.InvalidOperation = 1;
+        }
+    }
+    if (Res)
+    {
+        g_Reg->DoFloatingPointException(g_System->m_PipelineStage == PIPELINE_STAGE_JUMP);
+        g_System->m_PipelineStage = PIPELINE_STAGE_JUMP;
+        g_System->m_JumpToLocation = (*_PROGRAM_COUNTER);
+    }
+    return Res;
+}
diff --git a/Source/Project64-core/N64System/Interpreter/InterpreterOps.h b/Source/Project64-core/N64System/Interpreter/InterpreterOps.h
index e17db20b8..7904724d2 100644
--- a/Source/Project64-core/N64System/Interpreter/InterpreterOps.h
+++ b/Source/Project64-core/N64System/Interpreter/InterpreterOps.h
@@ -251,6 +251,8 @@ protected:
     static void GenerateTLBReadException(uint64_t VAddr, const char * function);
     static void GenerateTLBWriteException(uint64_t VAddr, const char * function);
     static bool TestCop1UsableException(void);
+    static bool CheckFPUResult32(float & Result);
+    static bool CheckFPUException(void);
 
     static const uint32_t SWL_MASK[4], SWR_MASK[4], LWL_MASK[4], LWR_MASK[4];
     static const int32_t SWL_SHIFT[4], SWR_SHIFT[4], LWL_SHIFT[4], LWR_SHIFT[4];