diff --git a/src/vm/fp_functions.cpp b/src/vm/fp_functions.cpp
index ad340b5..3a87ae3 100644
--- a/src/vm/fp_functions.cpp
+++ b/src/vm/fp_functions.cpp
@@ -149,18 +149,24 @@ uint32_t fcvt_s(uint32_t v1, uint32_t op, uint8_t mode) {
 }
 
 uint32_t fmadd_s(uint32_t v1, uint32_t v2, uint32_t v3, uint32_t op, uint8_t mode) {
-    // Used values for op are:
-    // FMADD_S:  0b00
-    // FMSUB_S:  0b01
-    // FNMADD_S: 0b10
-    // FNMSUB_S: 0b11
-    // op should be {softfloat_mulAdd_subProd(2), softfloat_mulAdd_subC(1)}
+    uint32_t F32_SIGN = 1UL << 31;
+    switch(op) {
+    case 0: // FMADD_S
+        break;
+    case 1: // FMSUB_S
+        v3 ^= F32_SIGN;
+        break;
+    case 2: // FNMADD_S
+        v1 ^= F32_SIGN;
+        v3 ^= F32_SIGN;
+        break;
+    case 3: // FNMSUB_S
+        v1 ^= F32_SIGN;
+        break;
+    }
     softfloat_roundingMode = rmm_map[mode & 0x7];
     softfloat_exceptionFlags = 0;
-    float32_t res = softfloat_mulAddF32(v1, v2, v3, op & 0x1);
-    // Do not invert quiet NaN
-    if((op & 0b10) && (res.v != quiet_nan32))
-        res.v ^= 1ULL << 31;
+    float32_t res = softfloat_mulAddF32(v1, v2, v3, 0);
     return res.v;
 }