2 files changed, 174 insertions, 109 deletions

diff --git a/src/core/hle/service/hid/controllers/npad.cpp b/src/core/hle/service/hid/controllers/npad.cpp
index e742497e1..b65d59373 100644
--- a/src/core/hle/service/hid/controllers/npad.cpp
+++ b/src/core/hle/service/hid/controllers/npad.cpp
@@ -193,7 +193,8 @@ void Controller_NPad::InitNewlyAddedController(std::size_t controller_idx) {
     controller.battery_level[0] = BATTERY_FULL;
     controller.battery_level[1] = BATTERY_FULL;
     controller.battery_level[2] = BATTERY_FULL;
-    styleset_changed_events[controller_idx].writable->Signal();
+
+    SignalStyleSetChangedEvent(IndexToNPad(controller_idx));
 }
 
 void Controller_NPad::OnInit() {
@@ -249,6 +250,9 @@ void Controller_NPad::OnLoadInputDevices() {
         std::transform(players[i].analogs.begin() + Settings::NativeAnalog::STICK_HID_BEGIN,
                        players[i].analogs.begin() + Settings::NativeAnalog::STICK_HID_END,
                        sticks[i].begin(), Input::CreateDevice<Input::AnalogDevice>);
+        std::transform(players[i].motions.begin() + Settings::NativeMotion::MOTION_HID_BEGIN,
+                       players[i].motions.begin() + Settings::NativeMotion::MOTION_HID_END,
+                       motions[i].begin(), Input::CreateDevice<Input::MotionDevice>);
     }
 }
 
@@ -265,6 +269,7 @@ void Controller_NPad::RequestPadStateUpdate(u32 npad_id) {
     auto& rstick_entry = npad_pad_states[controller_idx].r_stick;
     const auto& button_state = buttons[controller_idx];
     const auto& analog_state = sticks[controller_idx];
+    const auto& motion_state = motions[controller_idx];
     const auto [stick_l_x_f, stick_l_y_f] =
         analog_state[static_cast<std::size_t>(JoystickId::Joystick_Left)]->GetStatus();
     const auto [stick_r_x_f, stick_r_y_f] =
@@ -359,6 +364,45 @@ void Controller_NPad::OnUpdate(const Core::Timing::CoreTiming& core_timing, u8*
             continue;
         }
         const u32 npad_index = static_cast<u32>(i);
+
+        const std::array<SixAxisGeneric*, 6> controller_sixaxes{
+            &npad.sixaxis_full,       &npad.sixaxis_handheld, &npad.sixaxis_dual_left,
+            &npad.sixaxis_dual_right, &npad.sixaxis_left,     &npad.sixaxis_right,
+        };
+
+        for (auto* sixaxis_sensor : controller_sixaxes) {
+            sixaxis_sensor->common.entry_count = 16;
+            sixaxis_sensor->common.total_entry_count = 17;
+
+            const auto& last_entry =
+                sixaxis_sensor->sixaxis[sixaxis_sensor->common.last_entry_index];
+
+            sixaxis_sensor->common.timestamp = core_timing.GetCPUTicks();
+            sixaxis_sensor->common.last_entry_index =
+                (sixaxis_sensor->common.last_entry_index + 1) % 17;
+
+            auto& cur_entry = sixaxis_sensor->sixaxis[sixaxis_sensor->common.last_entry_index];
+
+            cur_entry.timestamp = last_entry.timestamp + 1;
+            cur_entry.timestamp2 = cur_entry.timestamp;
+        }
+
+        // Try to read sixaxis sensor states
+        std::array<MotionDevice, 2> motion_devices;
+
+        if (sixaxis_sensors_enabled && Settings::values.motion_enabled) {
+            sixaxis_at_rest = true;
+            for (std::size_t e = 0; e < motion_devices.size(); ++e) {
+                const auto& device = motions[i][e];
+                if (device) {
+                    std::tie(motion_devices[e].accel, motion_devices[e].gyro,
+                             motion_devices[e].rotation, motion_devices[e].orientation) =
+                        device->GetStatus();
+                    sixaxis_at_rest = sixaxis_at_rest && motion_devices[e].gyro.Length2() < 0.0001f;
+                }
+            }
+        }
+
         RequestPadStateUpdate(npad_index);
         auto& pad_state = npad_pad_states[npad_index];
 
@@ -376,6 +420,18 @@ void Controller_NPad::OnUpdate(const Core::Timing::CoreTiming& core_timing, u8*
 
         libnx_entry.connection_status.raw = 0;
         libnx_entry.connection_status.IsConnected.Assign(1);
+        auto& full_sixaxis_entry =
+            npad.sixaxis_full.sixaxis[npad.sixaxis_full.common.last_entry_index];
+        auto& handheld_sixaxis_entry =
+            npad.sixaxis_handheld.sixaxis[npad.sixaxis_handheld.common.last_entry_index];
+        auto& dual_left_sixaxis_entry =
+            npad.sixaxis_dual_left.sixaxis[npad.sixaxis_dual_left.common.last_entry_index];
+        auto& dual_right_sixaxis_entry =
+            npad.sixaxis_dual_right.sixaxis[npad.sixaxis_dual_right.common.last_entry_index];
+        auto& left_sixaxis_entry =
+            npad.sixaxis_left.sixaxis[npad.sixaxis_left.common.last_entry_index];
+        auto& right_sixaxis_entry =
+            npad.sixaxis_right.sixaxis[npad.sixaxis_right.common.last_entry_index];
 
         switch (controller_type) {
         case NPadControllerType::None:
@@ -390,6 +446,13 @@ void Controller_NPad::OnUpdate(const Core::Timing::CoreTiming& core_timing, u8*
             main_controller.pad.r_stick = pad_state.r_stick;
 
             libnx_entry.connection_status.IsWired.Assign(1);
+
+            if (sixaxis_sensors_enabled && motions[i][0]) {
+                full_sixaxis_entry.accel = motion_devices[0].accel;
+                full_sixaxis_entry.gyro = motion_devices[0].gyro;
+                full_sixaxis_entry.rotation = motion_devices[0].rotation;
+                full_sixaxis_entry.orientation = motion_devices[0].orientation;
+            }
             break;
         case NPadControllerType::Handheld:
             handheld_entry.connection_status.raw = 0;
@@ -408,6 +471,13 @@ void Controller_NPad::OnUpdate(const Core::Timing::CoreTiming& core_timing, u8*
             libnx_entry.connection_status.IsRightJoyConnected.Assign(1);
             libnx_entry.connection_status.IsLeftJoyWired.Assign(1);
             libnx_entry.connection_status.IsRightJoyWired.Assign(1);
+
+            if (sixaxis_sensors_enabled && motions[i][0]) {
+                handheld_sixaxis_entry.accel = motion_devices[0].accel;
+                handheld_sixaxis_entry.gyro = motion_devices[0].gyro;
+                handheld_sixaxis_entry.rotation = motion_devices[0].rotation;
+                handheld_sixaxis_entry.orientation = motion_devices[0].orientation;
+            }
             break;
         case NPadControllerType::JoyDual:
             dual_entry.connection_status.raw = 0;
@@ -420,6 +490,21 @@ void Controller_NPad::OnUpdate(const Core::Timing::CoreTiming& core_timing, u8*
 
             libnx_entry.connection_status.IsLeftJoyConnected.Assign(1);
             libnx_entry.connection_status.IsRightJoyConnected.Assign(1);
+
+            if (sixaxis_sensors_enabled && motions[i][0]) {
+                // Set motion for the left joycon
+                dual_left_sixaxis_entry.accel = motion_devices[0].accel;
+                dual_left_sixaxis_entry.gyro = motion_devices[0].gyro;
+                dual_left_sixaxis_entry.rotation = motion_devices[0].rotation;
+                dual_left_sixaxis_entry.orientation = motion_devices[0].orientation;
+            }
+            if (sixaxis_sensors_enabled && motions[i][1]) {
+                // Set motion for the right joycon
+                dual_right_sixaxis_entry.accel = motion_devices[1].accel;
+                dual_right_sixaxis_entry.gyro = motion_devices[1].gyro;
+                dual_right_sixaxis_entry.rotation = motion_devices[1].rotation;
+                dual_right_sixaxis_entry.orientation = motion_devices[1].orientation;
+            }
             break;
         case NPadControllerType::JoyLeft:
             left_entry.connection_status.raw = 0;
@@ -430,6 +515,13 @@ void Controller_NPad::OnUpdate(const Core::Timing::CoreTiming& core_timing, u8*
             left_entry.pad.r_stick = pad_state.r_stick;
 
             libnx_entry.connection_status.IsLeftJoyConnected.Assign(1);
+
+            if (sixaxis_sensors_enabled && motions[i][0]) {
+                left_sixaxis_entry.accel = motion_devices[0].accel;
+                left_sixaxis_entry.gyro = motion_devices[0].gyro;
+                left_sixaxis_entry.rotation = motion_devices[0].rotation;
+                left_sixaxis_entry.orientation = motion_devices[0].orientation;
+            }
             break;
         case NPadControllerType::JoyRight:
             right_entry.connection_status.raw = 0;
@@ -440,6 +532,13 @@ void Controller_NPad::OnUpdate(const Core::Timing::CoreTiming& core_timing, u8*
             right_entry.pad.r_stick = pad_state.r_stick;
 
             libnx_entry.connection_status.IsRightJoyConnected.Assign(1);
+
+            if (sixaxis_sensors_enabled && motions[i][1]) {
+                right_sixaxis_entry.accel = motion_devices[1].accel;
+                right_sixaxis_entry.gyro = motion_devices[1].gyro;
+                right_sixaxis_entry.rotation = motion_devices[1].rotation;
+                right_sixaxis_entry.orientation = motion_devices[1].orientation;
+            }
             break;
         case NPadControllerType::Pokeball:
             pokeball_entry.connection_status.raw = 0;
@@ -518,13 +617,17 @@ void Controller_NPad::VibrateController(const std::vector<u32>& controller_ids,
     last_processed_vibration = vibrations.back();
 }
 
+Controller_NPad::Vibration Controller_NPad::GetLastVibration() const {
+    return last_processed_vibration;
+}
+
 std::shared_ptr<Kernel::ReadableEvent> Controller_NPad::GetStyleSetChangedEvent(u32 npad_id) const {
     const auto& styleset_event = styleset_changed_events[NPadIdToIndex(npad_id)];
     return styleset_event.readable;
 }
 
-Controller_NPad::Vibration Controller_NPad::GetLastVibration() const {
-    return last_processed_vibration;
+void Controller_NPad::SignalStyleSetChangedEvent(u32 npad_id) const {
+    styleset_changed_events[NPadIdToIndex(npad_id)].writable->Signal();
 }
 
 void Controller_NPad::AddNewControllerAt(NPadControllerType controller, std::size_t npad_index) {
@@ -534,7 +637,7 @@ void Controller_NPad::AddNewControllerAt(NPadControllerType controller, std::siz
 void Controller_NPad::UpdateControllerAt(NPadControllerType controller, std::size_t npad_index,
                                          bool connected) {
     if (!connected) {
-        DisconnectNPad(IndexToNPad(npad_index));
+        DisconnectNPadAtIndex(npad_index);
         return;
     }
 
@@ -554,16 +657,19 @@ void Controller_NPad::UpdateControllerAt(NPadControllerType controller, std::siz
 }
 
 void Controller_NPad::DisconnectNPad(u32 npad_id) {
-    const auto npad_index = NPadIdToIndex(npad_id);
-    connected_controllers[npad_index].is_connected = false;
+    DisconnectNPadAtIndex(NPadIdToIndex(npad_id));
+}
+
+void Controller_NPad::DisconnectNPadAtIndex(std::size_t npad_index) {
     Settings::values.players[npad_index].connected = false;
+    connected_controllers[npad_index].is_connected = false;
 
     auto& controller = shared_memory_entries[npad_index];
     controller.joy_styles.raw = 0; // Zero out
     controller.device_type.raw = 0;
     controller.properties.raw = 0;
 
-    styleset_changed_events[npad_index].writable->Signal();
+    SignalStyleSetChangedEvent(IndexToNPad(npad_index));
 }
 
 void Controller_NPad::SetGyroscopeZeroDriftMode(GyroscopeZeroDriftMode drift_mode) {
@@ -574,6 +680,14 @@ Controller_NPad::GyroscopeZeroDriftMode Controller_NPad::GetGyroscopeZeroDriftMo
     return gyroscope_zero_drift_mode;
 }
 
+bool Controller_NPad::IsSixAxisSensorAtRest() const {
+    return sixaxis_at_rest;
+}
+
+void Controller_NPad::SetSixAxisEnabled(bool six_axis_status) {
+    sixaxis_sensors_enabled = six_axis_status;
+}
+
 void Controller_NPad::MergeSingleJoyAsDualJoy(u32 npad_id_1, u32 npad_id_2) {
     const auto npad_index_1 = NPadIdToIndex(npad_id_1);
     const auto npad_index_2 = NPadIdToIndex(npad_id_2);
@@ -666,13 +780,13 @@ void Controller_NPad::ClearAllConnectedControllers() {
 }
 
 void Controller_NPad::DisconnectAllConnectedControllers() {
-    for (ControllerHolder& controller : connected_controllers) {
+    for (auto& controller : connected_controllers) {
         controller.is_connected = false;
     }
 }
 
 void Controller_NPad::ConnectAllDisconnectedControllers() {
-    for (ControllerHolder& controller : connected_controllers) {
+    for (auto& controller : connected_controllers) {
         if (controller.type != NPadControllerType::None && !controller.is_connected) {
             controller.is_connected = true;
         }
@@ -680,7 +794,7 @@ void Controller_NPad::ConnectAllDisconnectedControllers() {
 }
 
 void Controller_NPad::ClearAllControllers() {
-    for (ControllerHolder& controller : connected_controllers) {
+    for (auto& controller : connected_controllers) {
         controller.type = NPadControllerType::None;
         controller.is_connected = false;
     }
@@ -728,92 +842,4 @@ bool Controller_NPad::IsControllerSupported(NPadControllerType controller) const
     return false;
 }
 
-Controller_NPad::NPadControllerType Controller_NPad::DecideBestController(
-    NPadControllerType priority) const {
-    if (IsControllerSupported(priority)) {
-        return priority;
-    }
-    const auto is_docked = Settings::values.use_docked_mode;
-    if (is_docked && priority == NPadControllerType::Handheld) {
-        priority = NPadControllerType::JoyDual;
-        if (IsControllerSupported(priority)) {
-            return priority;
-        }
-    }
-    std::vector<NPadControllerType> priority_list;
-    switch (priority) {
-    case NPadControllerType::ProController:
-        priority_list.push_back(NPadControllerType::JoyDual);
-        if (!is_docked) {
-            priority_list.push_back(NPadControllerType::Handheld);
-        }
-        priority_list.push_back(NPadControllerType::JoyLeft);
-        priority_list.push_back(NPadControllerType::JoyRight);
-        priority_list.push_back(NPadControllerType::Pokeball);
-        break;
-    case NPadControllerType::Handheld:
-        priority_list.push_back(NPadControllerType::JoyDual);
-        priority_list.push_back(NPadControllerType::ProController);
-        priority_list.push_back(NPadControllerType::JoyLeft);
-        priority_list.push_back(NPadControllerType::JoyRight);
-        priority_list.push_back(NPadControllerType::Pokeball);
-        break;
-    case NPadControllerType::JoyDual:
-        if (!is_docked) {
-            priority_list.push_back(NPadControllerType::Handheld);
-        }
-        priority_list.push_back(NPadControllerType::ProController);
-        priority_list.push_back(NPadControllerType::JoyLeft);
-        priority_list.push_back(NPadControllerType::JoyRight);
-        priority_list.push_back(NPadControllerType::Pokeball);
-        break;
-    case NPadControllerType::JoyLeft:
-        priority_list.push_back(NPadControllerType::JoyRight);
-        priority_list.push_back(NPadControllerType::JoyDual);
-        if (!is_docked) {
-            priority_list.push_back(NPadControllerType::Handheld);
-        }
-        priority_list.push_back(NPadControllerType::ProController);
-        priority_list.push_back(NPadControllerType::Pokeball);
-        break;
-    case NPadControllerType::JoyRight:
-        priority_list.push_back(NPadControllerType::JoyLeft);
-        priority_list.push_back(NPadControllerType::JoyDual);
-        if (!is_docked) {
-            priority_list.push_back(NPadControllerType::Handheld);
-        }
-        priority_list.push_back(NPadControllerType::ProController);
-        priority_list.push_back(NPadControllerType::Pokeball);
-        break;
-    case NPadControllerType::Pokeball:
-        priority_list.push_back(NPadControllerType::JoyLeft);
-        priority_list.push_back(NPadControllerType::JoyRight);
-        priority_list.push_back(NPadControllerType::JoyDual);
-        if (!is_docked) {
-            priority_list.push_back(NPadControllerType::Handheld);
-        }
-        priority_list.push_back(NPadControllerType::ProController);
-        break;
-    default:
-        priority_list.push_back(NPadControllerType::JoyDual);
-        if (!is_docked) {
-            priority_list.push_back(NPadControllerType::Handheld);
-        }
-        priority_list.push_back(NPadControllerType::ProController);
-        priority_list.push_back(NPadControllerType::JoyLeft);
-        priority_list.push_back(NPadControllerType::JoyRight);
-        priority_list.push_back(NPadControllerType::JoyDual);
-        break;
-    }
-
-    const auto iter = std::find_if(priority_list.begin(), priority_list.end(),
-                                   [this](auto type) { return IsControllerSupported(type); });
-    if (iter == priority_list.end()) {
-        UNIMPLEMENTED_MSG("Could not find supported controller!");
-        return priority;
-    }
-
-    return *iter;
-}
-
 } // namespace Service::HID
diff --git a/src/core/hle/service/hid/controllers/npad.h b/src/core/hle/service/hid/controllers/npad.h
index ad25c6fbf..78e7c320b 100644
--- a/src/core/hle/service/hid/controllers/npad.h
+++ b/src/core/hle/service/hid/controllers/npad.h
@@ -115,17 +115,23 @@ public:
     void VibrateController(const std::vector<u32>& controller_ids,
                            const std::vector<Vibration>& vibrations);
 
-    std::shared_ptr<Kernel::ReadableEvent> GetStyleSetChangedEvent(u32 npad_id) const;
     Vibration GetLastVibration() const;
 
+    std::shared_ptr<Kernel::ReadableEvent> GetStyleSetChangedEvent(u32 npad_id) const;
+    void SignalStyleSetChangedEvent(u32 npad_id) const;
+
     // Adds a new controller at an index.
     void AddNewControllerAt(NPadControllerType controller, std::size_t npad_index);
     // Adds a new controller at an index with connection status.
     void UpdateControllerAt(NPadControllerType controller, std::size_t npad_index, bool connected);
 
     void DisconnectNPad(u32 npad_id);
+    void DisconnectNPadAtIndex(std::size_t index);
+
     void SetGyroscopeZeroDriftMode(GyroscopeZeroDriftMode drift_mode);
     GyroscopeZeroDriftMode GetGyroscopeZeroDriftMode() const;
+    bool IsSixAxisSensorAtRest() const;
+    void SetSixAxisEnabled(bool six_axis_status);
     LedPattern GetLedPattern(u32 npad_id);
     void SetVibrationEnabled(bool can_vibrate);
     bool IsVibrationEnabled() const;
@@ -248,6 +254,24 @@ private:
     };
     static_assert(sizeof(NPadGeneric) == 0x350, "NPadGeneric is an invalid size");
 
+    struct SixAxisStates {
+        s64_le timestamp{};
+        INSERT_PADDING_WORDS(2);
+        s64_le timestamp2{};
+        Common::Vec3f accel{};
+        Common::Vec3f gyro{};
+        Common::Vec3f rotation{};
+        std::array<Common::Vec3f, 3> orientation{};
+        s64_le always_one{1};
+    };
+    static_assert(sizeof(SixAxisStates) == 0x68, "SixAxisStates is an invalid size");
+
+    struct SixAxisGeneric {
+        CommonHeader common{};
+        std::array<SixAxisStates, 17> sixaxis{};
+    };
+    static_assert(sizeof(SixAxisGeneric) == 0x708, "SixAxisGeneric is an invalid size");
+
     enum class ColorReadError : u32_le {
         ReadOk = 0,
         ColorDoesntExist = 1,
@@ -277,6 +301,13 @@ private:
         };
     };
 
+    struct MotionDevice {
+        Common::Vec3f accel;
+        Common::Vec3f gyro;
+        Common::Vec3f rotation;
+        std::array<Common::Vec3f, 3> orientation;
+    };
+
     struct NPadEntry {
         NPadType joy_styles;
         NPadAssignments pad_assignment;
@@ -296,9 +327,12 @@ private:
         NPadGeneric pokeball_states;
         NPadGeneric libnx; // TODO(ogniK): Find out what this actually is, libnx seems to only be
                            // relying on this for the time being
-        INSERT_PADDING_BYTES(
-            0x708 *
-            6); // TODO(ogniK): SixAxis states, require more information before implementation
+        SixAxisGeneric sixaxis_full;
+        SixAxisGeneric sixaxis_handheld;
+        SixAxisGeneric sixaxis_dual_left;
+        SixAxisGeneric sixaxis_dual_right;
+        SixAxisGeneric sixaxis_left;
+        SixAxisGeneric sixaxis_right;
         NPadDevice device_type;
         NPadProperties properties;
         INSERT_PADDING_WORDS(1);
@@ -315,21 +349,24 @@ private:
 
     void InitNewlyAddedController(std::size_t controller_idx);
     bool IsControllerSupported(NPadControllerType controller) const;
-    NPadControllerType DecideBestController(NPadControllerType priority) const;
     void RequestPadStateUpdate(u32 npad_id);
 
     u32 press_state{};
 
     NPadType style{};
     std::array<NPadEntry, 10> shared_memory_entries{};
-    std::array<
+    using ButtonArray = std::array<
         std::array<std::unique_ptr<Input::ButtonDevice>, Settings::NativeButton::NUM_BUTTONS_HID>,
-        10>
-        buttons;
-    std::array<
+        10>;
+    using StickArray = std::array<
         std::array<std::unique_ptr<Input::AnalogDevice>, Settings::NativeAnalog::NUM_STICKS_HID>,
-        10>
-        sticks;
+        10>;
+    using MotionArray = std::array<
+        std::array<std::unique_ptr<Input::MotionDevice>, Settings::NativeMotion::NUM_MOTION_HID>,
+        10>;
+    ButtonArray buttons;
+    StickArray sticks;
+    MotionArray motions;
     std::vector<u32> supported_npad_id_types{};
     NpadHoldType hold_type{NpadHoldType::Vertical};
     // Each controller should have their own styleset changed event
@@ -338,6 +375,8 @@ private:
     std::array<ControllerHolder, 10> connected_controllers{};
     GyroscopeZeroDriftMode gyroscope_zero_drift_mode{GyroscopeZeroDriftMode::Standard};
     bool can_controllers_vibrate{true};
+    bool sixaxis_sensors_enabled{true};
+    bool sixaxis_at_rest{true};
     std::array<ControllerPad, 10> npad_pad_states{};
     bool is_in_lr_assignment_mode{false};
     Core::System& system;