From f96ee475e4ce22ec1e18cd7ae1e8ff9fd7d0893a Mon Sep 17 00:00:00 2001
From: JosJuice <josjuice@gmail.com>
Date: Wed, 20 Jan 2021 14:18:05 +0100
Subject: [PATCH] Implement ArmFPURoundMode.cpp

Fixes https://bugs.dolphin-emu.org/issues/12388. Might also fix
other games that have problems with float/paired instructions
in JitArm64, but I haven't tested any.
---
 Source/Core/Common/ArmCPUDetect.cpp    |  1 +
 Source/Core/Common/ArmFPURoundMode.cpp | 78 ++++++++++++++++++++++++++
 Source/Core/Common/CMakeLists.txt      |  2 +-
 Source/Core/DolphinLib.ARM64.props     |  2 +-
 4 files changed, 81 insertions(+), 2 deletions(-)
 create mode 100644 Source/Core/Common/ArmFPURoundMode.cpp

diff --git a/Source/Core/Common/ArmCPUDetect.cpp b/Source/Core/Common/ArmCPUDetect.cpp
index 7fb0ef6765..a603ff03fd 100644
--- a/Source/Core/Common/ArmCPUDetect.cpp
+++ b/Source/Core/Common/ArmCPUDetect.cpp
@@ -69,6 +69,7 @@ void CPUInfo::Detect()
   CPU64bit = true;
   Mode64bit = true;
   vendor = CPUVendor::ARM;
+  bFlushToZero = true;
 
 #ifdef _WIN32
   num_cores = std::thread::hardware_concurrency();
diff --git a/Source/Core/Common/ArmFPURoundMode.cpp b/Source/Core/Common/ArmFPURoundMode.cpp
new file mode 100644
index 0000000000..323e456ae7
--- /dev/null
+++ b/Source/Core/Common/ArmFPURoundMode.cpp
@@ -0,0 +1,78 @@
+// Copyright 2021 Dolphin Emulator Project
+// Licensed under GPLv2+
+// Refer to the license.txt file included.
+
+#include "Common/CommonTypes.h"
+#include "Common/FPURoundMode.h"
+
+#ifdef _MSC_VER
+#include <intrin.h>
+#endif
+
+static u64 GetFPCR()
+{
+#ifdef _MSC_VER
+  return _ReadStatusReg(ARM64_FPCR);
+#else
+  u64 fpcr;
+  __asm__ __volatile__("mrs %0, fpcr" : "=r"(fpcr));
+  return fpcr;
+#endif
+}
+
+static void SetFPCR(u64 fpcr)
+{
+#ifdef _MSC_VER
+  _WriteStatusReg(ARM64_FPCR, fpcr);
+#else
+  __asm__ __volatile__("msr fpcr, %0" : : "ri"(fpcr));
+#endif
+}
+
+namespace FPURoundMode
+{
+static const u64 default_fpcr = GetFPCR();
+static u64 saved_fpcr = default_fpcr;
+
+void SetRoundMode(int mode)
+{
+  // We don't need to do anything here since SetSIMDMode is always called after calling this
+}
+
+void SetPrecisionMode(PrecisionMode mode)
+{
+}
+
+void SetSIMDMode(int rounding_mode, bool non_ieee_mode)
+{
+  // Flush-To-Zero (non-IEEE mode: denormal outputs are set to +/- 0)
+  constexpr u32 FZ = 1 << 24;
+
+  // lookup table for FPSCR.RN-to-FPCR.RMode translation
+  constexpr u32 rounding_mode_table[] = {
+      (0 << 22),  // nearest
+      (3 << 22),  // zero
+      (1 << 22),  // +inf
+      (2 << 22),  // -inf
+  };
+
+  const u64 base = default_fpcr & ~(0b111 << 22);
+  SetFPCR(base | rounding_mode_table[rounding_mode] | (non_ieee_mode ? FZ : 0));
+}
+
+void SaveSIMDState()
+{
+  saved_fpcr = GetFPCR();
+}
+
+void LoadSIMDState()
+{
+  SetFPCR(saved_fpcr);
+}
+
+void LoadDefaultSIMDState()
+{
+  SetFPCR(default_fpcr);
+}
+
+}  // namespace FPURoundMode
diff --git a/Source/Core/Common/CMakeLists.txt b/Source/Core/Common/CMakeLists.txt
index 9f7486a571..4846601305 100644
--- a/Source/Core/Common/CMakeLists.txt
+++ b/Source/Core/Common/CMakeLists.txt
@@ -199,7 +199,7 @@ if(_M_ARM_64)
     Arm64Emitter.h
     ArmCommon.h
     ArmCPUDetect.cpp
-    GenericFPURoundMode.cpp
+    ArmFPURoundMode.cpp
   )
 else()
   if(_M_X86) #X86
diff --git a/Source/Core/DolphinLib.ARM64.props b/Source/Core/DolphinLib.ARM64.props
index 61d9bd84b0..b7c7558845 100644
--- a/Source/Core/DolphinLib.ARM64.props
+++ b/Source/Core/DolphinLib.ARM64.props
@@ -13,7 +13,7 @@
   <ItemGroup>
     <ClCompile Include="Common\Arm64Emitter.cpp" />
     <ClCompile Include="Common\ArmCPUDetect.cpp" />
-    <ClCompile Include="Common\GenericFPURoundMode.cpp" />
+    <ClCompile Include="Common\ArmFPURoundMode.cpp" />
     <ClCompile Include="Core\PowerPC\JitArm64\Jit_Util.cpp" />
     <ClCompile Include="Core\PowerPC\JitArm64\Jit.cpp" />
     <ClCompile Include="Core\PowerPC\JitArm64\JitArm64_BackPatch.cpp" />