Motion Fixes (#1589)

* fix stalling when server is offline

* add retry timer to fail server connections, fix alt slot number

* fix alt slot key issue

* fix crash when saving controller config with empty fields

* code fixes

* add index check in motion hid update, made HandleResponse async

Co-authored-by: Emmanuel <nhv3@localhost.localdomain>

6 files changed, 212 insertions, 133 deletions

diff --git a/Ryujinx.HLE/HOS/Services/Hid/HidDevices/NpadDevices.cs b/Ryujinx.HLE/HOS/Services/Hid/HidDevices/NpadDevices.cs
index 0decbfea..2150f278 100644
--- a/Ryujinx.HLE/HOS/Services/Hid/HidDevices/NpadDevices.cs
+++ b/Ryujinx.HLE/HOS/Services/Hid/HidDevices/NpadDevices.cs
@@ -337,6 +337,11 @@ namespace Ryujinx.HLE.HOS.Services.Hid
                 {
                     i++;
 
+                    if (i >= states.Count)
+                    {
+                        return;
+                    }
+
                     SetSixAxisState(states[i], true);
                 }
             }
diff --git a/Ryujinx/Motion/Client.cs b/Ryujinx/Motion/Client.cs
index 07241ecd..c1822bcd 100644
--- a/Ryujinx/Motion/Client.cs
+++ b/Ryujinx/Motion/Client.cs
@@ -25,6 +25,7 @@ namespace Ryujinx.Motion
         private readonly Dictionary<int, UdpClient> _clients;
 
         private bool[] _clientErrorStatus = new bool[Enum.GetValues(typeof(PlayerIndex)).Length];
+        private long[] _clientRetryTimer  = new long[Enum.GetValues(typeof(PlayerIndex)).Length];
 
         public Client()
         {
@@ -63,18 +64,25 @@ namespace Ryujinx.Motion
 
         public void RegisterClient(int player, string host, int port)
         {
-            if (_clients.ContainsKey(player))
+            if (_clients.ContainsKey(player) || !CanConnect(player))
             {
                 return;
             }
 
-            try
+            lock (_clients)
             {
-                lock (_clients)
+                if (_clients.ContainsKey(player) || !CanConnect(player))
+                {
+                    return;
+                }
+
+                UdpClient client = null;
+
+                try
                 {
                     IPEndPoint endPoint = new IPEndPoint(IPAddress.Parse(host), port);
 
-                    UdpClient client = new UdpClient(host, port);
+                    client = new UdpClient(host, port);
 
                     _clients.Add(player, client);
                     _hosts.Add(player, endPoint);
@@ -86,23 +94,39 @@ namespace Ryujinx.Motion
                         ReceiveLoop(player);
                     });
                 }
-            }
-            catch (FormatException fex)
-            {
-                if (!_clientErrorStatus[player])
+                catch (FormatException fex)
                 {
-                    Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to connect to motion source at {host}:{port}. Error {fex.Message}");
+                    if (!_clientErrorStatus[player])
+                    {
+                        Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to connect to motion source at {host}:{port}. Error {fex.Message}");
 
-                    _clientErrorStatus[player] = true;
+                        _clientErrorStatus[player] = true;
+                    }
                 }
-            }
-            catch (SocketException ex)
-            {
-                if (!_clientErrorStatus[player])
+                catch (SocketException sex)
                 {
-                    Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to connect to motion source at {host}:{port}. Error code {ex.ErrorCode}");
+                    if (!_clientErrorStatus[player])
+                    {
+                        Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to connect to motion source at {host}:{port}. Error code {sex.ErrorCode}");
 
+                        _clientErrorStatus[player] = true;
+                    }
+
+                    RemoveClient(player);
+
+                    client?.Dispose();
+
+                    SetRetryTimer(player);
+                }
+                catch (Exception ex)
+                {
                     _clientErrorStatus[player] = true;
+
+                    RemoveClient(player);
+
+                    client?.Dispose();
+
+                    SetRetryTimer(player);
                 }
             }
         }
@@ -113,9 +137,10 @@ namespace Ryujinx.Motion
             {
                 if (_motionData.ContainsKey(player))
                 {
-                    input = _motionData[player][slot];
-
-                    return true;
+                    if (_motionData[player].TryGetValue(slot, out input))
+                    {
+                        return true;
+                    }
                 }
             }
 
@@ -124,6 +149,13 @@ namespace Ryujinx.Motion
             return false;
         }
 
+        private void RemoveClient(int clientId)
+        {
+            _clients?.Remove(clientId);
+
+            _hosts?.Remove(clientId);
+        }
+
         private void Send(byte[] data, int clientId)
         {
             if (_clients.TryGetValue(clientId, out UdpClient _client))
@@ -143,146 +175,185 @@ namespace Ryujinx.Motion
 
                         _clientErrorStatus[clientId] = true;
 
-                        _clients.Remove(clientId);
+                        RemoveClient(clientId);
+
+                        _client?.Dispose();
+
+                        SetRetryTimer(clientId);
+                    }
+                    catch (ObjectDisposedException dex)
+                    {
+                        _clientErrorStatus[clientId] = true;
 
-                        _hosts.Remove(clientId);
+                        RemoveClient(clientId);
 
                         _client?.Dispose();
+
+                        SetRetryTimer(clientId);
                     }
                 }
             }
         }
 
-        private byte[] Receive(int clientId)
+        private byte[] Receive(int clientId, int timeout = 0)
         {
-            if (_hosts.TryGetValue(clientId, out IPEndPoint endPoint))
+            if (_hosts.TryGetValue(clientId, out IPEndPoint endPoint) && _clients.TryGetValue(clientId, out UdpClient _client))
             {
-                if (_clients.TryGetValue(clientId, out UdpClient _client))
+                if (_client != null && _client.Client != null && _client.Client.Connected)
                 {
-                    if (_client != null && _client.Client != null)
+                    _client.Client.ReceiveTimeout = timeout;
+
+                    var result = _client?.Receive(ref endPoint);
+
+                    if (result.Length > 0)
                     {
-                        if (_client.Client.Connected)
-                        {
-                            try
-                            {
-                                var result = _client?.Receive(ref endPoint);
+                        _clientErrorStatus[clientId] = false;
+                    }
 
-                                if (result.Length > 0)
-                                {
-                                    _clientErrorStatus[clientId] = false;
-                                }
+                    return result;
+                }
+            }
 
-                                return result;
-                            }
-                            catch (SocketException ex)
-                            {
-                                if (!_clientErrorStatus[clientId])
-                                {
-                                    Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to receive data from motion source at {endPoint}. Error code {ex.ErrorCode}");
-                                }
+            throw new Exception($"Client {clientId} is not registered.");
+        }
 
-                                _clientErrorStatus[clientId] = true;
+        private void SetRetryTimer(int clientId)
+        {
+            var elapsedMs = PerformanceCounter.ElapsedMilliseconds;
 
-                                _clients.Remove(clientId);
+            _clientRetryTimer[clientId] = elapsedMs;
+        }
 
-                                _hosts.Remove(clientId);
+        private void ResetRetryTimer(int clientId)
+        {
+            _clientRetryTimer[clientId] = 0;
+        }
 
-                                _client?.Dispose();
-                            }
-                        }
-                    }
-                }
-            }
-            
-            return new byte[0];
+        private bool CanConnect(int clientId)
+        {
+            return _clientRetryTimer[clientId] == 0 ? true : PerformanceCounter.ElapsedMilliseconds - 5000 > _clientRetryTimer[clientId];
         }
 
         public void ReceiveLoop(int clientId)
         {
-            while (_active)
+            if (_hosts.TryGetValue(clientId, out IPEndPoint endPoint) && _clients.TryGetValue(clientId, out UdpClient _client))
             {
-                byte[] data = Receive(clientId);
-
-                if (data.Length == 0)
+                if (_client != null && _client.Client != null && _client.Client.Connected)
                 {
-                    continue;
-                }
+                    try
+                    {
+                        while (_active)
+                        {
+                            byte[] data = Receive(clientId);
+
+                            if (data.Length == 0)
+                            {
+                                continue;
+                            }
 
 #pragma warning disable CS4014
-                HandleResponse(data, clientId);
+                            Task.Run(() => HandleResponse(data, clientId));
 #pragma warning restore CS4014
+                        }
+                    }
+                    catch (SocketException ex)
+                    {
+                        if (!_clientErrorStatus[clientId])
+                        {
+                            Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to receive data from motion source at {endPoint}. Error code {ex.ErrorCode}");
+                        }
+
+                        _clientErrorStatus[clientId] = true;
+
+                        RemoveClient(clientId);
+
+                        _client?.Dispose();
+
+                        SetRetryTimer(clientId);
+                    }
+                    catch (ObjectDisposedException)
+                    {
+                        _clientErrorStatus[clientId] = true;
+
+                        RemoveClient(clientId);
+
+                        _client?.Dispose();
+
+                        SetRetryTimer(clientId);
+                    }
+                }
             }
         }
 
 #pragma warning disable CS1998
-        public async Task HandleResponse(byte[] data, int clientId)
+        public void HandleResponse(byte[] data, int clientId)
 #pragma warning restore CS1998
         {
+            ResetRetryTimer(clientId);
+
             MessageType type = (MessageType)BitConverter.ToUInt32(data.AsSpan().Slice(16, 4));
 
             data = data.AsSpan().Slice(16).ToArray();
 
-            using (MemoryStream mem = new MemoryStream(data))
+            using MemoryStream mem = new MemoryStream(data);
+
+            using BinaryReader reader = new BinaryReader(mem);
+
+            switch (type)
             {
-                using (BinaryReader reader = new BinaryReader(mem))
-                {
-                    switch (type)
+                case MessageType.Protocol:
+                    break;
+                case MessageType.Info:
+                    ControllerInfoResponse contollerInfo = reader.ReadStruct<ControllerInfoResponse>();
+                    break;
+                case MessageType.Data:
+                    ControllerDataResponse inputData = reader.ReadStruct<ControllerDataResponse>();
+
+                    Vector3 accelerometer = new Vector3()
                     {
-                        case MessageType.Protocol:
-                            break;
-                        case MessageType.Info:
-                            ControllerInfoResponse contollerInfo = reader.ReadStruct<ControllerInfoResponse>();
-                            break;
-                        case MessageType.Data:
-                            ControllerDataResponse inputData = reader.ReadStruct<ControllerDataResponse>();
-
-                            Vector3 accelerometer = new Vector3()
-                            {
-                                X = -inputData.AccelerometerX,
-                                Y = inputData.AccelerometerZ,
-                                Z = -inputData.AccelerometerY
-                            };
+                        X = -inputData.AccelerometerX,
+                        Y = inputData.AccelerometerZ,
+                        Z = -inputData.AccelerometerY
+                    };
 
-                            Vector3 gyroscrope = new Vector3()
-                            {
-                                X = inputData.GyroscopePitch,
-                                Y = inputData.GyroscopeRoll,
-                                Z = -inputData.GyroscopeYaw
-                            };
+                    Vector3 gyroscrope = new Vector3()
+                    {
+                        X = inputData.GyroscopePitch,
+                        Y = inputData.GyroscopeRoll,
+                        Z = -inputData.GyroscopeYaw
+                    };
 
-                            ulong timestamp = inputData.MotionTimestamp;
+                    ulong timestamp = inputData.MotionTimestamp;
 
-                            InputConfig config = ConfigurationState.Instance.Hid.InputConfig.Value.Find(x => x.PlayerIndex == (PlayerIndex)clientId);
+                    InputConfig config = ConfigurationState.Instance.Hid.InputConfig.Value.Find(x => x.PlayerIndex == (PlayerIndex)clientId);
 
-                            lock (_motionData)
+                    lock (_motionData)
+                    {
+                        int slot = inputData.Shared.Slot;
+
+                        if (_motionData.ContainsKey(clientId))
+                        {
+                            if (_motionData[clientId].ContainsKey(slot))
                             {
-                                int slot = inputData.Shared.Slot;
-
-                                if (_motionData.ContainsKey(clientId))
-                                {
-                                    if (_motionData[clientId].ContainsKey(slot))
-                                    {
-                                        var previousData = _motionData[clientId][slot];
-
-                                        previousData.Update(accelerometer, gyroscrope, timestamp, config.Sensitivity, (float)config.GyroDeadzone);
-                                    }
-                                    else
-                                    {
-                                        MotionInput input = new MotionInput();
-                                        input.Update(accelerometer, gyroscrope, timestamp, config.Sensitivity, (float)config.GyroDeadzone);
-                                        _motionData[clientId].Add(slot, input);
-                                    }
-                                }
-                                else
-                                {
-                                    MotionInput input = new MotionInput();
-                                    input.Update(accelerometer, gyroscrope, timestamp, config.Sensitivity, (float)config.GyroDeadzone);
-                                    _motionData.Add(clientId, new Dictionary<int, MotionInput>() { { slot, input } });
-                                }
+                                var previousData = _motionData[clientId][slot];
+
+                                previousData.Update(accelerometer, gyroscrope, timestamp, config.Sensitivity, (float)config.GyroDeadzone);
                             }
-                            break;
+                            else
+                            {
+                                MotionInput input = new MotionInput();
+                                input.Update(accelerometer, gyroscrope, timestamp, config.Sensitivity, (float)config.GyroDeadzone);
+                                _motionData[clientId].Add(slot, input);
+                            }
+                        }
+                        else
+                        {
+                            MotionInput input = new MotionInput();
+                            input.Update(accelerometer, gyroscrope, timestamp, config.Sensitivity, (float)config.GyroDeadzone);
+                            _motionData.Add(clientId, new Dictionary<int, MotionInput>() { { slot, input } });
+                        }
                     }
-                }
+                    break;
             }
         }
 
diff --git a/Ryujinx/Motion/MotionDevice.cs b/Ryujinx/Motion/MotionDevice.cs
index 82d84eb0..2e2050bc 100644
--- a/Ryujinx/Motion/MotionDevice.cs
+++ b/Ryujinx/Motion/MotionDevice.cs
@@ -38,13 +38,11 @@ namespace Ryujinx.Motion
             }
         }
 
-        public void Poll(PlayerIndex player, int slot)
+        public void Poll(InputConfig config, int slot)
         {
-            InputConfig config = ConfigurationState.Instance.Hid.InputConfig.Value.Find(x => x.PlayerIndex == player);
-
             Orientation = new float[9];
 
-            if (!config.EnableMotion || !_motionSource.TryGetData((int)player, slot, out MotionInput input))
+            if (!config.EnableMotion || !_motionSource.TryGetData((int)config.PlayerIndex, slot, out MotionInput input))
             {
                 Accelerometer = new Vector3();
                 Gyroscope     = new Vector3();
diff --git a/Ryujinx/Ui/ControllerWindow.cs b/Ryujinx/Ui/ControllerWindow.cs
index 40612874..77c8db0c 100644
--- a/Ryujinx/Ui/ControllerWindow.cs
+++ b/Ryujinx/Ui/ControllerWindow.cs
@@ -447,6 +447,8 @@ namespace Ryujinx.Ui
                 Enum.TryParse(_rSl.Label,          out Key rButtonSl);
                 Enum.TryParse(_rSr.Label,          out Key rButtonSr);
 
+                int.TryParse(_dsuServerPort.Buffer.Text, out int port);
+
                 return new KeyboardConfig
                 {
                     Index          = int.Parse(_inputDevice.ActiveId.Split("/")[1]),
@@ -489,11 +491,11 @@ namespace Ryujinx.Ui
                     EnableMotion  = _enableMotion.Active,
                     MirrorInput   = _mirrorInput.Active,
                     Slot          = (int)_slotNumber.Value,
-                    AltSlot       = (int)_slotNumber.Value,
+                    AltSlot       = (int)_altSlotNumber.Value,
                     Sensitivity   = (int)_sensitivity.Value,
                     GyroDeadzone  = _gyroDeadzone.Value,
                     DsuServerHost = _dsuServerHost.Buffer.Text,
-                    DsuServerPort = int.Parse(_dsuServerPort.Buffer.Text)
+                    DsuServerPort = port
                 };
             }
             
@@ -525,6 +527,8 @@ namespace Ryujinx.Ui
                 Enum.TryParse(_rSl.Label,          out ControllerInputId rButtonSl);
                 Enum.TryParse(_rSr.Label,          out ControllerInputId rButtonSr);
 
+                int.TryParse(_dsuServerPort.Buffer.Text, out int port);
+
                 return new ControllerConfig
                 {
                     Index            = int.Parse(_inputDevice.ActiveId.Split("/")[1]),
@@ -570,11 +574,11 @@ namespace Ryujinx.Ui
                     EnableMotion  = _enableMotion.Active,
                     MirrorInput   = _mirrorInput.Active,
                     Slot          = (int)_slotNumber.Value,
-                    AltSlot       = (int)_slotNumber.Value,
+                    AltSlot       = (int)_altSlotNumber.Value,
                     Sensitivity   = (int)_sensitivity.Value,
                     GyroDeadzone  = _gyroDeadzone.Value,
                     DsuServerHost = _dsuServerHost.Buffer.Text,
-                    DsuServerPort = int.Parse(_dsuServerPort.Buffer.Text)
+                    DsuServerPort = port
                 };
             }
 
diff --git a/Ryujinx/Ui/ControllerWindow.glade b/Ryujinx/Ui/ControllerWindow.glade
index d148cfae..2143e9de 100644
--- a/Ryujinx/Ui/ControllerWindow.glade
+++ b/Ryujinx/Ui/ControllerWindow.glade
@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<!-- Generated with glade 3.36.0 -->
+<!-- Generated with glade 3.22.1 -->
 <interface>
   <requires lib="gtk+" version="3.20"/>
   <object class="GtkAdjustment" id="_altSlotNumber">
@@ -9,28 +9,28 @@
   </object>
   <object class="GtkAdjustment" id="_controllerDeadzoneLeft">
     <property name="upper">1</property>
-    <property name="value">0.05</property>
+    <property name="value">0.050000000000000003</property>
     <property name="step_increment">0.01</property>
-    <property name="page_increment">0.1</property>
+    <property name="page_increment">0.10000000000000001</property>
   </object>
   <object class="GtkAdjustment" id="_controllerDeadzoneRight">
     <property name="upper">1</property>
-    <property name="value">0.05</property>
+    <property name="value">0.050000000000000003</property>
     <property name="step_increment">0.01</property>
-    <property name="page_increment">0.1</property>
+    <property name="page_increment">0.10000000000000001</property>
   </object>
   <object class="GtkAdjustment" id="_controllerTriggerThreshold">
     <property name="upper">1</property>
     <property name="value">0.5</property>
     <property name="step_increment">0.01</property>
-    <property name="page_increment">0.1</property>
+    <property name="page_increment">0.10000000000000001</property>
   </object>
   <object class="GtkAdjustment" id="_gyroDeadzone">
     <property name="upper">100</property>
     <property name="value">0.01</property>
     <property name="step_increment">0.01</property>
-    <property name="page_increment">0.1</property>
-    <property name="page_size">0.1</property>
+    <property name="page_increment">0.10000000000000001</property>
+    <property name="page_size">0.10000000000000001</property>
   </object>
   <object class="GtkAdjustment" id="_sensitivity">
     <property name="upper">1000</property>
@@ -50,6 +50,9 @@
     <property name="window_position">center</property>
     <property name="default_width">1100</property>
     <property name="default_height">600</property>
+    <child type="titlebar">
+      <placeholder/>
+    </child>
     <child>
       <object class="GtkBox">
         <property name="visible">True</property>
@@ -1803,6 +1806,7 @@
                                                 <property name="visible">True</property>
                                                 <property name="can_focus">True</property>
                                                 <property name="text" translatable="yes">0</property>
+                                                <property name="adjustment">_altSlotNumber</property>
                                                 <property name="climb_rate">1</property>
                                                 <property name="snap_to_ticks">True</property>
                                                 <property name="numeric">True</property>
@@ -2030,8 +2034,5 @@
         </child>
       </object>
     </child>
-    <child type="titlebar">
-      <placeholder/>
-    </child>
   </object>
 </interface>
diff --git a/Ryujinx/Ui/GLRenderer.cs b/Ryujinx/Ui/GLRenderer.cs
index 9cf23695..b635ad1c 100644
--- a/Ryujinx/Ui/GLRenderer.cs
+++ b/Ryujinx/Ui/GLRenderer.cs
@@ -512,7 +512,7 @@ namespace Ryujinx.Ui
 
                 currentButton |= _device.Hid.UpdateStickButtons(leftJoystick, rightJoystick);
 
-                motionDevice.Poll(inputConfig.PlayerIndex, inputConfig.Slot);
+                motionDevice.Poll(inputConfig, inputConfig.Slot);
 
                 SixAxisInput sixAxisInput = new SixAxisInput()
                 {
@@ -537,7 +537,7 @@ namespace Ryujinx.Ui
                 {
                     if (!inputConfig.MirrorInput)
                     {
-                        motionDevice.Poll(inputConfig.PlayerIndex, inputConfig.AltSlot);
+                        motionDevice.Poll(inputConfig, inputConfig.AltSlot);
 
                         sixAxisInput = new SixAxisInput()
                         {