fix flush mode (#1336)

* fix flush mode

* more

src/device.rs

@@ -363,8 +363,6 @@ impl Device {
                 cop1: cop1::Cop1 {
                     fcr0: 0,
                     fcr31: 0,
-                    //#[cfg(target_arch = "x86_64")]
-                    //flush_mode: 0,
                     fgr32: [[0; 4]; 32],
                     fgr64: [[0; 8]; 32],
                     instrs: [cop1::reserved; 32],

src/device/cop1.rs

@@ -17,8 +17,7 @@ use crate::{device, savestates};
 //const FCR31_CAUSE_INVALID_BIT: u32 = 1 << 16;
 const FCR31_CAUSE_UNIMP_BIT: u32 = 1 << 17;
 pub const FCR31_CMP_BIT: u32 = 1 << 23;
-//#[cfg(target_arch = "x86_64")]
-//const FCR31_FS_BIT: u32 = 1 << 24;
+const FCR31_FS_BIT: u32 = 1 << 24;
 
 const FCR31_CAUSE_MASK: u32 = 0b00000000000000111111000000000000;
 const FCR31_ENABLE_MASK: u32 = 0b00000000000000000000111110000000;
@@ -28,8 +27,6 @@ const FCR31_WRITE_MASK: u32 = 0b00000001100000111111111111111111;
 pub struct Cop1 {
     pub fcr0: u32,
     pub fcr31: u32,
-    //#[cfg(target_arch = "x86_64")]
-    //pub flush_mode: u32,
     pub fgr32: [[u8; 4]; 32],
     pub fgr64: [[u8; 8]; 32],
     #[serde(skip, default = "savestates::default_instructions")]
@@ -329,6 +326,7 @@ fn set_control_registers_fpu(device: &mut device::Device, index: u32, data: u32)
         }
 
         31 => {
+            let old_fcr31 = device.cpu.cop1.fcr31;
             device.cpu.cop1.fcr31 = data & FCR31_WRITE_MASK;
             // the Cause bits are ANDed with the Enable bits to check for exceptions
             // "Unimplemented Operation" has no Enable bit and always causes an exception
@@ -339,28 +337,11 @@ fn set_control_registers_fpu(device: &mut device::Device, index: u32, data: u32)
             {
                 device::exceptions::floating_point_exception(device)
             }
-            /*
-            #[cfg(target_arch = "x86_64")]
-            {
-                let flush_mode;
-                if (device.cpu.cop1.fcr31 & 2) != 0 {
-                    if (device.cpu.cop1.fcr31 & FCR31_FS_BIT) != 0 {
-                        flush_mode = std::arch::x86_64::_MM_FLUSH_ZERO_OFF
-                    } else {
-                        flush_mode = std::arch::x86_64::_MM_FLUSH_ZERO_ON;
-                    }
-                } else {
-                    flush_mode = std::arch::x86_64::_MM_FLUSH_ZERO_ON;
-                }
-                if flush_mode != device.cpu.cop1.flush_mode {
-                    #[allow(deprecated)]
-                    unsafe {
-                        std::arch::x86_64::_MM_SET_FLUSH_ZERO_MODE(flush_mode)
-                    };
-                    device.cpu.cop1.flush_mode = flush_mode;
-                }
+            // only the RM bit 1 and FS bit affect the host flush mode
+            const FLUSH_MODE_MASK: u32 = 2 | FCR31_FS_BIT;
+            if (old_fcr31 ^ device.cpu.cop1.fcr31) & FLUSH_MODE_MASK != 0 {
+                set_host_flush_mode(device);
             }
-            */
         }
         _ => {
             panic!("unknown FCR register")
@@ -368,6 +349,44 @@ fn set_control_registers_fpu(device: &mut device::Device, index: u32, data: u32)
     }
 }
 
+pub fn set_host_flush_mode(device: &device::Device) {
+    let fcr31 = device.cpu.cop1.fcr31;
+    let flush = !((fcr31 & 2) != 0 && (fcr31 & FCR31_FS_BIT) != 0);
+
+    #[cfg(target_arch = "x86_64")]
+    unsafe {
+        // MXCSR: FTZ (bit 15) flushes denormal results, DAZ (bit 6) flushes denormal
+        // inputs. Set both so behaviour matches aarch64's FPCR.FZ.
+        const MXCSR_FTZ_DAZ: u32 = (1 << 15) | (1 << 6);
+        let mut mxcsr: u32 = 0;
+        std::arch::asm!("stmxcsr [{}]", in(reg) &mut mxcsr, options(nostack, preserves_flags));
+        let new = if flush {
+            mxcsr | MXCSR_FTZ_DAZ
+        } else {
+            mxcsr & !MXCSR_FTZ_DAZ
+        };
+        if new != mxcsr {
+            std::arch::asm!("ldmxcsr [{}]", in(reg) &new, options(nostack, preserves_flags, readonly));
+        }
+    }
+
+    #[cfg(target_arch = "aarch64")]
+    unsafe {
+        // FPCR.FZ (bit 24) flushes both denormal inputs and results
+        const FPCR_FZ: u64 = 1 << 24;
+        let fpcr: u64;
+        std::arch::asm!("mrs {}, fpcr", out(reg) fpcr, options(nomem, nostack, preserves_flags));
+        let new = if flush {
+            fpcr | FPCR_FZ
+        } else {
+            fpcr & !FPCR_FZ
+        };
+        if new != fpcr {
+            std::arch::asm!("msr fpcr, {}", in(reg) new, options(nomem, nostack, preserves_flags));
+        }
+    }
+}
+
 pub fn set_fpr_single(device: &mut device::Device, index: usize, value: f32, clear_high: bool) {
     let bytes = value.to_ne_bytes();
     if device.cpu.cop0.regs[device::cop0::COP0_STATUS_REG] & device::cop0::COP0_STATUS_FR == 0 {
@@ -728,6 +747,7 @@ pub fn init(device: &mut device::Device) {
     device.cpu.cop1.fcr0 = 0b101000000000;
 
     map_instructions(device);
+    set_host_flush_mode(device);
 }
 
 pub fn set_fgr_registers(device: &mut device::Device, status_reg: u64) {

src/savestates.rs

@@ -323,6 +323,7 @@ pub fn load_savestate(device: &mut device::Device, rewind: bool, rewind_frame: O
         device::cpu::map_instructions(device);
         device::cop0::map_instructions(device);
         device::cop1::map_instructions(device);
+        device::cop1::set_host_flush_mode(device);
         device::cop2::map_instructions(device);
         device::rsp_cpu::map_instructions(device);