From 07b81f57babcc7aa41fddfc892148688e841d96e Mon Sep 17 00:00:00 2001
From: Morph <39850852+Morph1984@users.noreply.github.com>
Date: Wed, 7 Oct 2020 14:22:47 -0400
Subject: hid: Fix controller rumble based on new research

This fixes the issue where rumble is only sent to the first controller.
Now, individual controllers can receive their own rumble commands.
---
 src/core/hle/service/hid/controllers/npad.cpp | 45 ++++++++++++++++-----------
 src/core/hle/service/hid/controllers/npad.h   | 12 +++----
 2 files changed, 33 insertions(+), 24 deletions(-)

(limited to 'src/core/hle/service/hid/controllers')

diff --git a/src/core/hle/service/hid/controllers/npad.cpp b/src/core/hle/service/hid/controllers/npad.cpp
index b330c5e40..8a5e5abcf 100644
--- a/src/core/hle/service/hid/controllers/npad.cpp
+++ b/src/core/hle/service/hid/controllers/npad.cpp
@@ -667,35 +667,44 @@ void Controller_NPad::SetNpadMode(u32 npad_id, NpadAssignments assignment_mode)
     }
 }
 
-void Controller_NPad::VibrateController(const std::vector<u32>& controllers,
-                                        const std::vector<Vibration>& vibrations) {
+void Controller_NPad::VibrateController(const std::vector<DeviceHandle>& vibration_device_handles,
+                                        const std::vector<VibrationValue>& vibration_values) {
     LOG_TRACE(Service_HID, "called");
 
     if (!Settings::values.vibration_enabled.GetValue() || !can_controllers_vibrate) {
         return;
     }
-    bool success = true;
-    for (std::size_t i = 0; i < controllers.size(); ++i) {
-        if (!connected_controllers[i].is_connected) {
+
+    ASSERT_MSG(vibration_device_handles.size() == vibration_values.size(),
+               "The amount of device handles does not match with the amount of vibration values,"
+               "this is undefined behavior!");
+
+    for (std::size_t i = 0; i < vibration_device_handles.size(); ++i) {
+        const auto npad_index = NPadIdToIndex(vibration_device_handles[i].npad_id);
+        const auto device_index =
+            static_cast<std::size_t>(vibration_device_handles[i].device_index);
+
+        if (!connected_controllers[npad_index].is_connected) {
             continue;
         }
+
         using namespace Settings::NativeButton;
-        const auto& button_state = buttons[i];
-        if (button_state[A - BUTTON_HID_BEGIN]) {
-            if (button_state[A - BUTTON_HID_BEGIN]->SetRumblePlay(
-                    vibrations[0].amp_high, vibrations[0].amp_low, vibrations[0].freq_high,
-                    vibrations[0].freq_low)) {
-                success = false;
-            }
-        }
-    }
-    if (success) {
-        last_processed_vibration = vibrations.back();
+        const auto& button_state = buttons[npad_index];
+
+        // TODO: Vibrate left/right vibration motors independently if possible.
+        button_state[A - BUTTON_HID_BEGIN]->SetRumblePlay(
+            vibration_values[i].amp_high, vibration_values[i].amp_low,
+            vibration_values[i].freq_high, vibration_values[i].freq_low);
+
+        latest_vibration_values[npad_index][device_index] = vibration_values[i];
     }
 }
 
-Controller_NPad::Vibration Controller_NPad::GetLastVibration() const {
-    return last_processed_vibration;
+Controller_NPad::VibrationValue Controller_NPad::GetLastVibration(
+    const DeviceHandle& vibration_device_handle) const {
+    const auto npad_index = NPadIdToIndex(vibration_device_handle.npad_id);
+    const auto device_index = static_cast<std::size_t>(vibration_device_handle.device_index);
+    return latest_vibration_values[npad_index][device_index];
 }
 
 std::shared_ptr<Kernel::ReadableEvent> Controller_NPad::GetStyleSetChangedEvent(u32 npad_id) const {
diff --git a/src/core/hle/service/hid/controllers/npad.h b/src/core/hle/service/hid/controllers/npad.h
index 35dd2bf5f..c1b19103a 100644
--- a/src/core/hle/service/hid/controllers/npad.h
+++ b/src/core/hle/service/hid/controllers/npad.h
@@ -109,13 +109,13 @@ public:
     };
     static_assert(sizeof(NpadStyleSet) == 4, "NpadStyleSet is an invalid size");
 
-    struct Vibration {
+    struct VibrationValue {
         f32 amp_low;
         f32 freq_low;
         f32 amp_high;
         f32 freq_high;
     };
-    static_assert(sizeof(Vibration) == 0x10, "Vibration is an invalid size");
+    static_assert(sizeof(VibrationValue) == 0x10, "Vibration is an invalid size");
 
     struct LedPattern {
         explicit LedPattern(u64 light1, u64 light2, u64 light3, u64 light4) {
@@ -148,10 +148,10 @@ public:
 
     void SetNpadMode(u32 npad_id, NpadAssignments assignment_mode);
 
-    void VibrateController(const std::vector<u32>& controllers,
-                           const std::vector<Vibration>& vibrations);
+    void VibrateController(const std::vector<DeviceHandle>& vibration_device_handles,
+                           const std::vector<VibrationValue>& vibration_values);
 
-    Vibration GetLastVibration() const;
+    VibrationValue GetLastVibration(const DeviceHandle& vibration_device_handle) const;
 
     std::shared_ptr<Kernel::ReadableEvent> GetStyleSetChangedEvent(u32 npad_id) const;
     void SignalStyleSetChangedEvent(u32 npad_id) const;
@@ -410,7 +410,7 @@ private:
     NpadHandheldActivationMode handheld_activation_mode{NpadHandheldActivationMode::Dual};
     // Each controller should have their own styleset changed event
     std::array<Kernel::EventPair, 10> styleset_changed_events;
-    Vibration last_processed_vibration{};
+    std::array<std::array<VibrationValue, 3>, 10> latest_vibration_values;
     std::array<ControllerHolder, 10> connected_controllers{};
     std::array<bool, 10> unintended_home_button_input_protection{};
     GyroscopeZeroDriftMode gyroscope_zero_drift_mode{GyroscopeZeroDriftMode::Standard};
-- 
cgit v1.2.3