diff --git a/Source/Project64-core/N64System/Interpreter/InterpreterOps.cpp b/Source/Project64-core/N64System/Interpreter/InterpreterOps.cpp index 191a183b7..aee0073f6 100644 --- a/Source/Project64-core/N64System/Interpreter/InterpreterOps.cpp +++ b/Source/Project64-core/N64System/Interpreter/InterpreterOps.cpp @@ -2443,8 +2443,20 @@ void R4300iOp::COP1_D_ADD() { return; } + _FPCR[31] &= ~0x0003F000; fesetround(*_RoundingModel); - *(double *)_FPR_D[m_Opcode.fd] = *(double *)_FPR_D[m_Opcode.fs] + *(double *)_FPR_D[m_Opcode.ft]; + feclearexcept(FE_ALL_EXCEPT); + + if (!CheckFPUInput64(*(double *)_FPR_S[m_Opcode.fs]) || !CheckFPUInput64(*(double *)_FPR_S[m_Opcode.ft])) + { + return; + } + double Result = (*(double *)_FPR_D[m_Opcode.fs] + *(double *)_FPR_D[m_Opcode.ft]); + if (CheckFPUException() || CheckFPUResult64(Result)) + { + return; + } + *(uint64_t *)_FPR_D[m_Opcode.fd] = *(uint64_t *)&Result; } void R4300iOp::COP1_D_SUB() @@ -2848,6 +2860,49 @@ bool R4300iOp::CheckFPUInput32(const float & Value) return true; } +bool R4300iOp::CheckFPUInput64(const double & Value) +{ + int Type = fpclassify(Value); + bool Exception = false; + if (Type == FP_SUBNORMAL) + { + FPStatusReg & StatusReg = (FPStatusReg &)_FPCR[31]; + StatusReg.Cause.UnimplementedOperation = 1; + Exception = true; + } + else if (Type == FP_NAN) + { + uint64_t Value64 = *(uint64_t *)&Value; + FPStatusReg & StatusReg = (FPStatusReg &)_FPCR[31]; + if ((Value64 >= 0x7FF0000000000001 && Value64 <= 0x7FF7FFFFFFFFFFFF) || + (Value64 >= 0xFFF0000000000001 && Value64 <= 0xFFF7FFFFFFFFFFFF)) + { + StatusReg.Cause.UnimplementedOperation = 1; + Exception = true; + } + else + { + StatusReg.Cause.InvalidOperation = 1; + if (StatusReg.Enable.InvalidOperation) + { + Exception = true; + } + else + { + StatusReg.Flags.InvalidOperation = 1; + } + } + } + if (Exception) + { + g_Reg->DoFloatingPointException(g_System->m_PipelineStage == PIPELINE_STAGE_JUMP); + g_System->m_PipelineStage = PIPELINE_STAGE_JUMP; + g_System->m_JumpToLocation = (*_PROGRAM_COUNTER); + return false; + } + return true; +} + bool R4300iOp::CheckFPUResult32(float & Result) { int fptype = fpclassify(Result); @@ -2898,6 +2953,56 @@ bool R4300iOp::CheckFPUResult32(float & Result) return false; } +bool R4300iOp::CheckFPUResult64(double & Result) +{ + int fptype = fpclassify(Result); + if (fptype == FP_NAN) + { + *((uint64_t *)&Result) = 0x7FF7FFFFFFFFFFFF; + } + else if (fptype == FP_SUBNORMAL) + { + FPStatusReg & StatusReg = (FPStatusReg &)_FPCR[31]; + if (!StatusReg.FlushSubnormals || StatusReg.Enable.Underflow || StatusReg.Enable.Inexact) + { + StatusReg.Cause.UnimplementedOperation = 1; + g_Reg->DoFloatingPointException(g_System->m_PipelineStage == PIPELINE_STAGE_JUMP); + g_System->m_PipelineStage = PIPELINE_STAGE_JUMP; + g_System->m_JumpToLocation = (*_PROGRAM_COUNTER); + return true; + } + else + { + StatusReg.Cause.Underflow = 1; + if (!StatusReg.Enable.Underflow) + { + StatusReg.Flags.Underflow = 1; + } + + StatusReg.Cause.Inexact = 1; + if (!StatusReg.Enable.Inexact) + { + StatusReg.Flags.Inexact = 1; + } + + switch (*_RoundingModel) + { + case FE_TONEAREST: + case FE_TOWARDZERO: + Result = Result >= 0.0 ? 0.0 : -0.0; + break; + case FE_UPWARD: + Result = Result >= 0.0 ? 2.2250738585072014e-308 : -0.0; + break; + case FE_DOWNWARD: + Result = Result >= 0.0 ? 0.0 : -2.2250738585072014e-308; + break; + } + } + } + return false; +} + bool R4300iOp::CheckFPUException(void) { int Except = fetestexcept(FE_ALL_EXCEPT); diff --git a/Source/Project64-core/N64System/Interpreter/InterpreterOps.h b/Source/Project64-core/N64System/Interpreter/InterpreterOps.h index 561925279..ed6de6c57 100644 --- a/Source/Project64-core/N64System/Interpreter/InterpreterOps.h +++ b/Source/Project64-core/N64System/Interpreter/InterpreterOps.h @@ -252,7 +252,9 @@ protected: static void GenerateTLBWriteException(uint64_t VAddr, const char * function); static bool TestCop1UsableException(void); static bool CheckFPUInput32(const float & Value); + static bool CheckFPUInput64(const double & Value); static bool CheckFPUResult32(float & Result); + static bool CheckFPUResult64(double & Result); static bool CheckFPUException(void); static const uint32_t SWL_MASK[4], SWR_MASK[4], LWL_MASK[4], LWR_MASK[4];