4 files changed, 195 insertions, 15 deletions

diff --git a/src/core/hle/service/hid/controllers/npad.cpp b/src/core/hle/service/hid/controllers/npad.cpp
index e742497e1..acf748bf1 100644
--- a/src/core/hle/service/hid/controllers/npad.cpp
+++ b/src/core/hle/service/hid/controllers/npad.cpp
@@ -249,6 +249,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::RealMotionDevice>);
     }
 }
 
@@ -265,6 +268,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 +363,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) {
+            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() < 1.0f;
+                }
+            }
+        }
+
         RequestPadStateUpdate(npad_index);
         auto& pad_state = npad_pad_states[npad_index];
 
@@ -376,6 +419,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 +445,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 +470,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 +489,32 @@ 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) {
+                if (motions[i][0] && motions[i][1]) {
+                    // set both
+                    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;
+                    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;
+                } else if (motions[i][0]) {
+                    // set right
+                    dual_right_sixaxis_entry.accel = motion_devices[0].accel;
+                    dual_right_sixaxis_entry.gyro = motion_devices[0].gyro;
+                    dual_right_sixaxis_entry.rotation = motion_devices[0].rotation;
+                    dual_right_sixaxis_entry.orientation = motion_devices[0].orientation;
+                } else if (motions[i][1]) {
+                    // set right
+                    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 +525,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 +542,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][0]) {
+                right_sixaxis_entry.accel = motion_devices[0].accel;
+                right_sixaxis_entry.gyro = motion_devices[0].gyro;
+                right_sixaxis_entry.rotation = motion_devices[0].rotation;
+                right_sixaxis_entry.orientation = motion_devices[0].orientation;
+            }
             break;
         case NPadControllerType::Pokeball:
             pokeball_entry.connection_status.raw = 0;
@@ -574,6 +683,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);
diff --git a/src/core/hle/service/hid/controllers/npad.h b/src/core/hle/service/hid/controllers/npad.h
index ad25c6fbf..99d7e459a 100644
--- a/src/core/hle/service/hid/controllers/npad.h
+++ b/src/core/hle/service/hid/controllers/npad.h
@@ -126,6 +126,8 @@ public:
     void DisconnectNPad(u32 npad_id);
     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 +250,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 +297,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 +323,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);
@@ -322,14 +352,18 @@ private:
 
     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::RealMotionDevice>,
+                                              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 +372,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;
diff --git a/src/core/hle/service/hid/hid.cpp b/src/core/hle/service/hid/hid.cpp
index bd3c2f26b..302a25540 100644
--- a/src/core/hle/service/hid/hid.cpp
+++ b/src/core/hle/service/hid/hid.cpp
@@ -164,8 +164,8 @@ Hid::Hid(Core::System& system) : ServiceFramework("hid"), system(system) {
         {56, nullptr, "ActivateJoyXpad"},
         {58, nullptr, "GetJoyXpadLifoHandle"},
         {59, nullptr, "GetJoyXpadIds"},
-        {60, nullptr, "ActivateSixAxisSensor"},
-        {61, nullptr, "DeactivateSixAxisSensor"},
+        {60, &Hid::ActivateSixAxisSensor, "ActivateSixAxisSensor"},
+        {61, &Hid::DeactivateSixAxisSensor, "DeactivateSixAxisSensor"},
         {62, nullptr, "GetSixAxisSensorLifoHandle"},
         {63, nullptr, "ActivateJoySixAxisSensor"},
         {64, nullptr, "DeactivateJoySixAxisSensor"},
@@ -329,6 +329,31 @@ void Hid::GetXpadIDs(Kernel::HLERequestContext& ctx) {
     rb.Push(0);
 }
 
+void Hid::ActivateSixAxisSensor(Kernel::HLERequestContext& ctx) {
+    IPC::RequestParser rp{ctx};
+    const auto handle{rp.Pop<u32>()};
+    const auto applet_resource_user_id{rp.Pop<u64>()};
+    applet_resource->GetController<Controller_NPad>(HidController::NPad).SetSixAxisEnabled(true);
+    LOG_DEBUG(Service_HID, "called, handle={}, applet_resource_user_id={}", handle,
+              applet_resource_user_id);
+
+    IPC::ResponseBuilder rb{ctx, 2};
+    rb.Push(RESULT_SUCCESS);
+}
+
+void Hid::DeactivateSixAxisSensor(Kernel::HLERequestContext& ctx) {
+    IPC::RequestParser rp{ctx};
+    const auto handle{rp.Pop<u32>()};
+    const auto applet_resource_user_id{rp.Pop<u64>()};
+    applet_resource->GetController<Controller_NPad>(HidController::NPad).SetSixAxisEnabled(false);
+
+    LOG_DEBUG(Service_HID, "called, handle={}, applet_resource_user_id={}", handle,
+              applet_resource_user_id);
+
+    IPC::ResponseBuilder rb{ctx, 2};
+    rb.Push(RESULT_SUCCESS);
+}
+
 void Hid::ActivateDebugPad(Kernel::HLERequestContext& ctx) {
     IPC::RequestParser rp{ctx};
     const auto applet_resource_user_id{rp.Pop<u64>()};
@@ -484,13 +509,13 @@ void Hid::IsSixAxisSensorAtRest(Kernel::HLERequestContext& ctx) {
     const auto handle{rp.Pop<u32>()};
     const auto applet_resource_user_id{rp.Pop<u64>()};
 
-    LOG_WARNING(Service_HID, "(STUBBED) called, handle={}, applet_resource_user_id={}", handle,
-                applet_resource_user_id);
+    LOG_DEBUG(Service_HID, "called, handle={}, applet_resource_user_id={}", handle,
+              applet_resource_user_id);
 
     IPC::ResponseBuilder rb{ctx, 3};
     rb.Push(RESULT_SUCCESS);
-    // TODO (Hexagon12): Properly implement reading gyroscope values from controllers.
-    rb.Push(true);
+    rb.Push(applet_resource->GetController<Controller_NPad>(HidController::NPad)
+                .IsSixAxisSensorAtRest());
 }
 
 void Hid::SetSupportedNpadStyleSet(Kernel::HLERequestContext& ctx) {
diff --git a/src/core/hle/service/hid/hid.h b/src/core/hle/service/hid/hid.h
index efb07547f..e04aaf1e9 100644
--- a/src/core/hle/service/hid/hid.h
+++ b/src/core/hle/service/hid/hid.h
@@ -86,6 +86,8 @@ private:
     void CreateAppletResource(Kernel::HLERequestContext& ctx);
     void ActivateXpad(Kernel::HLERequestContext& ctx);
     void GetXpadIDs(Kernel::HLERequestContext& ctx);
+    void ActivateSixAxisSensor(Kernel::HLERequestContext& ctx);
+    void DeactivateSixAxisSensor(Kernel::HLERequestContext& ctx);
     void ActivateDebugPad(Kernel::HLERequestContext& ctx);
     void ActivateTouchScreen(Kernel::HLERequestContext& ctx);
     void ActivateMouse(Kernel::HLERequestContext& ctx);