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.

3 files changed, 69 insertions, 43 deletions

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};
diff --git a/src/core/hle/service/hid/hid.cpp b/src/core/hle/service/hid/hid.cpp
index fdac598f0..b0390bdb2 100644
--- a/src/core/hle/service/hid/hid.cpp
+++ b/src/core/hle/service/hid/hid.cpp
@@ -1012,15 +1012,23 @@ void Hid::GetVibrationDeviceInfo(Kernel::HLERequestContext& ctx) {
 
 void Hid::SendVibrationValue(Kernel::HLERequestContext& ctx) {
     IPC::RequestParser rp{ctx};
-    const auto controller{rp.Pop<u32>()};
-    const auto vibration_values{rp.PopRaw<Controller_NPad::Vibration>()};
-    const auto applet_resource_user_id{rp.Pop<u64>()};
+    struct Parameters {
+        Controller_NPad::DeviceHandle vibration_device_handle{};
+        Controller_NPad::VibrationValue vibration_value{};
+        INSERT_PADDING_WORDS(1);
+        u64 applet_resource_user_id{};
+    };
+
+    const auto parameters{rp.PopRaw<Parameters>()};
 
     applet_resource->GetController<Controller_NPad>(HidController::NPad)
-        .VibrateController({controller}, {vibration_values});
+        .VibrateController({parameters.vibration_device_handle}, {parameters.vibration_value});
 
-    LOG_DEBUG(Service_HID, "called, controller={}, applet_resource_user_id={}", controller,
-              applet_resource_user_id);
+    LOG_DEBUG(Service_HID,
+              "called, npad_type={}, npad_id={}, device_index={}, applet_resource_user_id={}",
+              parameters.vibration_device_handle.npad_type,
+              parameters.vibration_device_handle.npad_id,
+              parameters.vibration_device_handle.device_index, parameters.applet_resource_user_id);
 
     IPC::ResponseBuilder rb{ctx, 2};
     rb.Push(RESULT_SUCCESS);
@@ -1028,16 +1036,24 @@ void Hid::SendVibrationValue(Kernel::HLERequestContext& ctx) {
 
 void Hid::GetActualVibrationValue(Kernel::HLERequestContext& ctx) {
     IPC::RequestParser rp{ctx};
-    const auto controller_id{rp.Pop<u32>()};
-    const auto applet_resource_user_id{rp.Pop<u64>()};
+    struct Parameters {
+        Controller_NPad::DeviceHandle vibration_device_handle{};
+        INSERT_PADDING_WORDS(1);
+        u64 applet_resource_user_id{};
+    };
 
-    LOG_DEBUG(Service_HID, "called, controller_id={}, applet_resource_user_id={}", controller_id,
-              applet_resource_user_id);
+    const auto parameters{rp.PopRaw<Parameters>()};
+
+    LOG_DEBUG(Service_HID,
+              "called, npad_type={}, npad_id={}, device_index={}, applet_resource_user_id={}",
+              parameters.vibration_device_handle.npad_type,
+              parameters.vibration_device_handle.npad_id,
+              parameters.vibration_device_handle.device_index, parameters.applet_resource_user_id);
 
     IPC::ResponseBuilder rb{ctx, 6};
     rb.Push(RESULT_SUCCESS);
-    rb.PushRaw(
-        applet_resource->GetController<Controller_NPad>(HidController::NPad).GetLastVibration());
+    rb.PushRaw(applet_resource->GetController<Controller_NPad>(HidController::NPad)
+                   .GetLastVibration(parameters.vibration_device_handle));
 }
 
 void Hid::CreateActiveVibrationDeviceList(Kernel::HLERequestContext& ctx) {
@@ -1072,18 +1088,19 @@ void Hid::SendVibrationValues(Kernel::HLERequestContext& ctx) {
     IPC::RequestParser rp{ctx};
     const auto applet_resource_user_id{rp.Pop<u64>()};
 
-    const auto controllers = ctx.ReadBuffer(0);
+    const auto handles = ctx.ReadBuffer(0);
     const auto vibrations = ctx.ReadBuffer(1);
 
-    std::vector<u32> controller_list(controllers.size() / sizeof(u32));
-    std::vector<Controller_NPad::Vibration> vibration_list(vibrations.size() /
-                                                           sizeof(Controller_NPad::Vibration));
+    std::vector<Controller_NPad::DeviceHandle> vibration_device_handles(
+        handles.size() / sizeof(Controller_NPad::DeviceHandle));
+    std::vector<Controller_NPad::VibrationValue> vibration_values(
+        vibrations.size() / sizeof(Controller_NPad::VibrationValue));
 
-    std::memcpy(controller_list.data(), controllers.data(), controllers.size());
-    std::memcpy(vibration_list.data(), vibrations.data(), vibrations.size());
+    std::memcpy(vibration_device_handles.data(), handles.data(), handles.size());
+    std::memcpy(vibration_values.data(), vibrations.data(), vibrations.size());
 
     applet_resource->GetController<Controller_NPad>(HidController::NPad)
-        .VibrateController(controller_list, vibration_list);
+        .VibrateController(vibration_device_handles, vibration_values);
 
     LOG_DEBUG(Service_HID, "called, applet_resource_user_id={}", applet_resource_user_id);