4 files changed, 63 insertions, 7 deletions

diff --git a/src/common/input.h b/src/common/input.h
index 54fcb24b0..bb42aaacc 100644
--- a/src/common/input.h
+++ b/src/common/input.h
@@ -72,6 +72,7 @@ enum class PollingError {
 enum class VibrationAmplificationType {
     Linear,
     Exponential,
+    Test,
 };
 
 // Analog properties for calibration
diff --git a/src/core/hid/emulated_controller.cpp b/src/core/hid/emulated_controller.cpp
index ba1dcd171..bd2384515 100644
--- a/src/core/hid/emulated_controller.cpp
+++ b/src/core/hid/emulated_controller.cpp
@@ -884,18 +884,42 @@ bool EmulatedController::SetVibration(std::size_t device_index, VibrationValue v
 }
 
 bool EmulatedController::TestVibration(std::size_t device_index) {
-    static constexpr VibrationValue test_vibration = {
+    if (device_index >= output_devices.size()) {
+        return false;
+    }
+    if (!output_devices[device_index]) {
+        return false;
+    }
+
+    const auto player_index = NpadIdTypeToIndex(npad_id_type);
+    const auto& player = Settings::values.players.GetValue()[player_index];
+
+    if (!player.vibration_enabled) {
+        return false;
+    }
+
+    const Common::Input::VibrationStatus test_vibration = {
         .low_amplitude = 0.001f,
-        .low_frequency = 160.0f,
+        .low_frequency = DEFAULT_VIBRATION_VALUE.low_frequency,
         .high_amplitude = 0.001f,
-        .high_frequency = 320.0f,
+        .high_frequency = DEFAULT_VIBRATION_VALUE.high_frequency,
+        .type = Common::Input::VibrationAmplificationType::Test,
+    };
+
+    const Common::Input::VibrationStatus zero_vibration = {
+        .low_amplitude = DEFAULT_VIBRATION_VALUE.low_amplitude,
+        .low_frequency = DEFAULT_VIBRATION_VALUE.low_frequency,
+        .high_amplitude = DEFAULT_VIBRATION_VALUE.high_amplitude,
+        .high_frequency = DEFAULT_VIBRATION_VALUE.high_frequency,
+        .type = Common::Input::VibrationAmplificationType::Test,
     };
 
     // Send a slight vibration to test for rumble support
-    SetVibration(device_index, test_vibration);
+    output_devices[device_index]->SetVibration(test_vibration);
 
     // Stop any vibration and return the result
-    return SetVibration(device_index, DEFAULT_VIBRATION_VALUE);
+    return output_devices[device_index]->SetVibration(zero_vibration) ==
+           Common::Input::VibrationError::None;
 }
 
 bool EmulatedController::SetPollingMode(Common::Input::PollingMode polling_mode) {
diff --git a/src/input_common/drivers/sdl_driver.cpp b/src/input_common/drivers/sdl_driver.cpp
index a5c63e74a..1a14ef10b 100644
--- a/src/input_common/drivers/sdl_driver.cpp
+++ b/src/input_common/drivers/sdl_driver.cpp
@@ -434,6 +434,7 @@ SDLDriver::SDLDriver(std::string input_engine_) : InputEngine(std::move(input_en
             using namespace std::chrono_literals;
             while (initialized) {
                 SDL_PumpEvents();
+                SendVibrations();
                 std::this_thread::sleep_for(1ms);
             }
         });
@@ -531,13 +532,31 @@ Common::Input::VibrationError SDLDriver::SetRumble(
         .type = Common::Input::VibrationAmplificationType::Exponential,
     };
 
-    if (!joystick->RumblePlay(new_vibration)) {
-        return Common::Input::VibrationError::Unknown;
+    if (vibration.type == Common::Input::VibrationAmplificationType::Test) {
+        if (!joystick->RumblePlay(new_vibration)) {
+            return Common::Input::VibrationError::Unknown;
+        }
+        return Common::Input::VibrationError::None;
     }
 
+    vibration_queue.Push(VibrationRequest{
+        .identifier = identifier,
+        .vibration = new_vibration,
+    });
+
     return Common::Input::VibrationError::None;
 }
 
+void SDLDriver::SendVibrations() {
+    while (!vibration_queue.Empty()) {
+        VibrationRequest request;
+        vibration_queue.Pop(request);
+        const auto joystick = GetSDLJoystickByGUID(request.identifier.guid.RawString(),
+                                                   static_cast<int>(request.identifier.port));
+        joystick->RumblePlay(request.vibration);
+    }
+}
+
 Common::ParamPackage SDLDriver::BuildAnalogParamPackageForButton(int port, std::string guid,
                                                                  s32 axis, float value) const {
     Common::ParamPackage params{};
diff --git a/src/input_common/drivers/sdl_driver.h b/src/input_common/drivers/sdl_driver.h
index dcd0d1e64..c82632506 100644
--- a/src/input_common/drivers/sdl_driver.h
+++ b/src/input_common/drivers/sdl_driver.h
@@ -12,6 +12,7 @@
 #include <SDL.h>
 
 #include "common/common_types.h"
+#include "common/threadsafe_queue.h"
 #include "input_common/input_engine.h"
 
 union SDL_Event;
@@ -64,12 +65,20 @@ public:
         const PadIdentifier& identifier, const Common::Input::VibrationStatus& vibration) override;
 
 private:
+    struct VibrationRequest {
+        PadIdentifier identifier;
+        Common::Input::VibrationStatus vibration;
+    };
+
     void InitJoystick(int joystick_index);
     void CloseJoystick(SDL_Joystick* sdl_joystick);
 
     /// Needs to be called before SDL_QuitSubSystem.
     void CloseJoysticks();
 
+    /// Takes all vibrations from the queue and sends the command to the controller
+    void SendVibrations();
+
     Common::ParamPackage BuildAnalogParamPackageForButton(int port, std::string guid, s32 axis,
                                                           float value = 0.1f) const;
     Common::ParamPackage BuildButtonParamPackageForButton(int port, std::string guid,
@@ -107,6 +116,9 @@ private:
     /// Returns true if the button is on the left joycon
     bool IsButtonOnLeftSide(Settings::NativeButton::Values button) const;
 
+    /// Queue of vibration request to controllers
+    Common::SPSCQueue<VibrationRequest> vibration_queue;
+
     /// Map of GUID of a list of corresponding virtual Joysticks
     std::unordered_map<std::string, std::vector<std::shared_ptr<SDLJoystick>>> joystick_map;
     std::mutex joystick_map_mutex;