1 files changed, 444 insertions, 0 deletions
diff --git a/Source/Core/InputCommon/ControllerInterface/DualShockUDPClient/DualShockUDPClient.cpp b/Source/Core/InputCommon/ControllerInterface/DualShockUDPClient/DualShockUDPClient.cpp
new file mode 100644
index 0000000000..60086d517c
--- /dev/null
+++ b/Source/Core/InputCommon/ControllerInterface/DualShockUDPClient/DualShockUDPClient.cpp
@@ -0,0 +1,444 @@
+// Copyright 2019 Dolphin Emulator Project
+// Licensed under GPLv2+
+// Refer to the license.txt file included.
+
+#include "InputCommon/ControllerInterface/DualShockUDPClient/DualShockUDPClient.h"
+
+#include <chrono>
+#include <cstring>
+
+#include <SFML/Network/SocketSelector.hpp>
+#include <SFML/Network/UdpSocket.hpp>
+
+#include "Common/Config/Config.h"
+#include "Common/Flag.h"
+#include "Common/Logging/Log.h"
+#include "Common/MathUtil.h"
+#include "Common/Matrix.h"
+#include "Common/Random.h"
+#include "Common/Thread.h"
+#include "Core/CoreTiming.h"
+#include "Core/HW/SystemTimers.h"
+#include "InputCommon/ControllerInterface/ControllerInterface.h"
+#include "InputCommon/ControllerInterface/DualShockUDPClient/DualShockUDPProto.h"
+
+namespace ciface::DualShockUDPClient
+{
+class Device : public Core::Device
+{
+private:
+  template <class T>
+  class Button : public Core::Device::Input
+  {
+  public:
+    Button(std::string name, const T& buttons, unsigned mask)
+        : m_name(std::move(name)), m_buttons(buttons), m_mask(mask)
+    {
+    }
+    std::string GetName() const override { return m_name; }
+    ControlState GetState() const override { return (m_buttons & m_mask) != 0; }
+
+  private:
+    const std::string m_name;
+    const T& m_buttons;
+    unsigned m_mask;
+  };
+
+  template <class T>
+  class AnalogInput : public Core::Device::Input
+  {
+  public:
+    AnalogInput(std::string name, const T& input, ControlState range, ControlState offset = 0)
+        : m_name(std::move(name)), m_input(input), m_range(range), m_offset(offset)
+    {
+    }
+    std::string GetName() const override { return m_name; }
+    ControlState GetState() const override { return (ControlState(m_input) + m_offset) / m_range; }
+
+  private:
+    const std::string m_name;
+    const T& m_input;
+    const ControlState m_range;
+    const ControlState m_offset;
+  };
+
+  class TouchInput : public AnalogInput<int>
+  {
+  public:
+    TouchInput(std::string name, const int& input, ControlState range)
+        : AnalogInput(std::move(name), input, range)
+    {
+    }
+    bool IsDetectable() override { return false; }
+  };
+
+  class AccelerometerInput : public AnalogInput<double>
+  {
+  public:
+    AccelerometerInput(std::string name, const double& input, ControlState range)
+        : AnalogInput(std::move(name), input, range)
+    {
+    }
+    bool IsDetectable() override { return false; }
+  };
+
+  using GyroInput = AccelerometerInput;
+
+public:
+  void UpdateInput() override;
+
+  Device(Proto::DsModel model, int index);
+
+  std::string GetName() const final override;
+  std::string GetSource() const final override;
+  std::optional<int> GetPreferredId() const final override;
+
+private:
+  const Proto::DsModel m_model;
+  const int m_index;
+  u32 m_client_uid;
+  sf::UdpSocket m_socket;
+  Common::DVec3 m_accel;
+  Common::DVec3 m_gyro;
+  std::chrono::steady_clock::time_point m_next_reregister;
+  Proto::MessageType::PadDataResponse m_pad_data;
+  Proto::Touch m_prev_touch;
+  bool m_prev_touch_valid;
+  int m_touch_x;
+  int m_touch_y;
+};
+
+static constexpr double GRAVITY_ACCELERATION = 9.80665;
+static constexpr char DEFAULT_SERVER_ADDRESS[] = "127.0.0.1";
+static constexpr u16 DEFAULT_SERVER_PORT = 26760;
+static constexpr auto SERVER_REREGISTER_INTERVAL = std::chrono::seconds{1};
+static constexpr auto SERVER_LISTPORTS_INTERVAL = std::chrono::seconds{1};
+static constexpr int TOUCH_X_AXIS_MAX = 1000;
+static constexpr int TOUCH_Y_AXIS_MAX = 500;
+
+namespace Settings
+{
+const Config::ConfigInfo<bool> SERVER_ENABLED{
+    {Config::System::DualShockUDPClient, "Server", "Enabled"}, false};
+const Config::ConfigInfo<std::string> SERVER_ADDRESS{
+    {Config::System::DualShockUDPClient, "Server", "IPAddress"}, DEFAULT_SERVER_ADDRESS};
+const Config::ConfigInfo<int> SERVER_PORT{{Config::System::DualShockUDPClient, "Server", "Port"},
+                                          DEFAULT_SERVER_PORT};
+}  // namespace Settings
+
+static bool s_server_enabled;
+static std::string s_server_address;
+static u16 s_server_port;
+static u32 s_client_uid;
+static std::chrono::steady_clock::time_point s_next_listports;
+static std::thread s_hotplug_thread;
+static Common::Flag s_hotplug_thread_running;
+static std::mutex s_port_info_mutex;
+static Proto::MessageType::PortInfo s_port_info[Proto::PORT_COUNT];
+static sf::UdpSocket s_socket;
+
+static bool IsSameController(const Proto::MessageType::PortInfo& a,
+                             const Proto::MessageType::PortInfo& b)
+{
+  // compare everything but battery_status
+  return a.pad_id == b.pad_id && a.pad_state == b.pad_state && a.model == b.model &&
+         a.connection_type == b.connection_type &&
+         memcmp(a.pad_mac_address, b.pad_mac_address, sizeof a.pad_mac_address) == 0;
+}
+
+static sf::Socket::Status ReceiveWithTimeout(sf::UdpSocket& socket, void* data, std::size_t size,
+                                             std::size_t& received, sf::IpAddress& remoteAddress,
+                                             unsigned short& remotePort, sf::Time timeout)
+{
+  sf::SocketSelector selector;
+  selector.add(socket);
+  if (selector.wait(timeout))
+    return socket.receive(data, size, received, remoteAddress, remotePort);
+  else
+    return sf::Socket::NotReady;
+}
+
+static void HotplugThreadFunc()
+{
+  Common::SetCurrentThreadName("DualShockUDPClient Hotplug Thread");
+  NOTICE_LOG(SERIALINTERFACE, "DualShockUDPClient hotplug thread started");
+
+  while (s_hotplug_thread_running.IsSet())
+  {
+    const auto now = std::chrono::steady_clock::now();
+    if (now >= s_next_listports)
+    {
+      s_next_listports = now + SERVER_LISTPORTS_INTERVAL;
+
+      // Request info on the four controller ports
+      Proto::Message<Proto::MessageType::ListPorts> msg(s_client_uid);
+      auto& list_ports = msg.m_message;
+      list_ports.pad_request_count = 4;
+      list_ports.pad_id[0] = 0;
+      list_ports.pad_id[1] = 1;
+      list_ports.pad_id[2] = 2;
+      list_ports.pad_id[3] = 3;
+      msg.Finish();
+      if (s_socket.send(&list_ports, sizeof list_ports, s_server_address, s_server_port) !=
+          sf::Socket::Status::Done)
+        ERROR_LOG(SERIALINTERFACE, "DualShockUDPClient HotplugThreadFunc send failed");
+    }
+
+    // Receive controller port info
+    Proto::Message<Proto::MessageType::FromServer> msg;
+    const auto timeout = s_next_listports - std::chrono::steady_clock::now();
+    // ReceiveWithTimeout treats a timeout of zero as infinite timeout, which we don't want
+    auto timeout_ms = std::chrono::duration_cast<std::chrono::milliseconds>(timeout).count();
+    timeout_ms = std::max<decltype(timeout_ms)>(timeout_ms, 1);
+    std::size_t received_bytes;
+    sf::IpAddress sender;
+    u16 port;
+    if (ReceiveWithTimeout(s_socket, &msg, sizeof(msg), received_bytes, sender, port,
+                           sf::milliseconds(timeout_ms)) == sf::Socket::Status::Done)
+    {
+      if (auto port_info = msg.CheckAndCastTo<Proto::MessageType::PortInfo>())
+      {
+        const bool port_changed = !IsSameController(*port_info, s_port_info[port_info->pad_id]);
+        {
+          std::lock_guard<std::mutex> lock(s_port_info_mutex);
+          s_port_info[port_info->pad_id] = *port_info;
+        }
+        if (port_changed)
+          PopulateDevices();
+      }
+    }
+  }
+  NOTICE_LOG(SERIALINTERFACE, "DualShockUDPClient hotplug thread stopped");
+}
+
+static void StartHotplugThread()
+{
+  // Mark the thread as running.
+  if (!s_hotplug_thread_running.TestAndSet())
+  {
+    // It was already running.
+    return;
+  }
+
+  s_hotplug_thread = std::thread(HotplugThreadFunc);
+}
+
+static void StopHotplugThread()
+{
+  // Tell the hotplug thread to stop.
+  if (!s_hotplug_thread_running.TestAndClear())
+  {
+    // It wasn't running, we're done.
+    return;
+  }
+
+  s_socket.unbind();  // interrupt blocking socket
+  s_hotplug_thread.join();
+}
+
+static void Restart()
+{
+  NOTICE_LOG(SERIALINTERFACE, "DualShockUDPClient Restart");
+
+  StopHotplugThread();
+
+  s_client_uid = Common::Random::GenerateValue<u32>();
+  s_next_listports = std::chrono::steady_clock::time_point::min();
+  for (int port_index = 0; port_index < Proto::PORT_COUNT; port_index++)
+  {
+    s_port_info[port_index] = {};
+    s_port_info[port_index].pad_id = port_index;
+  }
+
+  PopulateDevices();  // remove devices
+
+  if (s_server_enabled)
+    StartHotplugThread();
+}
+
+static void ConfigChanged()
+{
+  bool server_enabled = Config::Get(Settings::SERVER_ENABLED);
+  std::string server_address = Config::Get(Settings::SERVER_ADDRESS);
+  u16 server_port = Config::Get(Settings::SERVER_PORT);
+  if (server_enabled != s_server_enabled || server_address != s_server_address ||
+      server_port != s_server_port)
+  {
+    s_server_enabled = server_enabled;
+    s_server_address = server_address;
+    s_server_port = server_port;
+    Restart();
+  }
+}
+
+void Init()
+{
+  Config::AddConfigChangedCallback(ConfigChanged);
+}
+
+void PopulateDevices()
+{
+  NOTICE_LOG(SERIALINTERFACE, "DualShockUDPClient PopulateDevices");
+
+  g_controller_interface.RemoveDevice(
+      [](const auto* dev) { return dev->GetSource() == "DSUClient"; });
+
+  std::lock_guard<std::mutex> lock(s_port_info_mutex);
+  for (int port_index = 0; port_index < Proto::PORT_COUNT; port_index++)
+  {
+    Proto::MessageType::PortInfo port_info = s_port_info[port_index];
+    if (port_info.pad_state == Proto::DsState::Connected)
+      g_controller_interface.AddDevice(std::make_shared<Device>(port_info.model, port_index));
+  }
+}
+
+void DeInit()
+{
+  StopHotplugThread();
+}
+
+Device::Device(Proto::DsModel model, int index)
+    : m_model(model), m_index(index),
+      m_client_uid(Common::Random::GenerateValue<u32>()), m_accel{}, m_gyro{},
+      m_next_reregister(std::chrono::steady_clock::time_point::min()), m_pad_data{}, m_prev_touch{},
+      m_prev_touch_valid(false), m_touch_x(0), m_touch_y(0)
+{
+  m_socket.setBlocking(false);
+
+  AddInput(new AnalogInput<u8>("Pad W", m_pad_data.button_dpad_left_analog, 255));
+  AddInput(new AnalogInput<u8>("Pad S", m_pad_data.button_dpad_down_analog, 255));
+  AddInput(new AnalogInput<u8>("Pad E", m_pad_data.button_dpad_right_analog, 255));
+  AddInput(new AnalogInput<u8>("Pad N", m_pad_data.button_dpad_up_analog, 255));
+  AddInput(new AnalogInput<u8>("Square", m_pad_data.button_square_analog, 255));
+  AddInput(new AnalogInput<u8>("Cross", m_pad_data.button_cross_analog, 255));
+  AddInput(new AnalogInput<u8>("Circle", m_pad_data.button_circle_analog, 255));
+  AddInput(new AnalogInput<u8>("Triangle", m_pad_data.button_triangle_analog, 255));
+  AddInput(new AnalogInput<u8>("L1", m_pad_data.button_l1_analog, 255));
+  AddInput(new AnalogInput<u8>("R1", m_pad_data.button_r1_analog, 255));
+
+  AddInput(new AnalogInput<u8>("L2", m_pad_data.trigger_l2, 255));
+  AddInput(new AnalogInput<u8>("R2", m_pad_data.trigger_r2, 255));
+
+  AddInput(new Button<u8>("L3", m_pad_data.button_states1, 0x2));
+  AddInput(new Button<u8>("R3", m_pad_data.button_states1, 0x4));
+  AddInput(new Button<u8>("Share", m_pad_data.button_states1, 0x1));
+  AddInput(new Button<u8>("Options", m_pad_data.button_states1, 0x8));
+  AddInput(new Button<u8>("PS", m_pad_data.button_ps, 0x1));
+  AddInput(new Button<u8>("Touch Button", m_pad_data.button_touch, 0x1));
+
+  AddInput(new AnalogInput<u8>("Left X-", m_pad_data.left_stick_x, -128, -128));
+  AddInput(new AnalogInput<u8>("Left X+", m_pad_data.left_stick_x, 127, -128));
+  AddInput(new AnalogInput<u8>("Left Y-", m_pad_data.left_stick_y_inverted, -128, -128));
+  AddInput(new AnalogInput<u8>("Left Y+", m_pad_data.left_stick_y_inverted, 127, -128));
+  AddInput(new AnalogInput<u8>("Right X-", m_pad_data.right_stick_x, -128, -128));
+  AddInput(new AnalogInput<u8>("Right X+", m_pad_data.right_stick_x, 127, -128));
+  AddInput(new AnalogInput<u8>("Right Y-", m_pad_data.right_stick_y_inverted, -128, -128));
+  AddInput(new AnalogInput<u8>("Right Y+", m_pad_data.right_stick_y_inverted, 127, -128));
+
+  AddInput(new TouchInput("Touch X-", m_touch_x, -TOUCH_X_AXIS_MAX));
+  AddInput(new TouchInput("Touch X+", m_touch_x, TOUCH_X_AXIS_MAX));
+  AddInput(new TouchInput("Touch Y-", m_touch_y, -TOUCH_Y_AXIS_MAX));
+  AddInput(new TouchInput("Touch Y+", m_touch_y, TOUCH_Y_AXIS_MAX));
+
+  AddInput(new AccelerometerInput("Accel Left", m_accel.x, 1));
+  AddInput(new AccelerometerInput("Accel Right", m_accel.x, -1));
+  AddInput(new AccelerometerInput("Accel Backward", m_accel.y, 1));
+  AddInput(new AccelerometerInput("Accel Forward", m_accel.y, -1));
+  AddInput(new AccelerometerInput("Accel Up", m_accel.z, 1));
+  AddInput(new AccelerometerInput("Accel Down", m_accel.z, -1));
+
+  AddInput(new GyroInput("Gyro Pitch Up", m_gyro.x, -1));
+  AddInput(new GyroInput("Gyro Pitch Down", m_gyro.x, 1));
+  AddInput(new GyroInput("Gyro Roll Right", m_gyro.y, -1));
+  AddInput(new GyroInput("Gyro Roll Left", m_gyro.y, 1));
+  AddInput(new GyroInput("Gyro Yaw Right", m_gyro.z, -1));
+  AddInput(new GyroInput("Gyro Yaw Left", m_gyro.z, 1));
+}
+
+std::string Device::GetName() const
+{
+  switch (m_model)
+  {
+  case Proto::DsModel::None:
+    return "None";
+  case Proto::DsModel::DS3:
+    return "DualShock 3";
+  case Proto::DsModel::DS4:
+    return "DualShock 4";
+  case Proto::DsModel::Generic:
+    return "Generic Gamepad";
+  default:
+    return "Device";
+  }
+}
+
+std::string Device::GetSource() const
+{
+  return "DSUClient";
+}
+
+void Device::UpdateInput()
+{
+  // Regularly tell the UDP server to feed us controller data
+  const auto now = std::chrono::steady_clock::now();
+  if (now >= m_next_reregister)
+  {
+    m_next_reregister = now + SERVER_REREGISTER_INTERVAL;
+
+    Proto::Message<Proto::MessageType::PadDataRequest> msg(m_client_uid);
+    auto& data_req = msg.m_message;
+    data_req.register_flags = Proto::RegisterFlags::PadID;
+    data_req.pad_id_to_register = m_index;
+    msg.Finish();
+    if (m_socket.send(&data_req, sizeof(data_req), s_server_address, s_server_port) !=
+        sf::Socket::Status::Done)
+      ERROR_LOG(SERIALINTERFACE, "DualShockUDPClient UpdateInput send failed");
+  }
+
+  // Receive and handle controller data
+  Proto::Message<Proto::MessageType::FromServer> msg;
+  std::size_t received_bytes;
+  sf::IpAddress sender;
+  u16 port;
+  while (m_socket.receive(&msg, sizeof msg, received_bytes, sender, port) ==
+         sf::Socket::Status::Done)
+  {
+    if (auto pad_data = msg.CheckAndCastTo<Proto::MessageType::PadDataResponse>())
+    {
+      m_pad_data = *pad_data;
+
+      m_accel.x = m_pad_data.accelerometer_x_g;
+      m_accel.z = -m_pad_data.accelerometer_y_g;
+      m_accel.y = -m_pad_data.accelerometer_z_inverted_g;
+      m_gyro.x = -m_pad_data.gyro_pitch_deg_s;
+      m_gyro.z = -m_pad_data.gyro_yaw_deg_s;
+      m_gyro.y = -m_pad_data.gyro_roll_deg_s;
+
+      // Convert Gs to meters per second squared
+      m_accel = m_accel * GRAVITY_ACCELERATION;
+
+      // Convert degrees per second to radians per second
+      m_gyro = m_gyro * (MathUtil::TAU / 360);
+
+      // Update touch pad relative coordinates
+      if (m_pad_data.touch1.id != m_prev_touch.id)
+        m_prev_touch_valid = false;
+      if (m_prev_touch_valid)
+      {
+        m_touch_x += m_pad_data.touch1.x - m_prev_touch.x;
+        m_touch_y += m_pad_data.touch1.y - m_prev_touch.y;
+        m_touch_x = std::clamp(m_touch_x, -TOUCH_X_AXIS_MAX, TOUCH_X_AXIS_MAX);
+        m_touch_y = std::clamp(m_touch_y, -TOUCH_Y_AXIS_MAX, TOUCH_Y_AXIS_MAX);
+      }
+      m_prev_touch = m_pad_data.touch1;
+      m_prev_touch_valid = true;
+    }
+  }
+}
+
+std::optional<int> Device::GetPreferredId() const
+{
+  return m_index;
+}
+
+}  // namespace ciface::DualShockUDPClient