From 552392e4f6553981556c75432626b4e5bd76467c Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Wed, 30 Jan 2019 14:11:04 -0600
Subject: [PATCH] HW: SI_Device_GCSteeringWheel: Fix handling of force
 commands.

---
 .../Core/HW/SI/SI_DeviceGCSteeringWheel.cpp   | 29 ++++++++++++-------
 .../Core/HW/SI/SI_DeviceGCSteeringWheel.h     |  6 ++++
 2 files changed, 24 insertions(+), 11 deletions(-)

diff --git a/Source/Core/Core/HW/SI/SI_DeviceGCSteeringWheel.cpp b/Source/Core/Core/HW/SI/SI_DeviceGCSteeringWheel.cpp
index 42f583e94e..c72cddc1dc 100644
--- a/Source/Core/Core/HW/SI/SI_DeviceGCSteeringWheel.cpp
+++ b/Source/Core/Core/HW/SI/SI_DeviceGCSteeringWheel.cpp
@@ -81,26 +81,33 @@ void CSIDevice_GCSteeringWheel::SendCommand(u32 command, u8 poll)
 
   if (wheel_command.command == CMD_FORCE)
   {
-    // 0 = left strong, 127 = left weak, 128 = right weak, 255 = right strong
-    unsigned int strength = wheel_command.parameter1;
-
-    // 06 = motor on, 04 = motor off
-    unsigned int type = wheel_command.parameter2;
-
     // get the correct pad number that should rumble locally when using netplay
     const int pad_num = NetPlay_InGamePadToLocalPad(m_device_number);
 
     if (pad_num < 4)
     {
-      if (type == 0x06)
+      // Lowest bit is the high bit of the strength field.
+      const auto type = ForceCommandType(wheel_command.parameter2 >> 1);
+
+      // Strength is a 9 bit value from 0 to 256.
+      // 0 = left strong, 256 = right strong
+      const u32 strength = ((wheel_command.parameter2 & 1) << 8) | wheel_command.parameter1;
+
+      switch (type)
       {
-        // map 0..255 to -1.0..1.0
-        ControlState mapped_strength = strength / 127.5 - 1;
+      case ForceCommandType::MotorOn:
+      {
+        // Map 0..256 to -1.0..1.0
+        const ControlState mapped_strength = strength / 128.0 - 1;
         Pad::Rumble(pad_num, mapped_strength);
+        break;
       }
-      else
-      {
+      case ForceCommandType::MotorOff:
         Pad::Rumble(pad_num, 0);
+        break;
+      default:
+        WARN_LOG(SERIALINTERFACE, "Unknown CMD_FORCE type %i", int(type));
+        break;
       }
     }
 
diff --git a/Source/Core/Core/HW/SI/SI_DeviceGCSteeringWheel.h b/Source/Core/Core/HW/SI/SI_DeviceGCSteeringWheel.h
index 07fbd21a81..c9afee2773 100644
--- a/Source/Core/Core/HW/SI/SI_DeviceGCSteeringWheel.h
+++ b/Source/Core/Core/HW/SI/SI_DeviceGCSteeringWheel.h
@@ -32,5 +32,11 @@ private:
     CMD_FORCE = 0x30,
     CMD_WRITE = 0x40
   };
+
+  enum class ForceCommandType : u8
+  {
+    MotorOn = 0x03,
+    MotorOff = 0x02,
+  };
 };
 }  // namespace SerialInterface