From a347b3606ec12ec1fb9e7d72129c00283dc179eb Mon Sep 17 00:00:00 2001
From: Connor McLaughlin <stenzek@gmail.com>
Date: Sun, 15 Dec 2019 21:58:27 +1000
Subject: [PATCH] Implement support for analog controllers

---
 src/core/CMakeLists.txt        |   2 +
 src/core/analog_controller.cpp | 360 +++++++++++++++++++++++++++++++++
 src/core/analog_controller.h   | 132 ++++++++++++
 src/core/controller.cpp        |  22 +-
 src/core/core.vcxproj          |   2 +
 src/core/core.vcxproj.filters  |   2 +
 src/core/settings.cpp          |   5 +-
 src/core/types.h               |   1 +
 8 files changed, 517 insertions(+), 9 deletions(-)
 create mode 100644 src/core/analog_controller.cpp
 create mode 100644 src/core/analog_controller.h

diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt
index 4b5050f16..b28c674fa 100644
--- a/src/core/CMakeLists.txt
+++ b/src/core/CMakeLists.txt
@@ -1,4 +1,6 @@
 add_library(core
+    analog_controller.cpp
+    analog_controller.h
     bios.cpp
     bios.h
     bus.cpp
diff --git a/src/core/analog_controller.cpp b/src/core/analog_controller.cpp
new file mode 100644
index 000000000..c631f4148
--- /dev/null
+++ b/src/core/analog_controller.cpp
@@ -0,0 +1,360 @@
+#include "analog_controller.h"
+#include "YBaseLib/Log.h"
+#include "common/state_wrapper.h"
+Log_SetChannel(AnalogController);
+
+AnalogController::AnalogController()
+{
+  m_axis_state.fill(0x80);
+}
+
+AnalogController::~AnalogController() = default;
+
+ControllerType AnalogController::GetType() const
+{
+  return ControllerType::AnalogController;
+}
+
+void AnalogController::Reset()
+{
+  m_analog_mode = false;
+  m_configuration_mode = false;
+  m_command_param = 0;
+}
+
+bool AnalogController::DoState(StateWrapper& sw)
+{
+  if (!Controller::DoState(sw))
+    return false;
+
+  sw.Do(&m_analog_mode);
+  sw.Do(&m_configuration_mode);
+  sw.Do(&m_command_param);
+  sw.Do(&m_state);
+  return true;
+}
+
+std::optional<s32> AnalogController::GetAxisCodeByName(std::string_view axis_name) const
+{
+  return StaticGetAxisCodeByName(axis_name);
+}
+
+std::optional<s32> AnalogController::GetButtonCodeByName(std::string_view button_name) const
+{
+  return StaticGetButtonCodeByName(button_name);
+}
+
+void AnalogController::SetAxisState(s32 axis_code, float value)
+{
+  if (axis_code < 0 || axis_code >= static_cast<s32>(Button::Count))
+    return;
+
+  // -1..1 -> 0..255
+  const u8 u8_value = static_cast<u8>(std::clamp(((value + 1.0f) / 2.0f) * 255.0f, 0.0f, 255.0f));
+
+  SetAxisState(static_cast<Axis>(axis_code), u8_value);
+}
+
+void AnalogController::SetAxisState(Axis axis, u8 value)
+{
+  m_axis_state[static_cast<u8>(axis)] = value;
+
+  // TODO: Map to buttons in digital mode
+}
+
+void AnalogController::SetButtonState(Button button, bool pressed)
+{
+  if (pressed)
+    m_button_state &= ~(u16(1) << static_cast<u8>(button));
+  else
+    m_button_state |= u16(1) << static_cast<u8>(button);
+}
+
+void AnalogController::SetButtonState(s32 button_code, bool pressed)
+{
+  if (button_code < 0 || button_code >= static_cast<s32>(Button::Count))
+    return;
+
+  SetButtonState(static_cast<Button>(button_code), pressed);
+}
+
+void AnalogController::ResetTransferState()
+{
+  m_state = State::Idle;
+}
+
+u16 AnalogController::GetID() const
+{
+  static constexpr u16 DIGITAL_MODE_ID = 0x5A41;
+  static constexpr u16 ANALOG_MODE_ID = 0x5A73;
+  static constexpr u16 CONFIG_MODE_ID = 0x5AF3;
+
+  if (m_configuration_mode)
+    return CONFIG_MODE_ID;
+
+  return m_analog_mode ? ANALOG_MODE_ID : DIGITAL_MODE_ID;
+}
+
+bool AnalogController::Transfer(const u8 data_in, u8* data_out)
+{
+  bool ack;
+#ifdef Y_BUILD_CONFIG_DEBUG
+  u8 old_state = static_cast<u8>(m_state);
+#endif
+
+  switch (m_state)
+  {
+#define FIXED_REPLY_STATE(state, reply, ack_value, next_state)                                                         \
+  case state:                                                                                                          \
+  {                                                                                                                    \
+    *data_out = reply;                                                                                                 \
+    m_state = next_state;                                                                                              \
+    ack = ack_value;                                                                                                   \
+  }                                                                                                                    \
+  break;
+
+#define ID_STATE_MSB(state, next_state)                                                                                \
+  case state:                                                                                                          \
+  {                                                                                                                    \
+    *data_out = Truncate8(GetID() >> 8);                                                                               \
+    m_state = next_state;                                                                                              \
+    ack = true;                                                                                                        \
+  }                                                                                                                    \
+  break;
+
+    case State::Idle:
+    {
+      // ack when sent 0x01, send ID for 0x42
+      if (data_in == 0x42)
+      {
+        *data_out = Truncate8(GetID());
+        m_state = State::GetStateIDMSB;
+        ack = true;
+      }
+      else if (data_in == 0x43)
+      {
+        *data_out = Truncate8(GetID());
+        m_state = State::ConfigModeIDMSB;
+        ack = true;
+      }
+      else if (m_configuration_mode && data_in == 0x44)
+      {
+        *data_out = Truncate8(GetID());
+        m_state = State::SetAnalogModeIDMSB;
+        ack = true;
+      }
+      else if (m_configuration_mode && data_in == 0x45)
+      {
+        *data_out = Truncate8(GetID());
+        m_state = State::GetAnalogModeIDMSB;
+        ack = true;
+      }
+      else if (m_configuration_mode && data_in == 0x46)
+      {
+        *data_out = Truncate8(GetID());
+        m_state = State::Command46IDMSB;
+        ack = true;
+      }
+      else if (m_configuration_mode && data_in == 0x47)
+      {
+        *data_out = Truncate8(GetID());
+        m_state = State::Command47IDMSB;
+        ack = true;
+      }
+      else if (m_configuration_mode && data_in == 0x4C)
+      {
+        *data_out = Truncate8(GetID());
+        m_state = State::Command4CIDMSB;
+        ack = true;
+      }
+      else if (m_configuration_mode && data_in == 0x4D)
+      {
+        m_analog_mode = true;
+        m_rumble_unlocked = true;
+        *data_out = Truncate8(GetID());
+        m_state = State::UnlockRumbleIDMSB;
+        ack = true;
+      }
+      else
+      {
+        Log_DebugPrintf("data_in = 0x%02X", data_in);
+        *data_out = 0xFF;
+        ack = (data_in == 0x01);
+      }
+    }
+    break;
+
+      ID_STATE_MSB(State::GetStateIDMSB, State::GetStateButtonsLSB);
+      FIXED_REPLY_STATE(State::GetStateButtonsLSB, Truncate8(m_button_state), true, State::GetStateButtonsMSB);
+      FIXED_REPLY_STATE(State::GetStateButtonsMSB, Truncate8(m_button_state >> 8), m_analog_mode,
+                        m_analog_mode ? State::GetStateRightAxisX : State::Idle);
+      FIXED_REPLY_STATE(State::GetStateRightAxisX, Truncate8(m_axis_state[static_cast<u8>(Axis::RightX)]), true,
+                        State::GetStateRightAxisY);
+      FIXED_REPLY_STATE(State::GetStateRightAxisY, Truncate8(m_axis_state[static_cast<u8>(Axis::RightY)]), true,
+                        State::GetStateLeftAxisX);
+      FIXED_REPLY_STATE(State::GetStateLeftAxisX, Truncate8(m_axis_state[static_cast<u8>(Axis::LeftX)]), true,
+                        State::GetStateLeftAxisY);
+      FIXED_REPLY_STATE(State::GetStateLeftAxisY, Truncate8(m_axis_state[static_cast<u8>(Axis::LeftY)]), false,
+                        State::Idle);
+
+      ID_STATE_MSB(State::ConfigModeIDMSB, State::ConfigModeSetMode);
+
+    case State::ConfigModeSetMode:
+    {
+      Log_DebugPrintf("0x%02x(%s) config mode", data_in, data_in == 1 ? "enter" : "leave");
+      m_configuration_mode = (data_in == 1);
+      *data_out = Truncate8(m_button_state);
+      m_state = State::GetStateButtonsMSB;
+      ack = true;
+    }
+    break;
+
+      ID_STATE_MSB(State::SetAnalogModeIDMSB, State::SetAnalogModeVal);
+
+    case State::SetAnalogModeVal:
+    {
+      Log_DebugPrintf("analog mode val 0x%02x", data_in);
+      m_command_param = data_in;
+      *data_out = 0x00;
+      m_state = State::SetAnalogModeSel;
+      ack = true;
+    }
+    break;
+
+    case State::SetAnalogModeSel:
+    {
+      Log_DebugPrintf("analog mode sel 0x%02x", data_in);
+      if (data_in != 0x00)
+        m_analog_mode = (m_command_param == 0x01);
+
+      *data_out = 0x00;
+      m_state = State::Pad4Bytes;
+      ack = true;
+    }
+    break;
+
+      ID_STATE_MSB(State::GetAnalogModeIDMSB, State::GetAnalogMode1);
+      FIXED_REPLY_STATE(State::GetAnalogMode1, 0x01, true, State::GetAnalogMode2);
+      FIXED_REPLY_STATE(State::GetAnalogMode2, 0x02, true, State::GetAnalogMode3);
+      FIXED_REPLY_STATE(State::GetAnalogMode3, BoolToUInt8(m_analog_mode), true, State::GetAnalogMode4);
+      FIXED_REPLY_STATE(State::GetAnalogMode4, 0x02, true, State::GetAnalogMode5);
+      FIXED_REPLY_STATE(State::GetAnalogMode5, 0x01, true, State::GetAnalogMode6);
+      FIXED_REPLY_STATE(State::GetAnalogMode6, 0x00, false, State::Idle);
+
+      ID_STATE_MSB(State::Command46IDMSB, State::Command461);
+
+    case State::Command461:
+    {
+      Log_DebugPrintf("command 46 param 0x%02X", data_in);
+      m_command_param = data_in;
+      *data_out = 0x00;
+      m_state = State::Command462;
+      ack = true;
+    }
+    break;
+
+      FIXED_REPLY_STATE(State::Command462, 0x00, true, State::Command463);
+      FIXED_REPLY_STATE(State::Command463, 0x01, true, State::Command464);
+      FIXED_REPLY_STATE(State::Command464, ((m_command_param == 1) ? 1 : 2), true, State::Command465);
+      FIXED_REPLY_STATE(State::Command465, ((m_command_param == 1) ? 1 : 0), true, State::Command466);
+      FIXED_REPLY_STATE(State::Command466, ((m_command_param == 1) ? 0x14 : 0x0A), false, State::Idle);
+
+      ID_STATE_MSB(State::Command47IDMSB, State::Command471);
+      FIXED_REPLY_STATE(State::Command471, 0x00, true, State::Command472);
+      FIXED_REPLY_STATE(State::Command472, 0x00, true, State::Command473);
+      FIXED_REPLY_STATE(State::Command473, 0x02, true, State::Command474);
+      FIXED_REPLY_STATE(State::Command474, 0x00, true, State::Command475);
+      FIXED_REPLY_STATE(State::Command475, 0x01, true, State::Command476);
+      FIXED_REPLY_STATE(State::Command476, 0x00, false, State::Idle);
+
+      ID_STATE_MSB(State::Command4CIDMSB, State::Command4CMode);
+
+    case State::Command4CMode:
+    {
+      m_analog_mode = (data_in != 0x00);
+      Log_DebugPrintf("analog mode %s by 0x4c", m_analog_mode ? "enabled" : "disabled");
+      *data_out = 0x00;
+      m_state = State::Command4C1;
+      ack = true;
+    }
+    break;
+
+      FIXED_REPLY_STATE(State::Command4C1, 0x00, true, State::Command4C2);
+      FIXED_REPLY_STATE(State::Command4C2, 0x00, true, State::Command4C3);
+      FIXED_REPLY_STATE(State::Command4C3, m_analog_mode ? 0x07 : 0x04, true, State::Command4C4);
+      FIXED_REPLY_STATE(State::Command4C4, 0x00, true, State::Command4C5);
+      FIXED_REPLY_STATE(State::Command4C5, 0x00, false, State::Idle);
+
+      ID_STATE_MSB(State::UnlockRumbleIDMSB, State::Pad6Bytes);
+
+      FIXED_REPLY_STATE(State::Pad6Bytes, 0x00, true, State::Pad5Bytes);
+      FIXED_REPLY_STATE(State::Pad5Bytes, 0x00, true, State::Pad4Bytes);
+      FIXED_REPLY_STATE(State::Pad4Bytes, 0x00, true, State::Pad3Bytes);
+      FIXED_REPLY_STATE(State::Pad3Bytes, 0x00, true, State::Pad2Bytes);
+      FIXED_REPLY_STATE(State::Pad2Bytes, 0x00, true, State::Pad1Byte);
+      FIXED_REPLY_STATE(State::Pad1Byte, 0x00, false, State::Idle);
+
+    default:
+    {
+      UnreachableCode();
+      return false;
+    }
+  }
+
+  Log_DebugPrintf("Transfer, old_state=%u, new_state=%u, data_in=0x%02X, data_out=0x%02X, ack=%s",
+                  static_cast<u32>(old_state), static_cast<u32>(m_state), data_in, *data_out, ack ? "true" : "false");
+  return ack;
+}
+
+std::unique_ptr<AnalogController> AnalogController::Create()
+{
+  return std::make_unique<AnalogController>();
+}
+
+std::optional<s32> AnalogController::StaticGetAxisCodeByName(std::string_view axis_name)
+{
+#define AXIS(name)                                                                                                     \
+  if (axis_name == #name)                                                                                              \
+  {                                                                                                                    \
+    return static_cast<s32>(ZeroExtend32(static_cast<u8>(Axis::name)));                                                \
+  }
+
+  AXIS(LeftX);
+  AXIS(LeftY);
+  AXIS(RightX);
+  AXIS(RightY);
+
+  return std::nullopt;
+
+#undef AXIS
+}
+
+std::optional<s32> AnalogController::StaticGetButtonCodeByName(std::string_view button_name)
+{
+#define BUTTON(name)                                                                                                   \
+  if (button_name == #name)                                                                                            \
+  {                                                                                                                    \
+    return static_cast<s32>(ZeroExtend32(static_cast<u8>(Button::name)));                                              \
+  }
+
+  BUTTON(Select);
+  BUTTON(L3);
+  BUTTON(R3);
+  BUTTON(Start);
+  BUTTON(Up);
+  BUTTON(Right);
+  BUTTON(Down);
+  BUTTON(Left);
+  BUTTON(L2);
+  BUTTON(R2);
+  BUTTON(L1);
+  BUTTON(R1);
+  BUTTON(Triangle);
+  BUTTON(Circle);
+  BUTTON(Cross);
+  BUTTON(Square);
+
+  return std::nullopt;
+
+#undef BUTTON
+}
diff --git a/src/core/analog_controller.h b/src/core/analog_controller.h
new file mode 100644
index 000000000..da627eec9
--- /dev/null
+++ b/src/core/analog_controller.h
@@ -0,0 +1,132 @@
+#pragma once
+#include "controller.h"
+#include <array>
+#include <memory>
+#include <optional>
+#include <string_view>
+
+class AnalogController final : public Controller
+{
+public:
+  enum class Axis : u8
+  {
+    LeftX,
+    LeftY,
+    RightX,
+    RightY,
+    Count
+  };
+
+  enum class Button : u8
+  {
+    Select = 0,
+    L3 = 1,
+    R3 = 2,
+    Start = 3,
+    Up = 4,
+    Right = 5,
+    Down = 6,
+    Left = 7,
+    L2 = 8,
+    R2 = 9,
+    L1 = 10,
+    R1 = 11,
+    Triangle = 12,
+    Circle = 13,
+    Cross = 14,
+    Square = 15,
+    Count
+  };
+
+  AnalogController();
+  ~AnalogController() override;
+
+  static std::unique_ptr<AnalogController> Create();
+  static std::optional<s32> StaticGetAxisCodeByName(std::string_view axis_name);
+  static std::optional<s32> StaticGetButtonCodeByName(std::string_view button_name);
+
+  ControllerType GetType() const override;
+  std::optional<s32> GetAxisCodeByName(std::string_view axis_name) const override;
+  std::optional<s32> GetButtonCodeByName(std::string_view button_name) const override;
+
+  void Reset() override;
+  bool DoState(StateWrapper& sw) override;
+
+  void SetAxisState(s32 axis_code, float value) override;
+  void SetButtonState(s32 button_code, bool pressed) override;
+
+  void ResetTransferState() override;
+  bool Transfer(const u8 data_in, u8* data_out) override;
+
+  void SetAxisState(Axis axis, u8 value);
+  void SetButtonState(Button button, bool pressed);
+
+private:
+  enum class State : u8
+  {
+    Idle,
+    GetStateIDMSB,
+    GetStateButtonsLSB,
+    GetStateButtonsMSB,
+    GetStateRightAxisX,
+    GetStateRightAxisY,
+    GetStateLeftAxisX,
+    GetStateLeftAxisY,
+    ConfigModeIDMSB,
+    ConfigModeSetMode,
+    SetAnalogModeIDMSB,
+    SetAnalogModeVal,
+    SetAnalogModeSel,
+    GetAnalogModeIDMSB,
+    GetAnalogMode1,
+    GetAnalogMode2,
+    GetAnalogMode3,
+    GetAnalogMode4,
+    GetAnalogMode5,
+    GetAnalogMode6,
+    UnlockRumbleIDMSB,
+    Command46IDMSB,
+    Command461,
+    Command462,
+    Command463,
+    Command464,
+    Command465,
+    Command466,
+    Command47IDMSB,
+    Command471,
+    Command472,
+    Command473,
+    Command474,
+    Command475,
+    Command476,
+    Command4CIDMSB,
+    Command4CMode,
+    Command4C1,
+    Command4C2,
+    Command4C3,
+    Command4C4,
+    Command4C5,
+    Pad6Bytes,
+    Pad5Bytes,
+    Pad4Bytes,
+    Pad3Bytes,
+    Pad2Bytes,
+    Pad1Byte,
+  };
+
+  u16 GetID() const;
+
+  bool m_analog_mode = false;
+  bool m_rumble_unlocked = false;
+  bool m_configuration_mode = false;
+  u8 m_command_param = 0;
+
+  std::array<u8, static_cast<u8>(Axis::Count)> m_axis_state{};
+
+  // buttons are active low
+  u16 m_button_state = UINT16_C(0xFFFF);
+
+  std::array<u8, 2> m_motor_state{};
+
+  State m_state = State::Idle;
+};
diff --git a/src/core/controller.cpp b/src/core/controller.cpp
index 34964dc95..666a440d8 100644
--- a/src/core/controller.cpp
+++ b/src/core/controller.cpp
@@ -1,4 +1,5 @@
 #include "controller.h"
+#include "analog_controller.h"
 #include "common/state_wrapper.h"
 #include "digital_controller.h"
 
@@ -29,11 +30,12 @@ std::unique_ptr<Controller> Controller::Create(ControllerType type)
 {
   switch (type)
   {
-    return {};
-
     case ControllerType::DigitalController:
       return DigitalController::Create();
 
+    case ControllerType::AnalogController:
+      return AnalogController::Create();
+
     case ControllerType::None:
     default:
       return {};
@@ -45,6 +47,11 @@ std::optional<s32> Controller::GetAxisCodeByName(std::string_view button_name) c
   return std::nullopt;
 }
 
+std::optional<s32> Controller::GetButtonCodeByName(std::string_view button_name) const
+{
+  return std::nullopt;
+}
+
 std::optional<s32> Controller::GetAxisCodeByName(ControllerType type, std::string_view axis_name)
 {
   switch (type)
@@ -52,6 +59,9 @@ std::optional<s32> Controller::GetAxisCodeByName(ControllerType type, std::strin
     case ControllerType::DigitalController:
       return DigitalController::StaticGetAxisCodeByName(axis_name);
 
+    case ControllerType::AnalogController:
+      return AnalogController::StaticGetAxisCodeByName(axis_name);
+
     case ControllerType::None:
     default:
       return std::nullopt;
@@ -65,13 +75,11 @@ std::optional<s32> Controller::GetButtonCodeByName(ControllerType type, std::str
     case ControllerType::DigitalController:
       return DigitalController::StaticGetButtonCodeByName(button_name);
 
+    case ControllerType::AnalogController:
+      return AnalogController::StaticGetButtonCodeByName(button_name);
+
     case ControllerType::None:
     default:
       return std::nullopt;
   }
 }
-
-std::optional<s32> Controller::GetButtonCodeByName(std::string_view button_name) const
-{
-  return std::nullopt;
-}
diff --git a/src/core/core.vcxproj b/src/core/core.vcxproj
index 7cfa9f99b..ed1e2beed 100644
--- a/src/core/core.vcxproj
+++ b/src/core/core.vcxproj
@@ -35,6 +35,7 @@
     </ProjectConfiguration>
   </ItemGroup>
   <ItemGroup>
+    <ClCompile Include="analog_controller.cpp" />
     <ClCompile Include="bios.cpp" />
     <ClCompile Include="bus.cpp" />
     <ClCompile Include="cdrom.cpp" />
@@ -82,6 +83,7 @@
     <ClCompile Include="timers.cpp" />
   </ItemGroup>
   <ItemGroup>
+    <ClInclude Include="analog_controller.h" />
     <ClInclude Include="bios.h" />
     <ClInclude Include="bus.h" />
     <ClInclude Include="cdrom.h" />
diff --git a/src/core/core.vcxproj.filters b/src/core/core.vcxproj.filters
index f48756ea3..0481e643c 100644
--- a/src/core/core.vcxproj.filters
+++ b/src/core/core.vcxproj.filters
@@ -37,6 +37,7 @@
     <ClCompile Include="gpu_hw_opengl_es.cpp" />
     <ClCompile Include="sio.cpp" />
     <ClCompile Include="controller.cpp" />
+    <ClCompile Include="analog_controller.cpp" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="types.h" />
@@ -76,6 +77,7 @@
     <ClInclude Include="gpu_hw_opengl_es.h" />
     <ClInclude Include="sio.h" />
     <ClInclude Include="controller.h" />
+    <ClInclude Include="analog_controller.h" />
   </ItemGroup>
   <ItemGroup>
     <None Include="cpu_core.inl" />
diff --git a/src/core/settings.cpp b/src/core/settings.cpp
index 13b3691ac..f24d918e4 100644
--- a/src/core/settings.cpp
+++ b/src/core/settings.cpp
@@ -217,8 +217,9 @@ const char* Settings::GetRendererDisplayName(GPURenderer renderer)
   return s_gpu_renderer_display_names[static_cast<int>(renderer)];
 }
 
-static std::array<const char*, 2> s_controller_type_names = {{"None", "DigitalController"}};
-static std::array<const char*, 2> s_controller_display_names = {{"None", "Digital Controller"}};
+static std::array<const char*, 3> s_controller_type_names = {{"None", "DigitalController", "AnalogController"}};
+static std::array<const char*, 3> s_controller_display_names = {
+  {"None", "Digital Controller", "Analog Controller (DualShock)"}};
 
 std::optional<ControllerType> Settings::ParseControllerTypeName(const char* str)
 {
diff --git a/src/core/types.h b/src/core/types.h
index 059c9ec9c..e6df4303d 100644
--- a/src/core/types.h
+++ b/src/core/types.h
@@ -51,6 +51,7 @@ enum class ControllerType
 {
   None,
   DigitalController,
+  AnalogController
 };
 
 enum : u32