Input: Implement HD Rumble for compatible Nin devices (#40)

This PR addresses [this issue](https://github.com/Ryubing/Issues/issues/231) and implements HD Rumble for compatible Nin devices.
## New Features
- Add the option for Gamepads to implement HD Rumble
- Add the HD rumble capability to SDL3Gamepad and SDL3JoyCon when they meet certain requirements

Reviewed-on: https://git.ryujinx.app/projects/Ryubing/pulls/40

src/Ryujinx.Input.SDL3/NpadHdRumble.cs

@@ -0,0 +1,151 @@
+using Ryujinx.HLE.HOS.Services.Hid;
+using SDL;
+using static SDL.SDL3;
+using System;
+
+namespace Ryujinx.Input.SDL3
+{
+    /// <summary>
+    /// Manages a HID handle of a gamepad to encode and write HD rumble commands for Nin controllers.
+    /// </summary>
+    public unsafe class NpadHdRumble : IDisposable
+    {
+        private readonly SDL_hid_device* _hidHandle;
+
+        private int _globalCount;
+
+        private NpadHdRumble(SDL_hid_device* hidHandle)
+        {
+            _hidHandle = hidHandle;
+        }
+
+        public static NpadHdRumble Create(SDL_Gamepad* gamepadHandle)
+        {
+            ushort vendor = SDL_GetGamepadVendor(gamepadHandle);
+            if (vendor != 0x057e)
+            {
+                return null;
+            }
+
+            ushort product = SDL_GetGamepadProduct(gamepadHandle);
+            if (product != 0x2006 && product != 0x2007 && product != 0x2009 && product != 0x200e)
+            {
+                return null;
+            }
+
+            return new NpadHdRumble(SDL_hid_open(vendor, product, 0));
+        }
+
+        // Some of the code was translated from https://github.com/MIZUSHIKI/JoyShockLibrary-plus-HDRumble
+        private void WriteHdRumble(
+            int encLeftLowFreq, int encLeftLowAmp,
+            int encLeftHighFreq, int encLeftHighAmp,
+            int encRightLowFreq, int encRightLowAmp,
+            int encRightHighFreq, int encRightHighAmp)
+        {
+            byte[] buf = new byte[10];
+
+            buf[0] = 0x10;
+            buf[1] = (byte)((++_globalCount) & 0xF);
+
+            buf[2] = (byte)(encLeftHighFreq & 0xFF);
+            buf[3] = (byte)(encLeftHighAmp + ((encLeftHighFreq >> 8) & 0xFF));
+            buf[4] = (byte)(encLeftLowFreq + ((encLeftLowAmp >> 8) & 0xFF));
+            buf[5] = (byte)(encLeftLowAmp & 0xFF);
+
+            buf[6] = (byte)(encRightHighFreq & 0xFF);
+            buf[7] = (byte)(encRightHighAmp + ((encRightHighFreq >> 8) & 0xFF));
+            buf[8] = (byte)(encRightLowFreq + ((encRightLowAmp >> 8) & 0xFF));
+            buf[9] = (byte)(encRightLowAmp & 0xFF);
+
+            if (_globalCount > 0xF)
+            {
+                _globalCount = 0x0;
+            }
+
+            fixed (byte* ptr = buf)
+            {
+                SDL_hid_write(_hidHandle, ptr, (nuint)buf.Length);
+            }
+        }
+
+
+        private static int EncodeLowFreq(float lowFreq)
+        {
+            float lf = Math.Clamp(lowFreq, 40.875885f, 626.286133f);
+            return (int)Math.Round(32 * Math.Log2(lf * 0.1f)) - 0x40;
+        }
+
+        private static int EncodeHighFreq(float highFreq)
+        {
+            float hf = Math.Clamp(highFreq, 81.75177f, 1252.572266f);
+            return ((int)Math.Round(32 * Math.Log2(hf * 0.1f)) - 0x60) * 4;
+        }
+
+        private static int EncodeLowAmp(float rawAmp)
+        {
+            int encodedAmp = 0;
+
+            if (rawAmp is > 0 and < 0.012f)
+            {
+                encodedAmp = 1;
+            }
+            else if (rawAmp is >= 0.012f and < 0.112f)
+            {
+                encodedAmp = (int)Math.Round(4 * Math.Log2(rawAmp * 110f));
+            }
+            else if (rawAmp is >= 0.112f and < 0.225f)
+            {
+                encodedAmp = (int)Math.Round(16 * Math.Log2(rawAmp * 17f));
+            }
+            else if (rawAmp is >= 0.225f and <= 1f)
+            {
+                encodedAmp = (int)Math.Round(32 * Math.Log2(rawAmp * 8.7f));
+            }
+
+            return (int)Math.Floor(encodedAmp / 2.0) + 64;
+        }
+
+        private static int EncodeHighAmp(float rawAmp)
+        {
+            int encodedAmp = 0;
+
+            if (rawAmp is > 0 and < 0.012f)
+            {
+                encodedAmp = 1;
+            }
+            else if (rawAmp is >= 0.012f and < 0.112f)
+            {
+                encodedAmp = (int)Math.Round(4 * Math.Log2(rawAmp * 110f));
+            }
+            else if (rawAmp is >= 0.112f and < 0.225f)
+            {
+                encodedAmp = (int)Math.Round(16 * Math.Log2(rawAmp * 17f));
+            }
+            else if (rawAmp is >= 0.225f and <= 1f)
+            {
+                encodedAmp = (int)Math.Round(32 * Math.Log2(rawAmp * 8.7f));
+            }
+
+            return encodedAmp * 2;
+        }
+
+        public bool HdRumble(VibrationValue left, VibrationValue right)
+        {
+            WriteHdRumble(EncodeLowFreq(left.FrequencyLow),
+                EncodeLowAmp(left.AmplitudeLow),
+                EncodeHighFreq(left.FrequencyHigh),
+                EncodeHighAmp(left.AmplitudeHigh),
+                EncodeLowFreq(right.FrequencyLow),
+                EncodeLowAmp(right.AmplitudeLow),
+                EncodeHighFreq(right.FrequencyHigh),
+                EncodeHighAmp(right.AmplitudeHigh));
+            return true;
+        }
+
+        public void Dispose()
+        {
+            SDL_hid_close(_hidHandle);
+        }
+    }
+}

src/Ryujinx.Input.SDL3/SDL3Gamepad.cs

@@ -2,6 +2,7 @@ using Ryujinx.Common.Configuration.Hid;
 using Ryujinx.Common.Configuration.Hid.Controller;
 using Ryujinx.Common.Logging;
 using Ryujinx.Common.Utilities;
+using Ryujinx.HLE.HOS.Services.Hid;
 using System;
 using System.Collections.Generic;
 using System.Numerics;
@@ -76,11 +77,14 @@ namespace Ryujinx.Input.SDL3
 
         private SDL_Gamepad* _gamepadHandle;
 
+        private NpadHdRumble _hdRumble;
+
         private float _triggerThreshold;
 
         public SDL3Gamepad(SDL_Gamepad* gamepadHandle, string driverId)
         {
             _gamepadHandle = gamepadHandle;
+            _hdRumble = NpadHdRumble.Create(gamepadHandle);
             _buttonsUserMapping = new List<ButtonMappingEntry>(20);
 
             Name = SDL_GetGamepadName(_gamepadHandle);
@@ -165,6 +169,10 @@ namespace Ryujinx.Input.SDL3
 
         protected virtual void Dispose(bool disposing)
         {
+            if (disposing && _hdRumble != null)
+            {
+                _hdRumble.Dispose();
+            }
             if (disposing && _gamepadHandle != null)
             {
                 SDL_CloseGamepad(_gamepadHandle);
@@ -184,6 +192,11 @@ namespace Ryujinx.Input.SDL3
             _triggerThreshold = triggerThreshold;
         }
 
+        public bool HDRumble(VibrationValue left, VibrationValue right)
+        {
+            return _hdRumble?.HdRumble(left, right) ?? false;
+        }
+
         public void Rumble(float lowFrequency, float highFrequency, uint durationMs)
         {
             if ((Features & GamepadFeaturesFlag.Rumble) == 0)

src/Ryujinx.Input.SDL3/SDL3JoyCon.cs

@@ -1,6 +1,7 @@
 using Ryujinx.Common.Configuration.Hid;
 using Ryujinx.Common.Configuration.Hid.Controller;
 using Ryujinx.Common.Logging;
+using Ryujinx.HLE.HOS.Services.Hid;
 using System;
 using System.Collections.Generic;
 using System.Numerics;
@@ -61,6 +62,8 @@ namespace Ryujinx.Input.SDL3
         public GamepadFeaturesFlag Features { get; }
 
         private SDL_Gamepad* _gamepadHandle;
+        
+        private NpadHdRumble _hdRumble;
 
         private enum JoyConType
         {
@@ -76,6 +79,7 @@ namespace Ryujinx.Input.SDL3
         public SDL3JoyCon(SDL_Gamepad* gamepadHandle, string driverId)
         {
             _gamepadHandle = gamepadHandle;
+            _hdRumble = NpadHdRumble.Create(gamepadHandle);
             _buttonsUserMapping = new List<ButtonMappingEntry>(10);
 
             Name = SDL_GetGamepadName(_gamepadHandle);
@@ -139,6 +143,10 @@ namespace Ryujinx.Input.SDL3
 
         protected virtual void Dispose(bool disposing)
         {
+            if (disposing && _hdRumble != null)
+            {
+                _hdRumble.Dispose();
+            }
             if (disposing && _gamepadHandle != null)
             {
                 SDL_CloseGamepad(_gamepadHandle);
@@ -156,6 +164,11 @@ namespace Ryujinx.Input.SDL3
         {
 
         }
+        
+        public bool HDRumble(VibrationValue left, VibrationValue right)
+        {
+            return _hdRumble?.HdRumble(left, right) ?? false;
+        }
 
         public void Rumble(float lowFrequency, float highFrequency, uint durationMs)
         {

src/Ryujinx.Input/HLE/NpadController.cs

@@ -559,18 +559,29 @@ namespace Ryujinx.Input.HLE
                 {
                     VibrationValue leftVibrationValue = dualVibrationValue.Item1;
                     VibrationValue rightVibrationValue = dualVibrationValue.Item2;
-
+                    
                     float low = Math.Min(1f, (float)((rightVibrationValue.AmplitudeLow * 0.85 + rightVibrationValue.AmplitudeHigh * 0.15) * controllerConfig.Rumble.StrongRumble));
                     float high = Math.Min(1f, (float)((leftVibrationValue.AmplitudeLow * 0.15 + leftVibrationValue.AmplitudeHigh * 0.85) * controllerConfig.Rumble.WeakRumble));
+                    
+                    leftVibrationValue.AmplitudeLow *= controllerConfig.Rumble.WeakRumble;
+                    leftVibrationValue.AmplitudeHigh *= controllerConfig.Rumble.StrongRumble;
+                    rightVibrationValue.AmplitudeLow *= controllerConfig.Rumble.WeakRumble;
+                    rightVibrationValue.AmplitudeHigh *= controllerConfig.Rumble.StrongRumble;
 
-                    _gamepad?.Rumble(low, high, uint.MaxValue);
+                    if (_gamepad?.HDRumble(leftVibrationValue, rightVibrationValue) == false)
+                    {
+                        _gamepad?.Rumble(low, high, uint.MaxValue);
+                    }
 
                     Logger.Debug?.Print(LogClass.Hid, $"Effect for {controllerConfig.PlayerIndex} " +
                         $"L.low.amp={leftVibrationValue.AmplitudeLow}, " +
                         $"L.high.amp={leftVibrationValue.AmplitudeHigh}, " +
+                        $"L.low.freq={leftVibrationValue.FrequencyLow}, " +
+                        $"L.high.freq={leftVibrationValue.FrequencyHigh}, " +
                         $"R.low.amp={rightVibrationValue.AmplitudeLow}, " +
                         $"R.high.amp={rightVibrationValue.AmplitudeHigh} " +
-                        $"--> ({low}, {high})");
+                        $"R.low.freq={rightVibrationValue.FrequencyLow}, " +
+                        $"R.high.freq={rightVibrationValue.FrequencyHigh}");
                 }
             }
         }

src/Ryujinx.Input/IGamepad.cs

@@ -1,5 +1,6 @@
 using Ryujinx.Common.Configuration.Hid;
 using Ryujinx.Common.Memory;
+using Ryujinx.HLE.HOS.Services.Hid;
 using System;
 using System.Numerics;
 using System.Runtime.CompilerServices;
@@ -74,6 +75,16 @@ namespace Ryujinx.Input
 
         public void ClearLed() => SetLed(0);
 
+        /// <summary>
+        /// Starts an HD vibration effect on the gamepad if available.
+        /// </summary>
+        /// <param name="left">The vibration data for the left side</param>
+        /// <param name="right">The vibration data for the right side</param>
+        bool HDRumble(VibrationValue left, VibrationValue right)
+        {
+            return false;
+        }
+
         /// <summary>
         /// Starts a rumble effect on the gamepad.
         /// </summary>