7 files changed, 201 insertions, 12 deletions

diff --git a/src/core/hle/kernel/transfer_memory.cpp b/src/core/hle/kernel/transfer_memory.cpp
index cad063e4d..1dd65468d 100644
--- a/src/core/hle/kernel/transfer_memory.cpp
+++ b/src/core/hle/kernel/transfer_memory.cpp
@@ -36,6 +36,10 @@ std::shared_ptr<TransferMemory> TransferMemory::Create(KernelCore& kernel,
     return transfer_memory;
 }
 
+u8* TransferMemory::GetPointer() {
+    return memory.GetPointer(base_address);
+}
+
 const u8* TransferMemory::GetPointer() const {
     return memory.GetPointer(base_address);
 }
diff --git a/src/core/hle/kernel/transfer_memory.h b/src/core/hle/kernel/transfer_memory.h
index 521951424..59328c0fe 100644
--- a/src/core/hle/kernel/transfer_memory.h
+++ b/src/core/hle/kernel/transfer_memory.h
@@ -57,6 +57,9 @@ public:
     }
 
     /// Gets a pointer to the backing block of this instance.
+    u8* GetPointer();
+
+    /// Gets a pointer to the backing block of this instance.
     const u8* GetPointer() const;
 
     /// Gets the size of the memory backing this instance in bytes.
diff --git a/src/core/hle/service/hid/controllers/console_sixaxis.cpp b/src/core/hle/service/hid/controllers/console_sixaxis.cpp
new file mode 100644
index 000000000..801e14b79
--- /dev/null
+++ b/src/core/hle/service/hid/controllers/console_sixaxis.cpp
@@ -0,0 +1,90 @@
+// Copyright 2021 yuzu Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/settings.h"
+#include "core/core_timing.h"
+#include "core/hle/service/hid/controllers/console_sixaxis.h"
+
+namespace Service::HID {
+constexpr std::size_t SHARED_MEMORY_OFFSET = 0x3C200;
+
+Controller_ConsoleSixAxis::Controller_ConsoleSixAxis(Core::System& system)
+    : ControllerBase(system) {}
+Controller_ConsoleSixAxis::~Controller_ConsoleSixAxis() = default;
+
+void Controller_ConsoleSixAxis::OnInit() {}
+
+void Controller_ConsoleSixAxis::OnRelease() {}
+
+void Controller_ConsoleSixAxis::OnUpdate(const Core::Timing::CoreTiming& core_timing, u8* data,
+                                         std::size_t size) {
+    seven_six_axis.header.timestamp = core_timing.GetCPUTicks();
+    seven_six_axis.header.total_entry_count = 17;
+
+    if (!IsControllerActivated() || !is_transfer_memory_set) {
+        seven_six_axis.header.entry_count = 0;
+        seven_six_axis.header.last_entry_index = 0;
+        return;
+    }
+    seven_six_axis.header.entry_count = 16;
+
+    const auto& last_entry =
+        seven_six_axis.sevensixaxis_states[seven_six_axis.header.last_entry_index];
+    seven_six_axis.header.last_entry_index = (seven_six_axis.header.last_entry_index + 1) % 17;
+    auto& cur_entry = seven_six_axis.sevensixaxis_states[seven_six_axis.header.last_entry_index];
+
+    cur_entry.sampling_number = last_entry.sampling_number + 1;
+    cur_entry.sampling_number2 = cur_entry.sampling_number;
+
+    // Try to read sixaxis sensor states
+    MotionDevice motion_device{};
+    const auto& device = motions[0];
+    if (device) {
+        std::tie(motion_device.accel, motion_device.gyro, motion_device.rotation,
+                 motion_device.orientation, motion_device.quaternion) = device->GetStatus();
+        console_six_axis.is_seven_six_axis_sensor_at_rest = motion_device.gyro.Length2() < 0.0001f;
+    }
+
+    cur_entry.accel = motion_device.accel;
+    // Zero gyro values as they just mess up with the camera
+    // Note: Probably a correct sensivity setting must be set
+    cur_entry.gyro = {};
+    cur_entry.quaternion = {
+        {
+            motion_device.quaternion.xyz.y,
+            motion_device.quaternion.xyz.x,
+            -motion_device.quaternion.w,
+        },
+        -motion_device.quaternion.xyz.z,
+    };
+
+    console_six_axis.sampling_number++;
+    // TODO(German77): Find the purpose of those values
+    console_six_axis.verticalization_error = 0.0f;
+    console_six_axis.gyro_bias = {0.0f, 0.0f, 0.0f};
+
+    // Update console six axis shared memory
+    std::memcpy(data + SHARED_MEMORY_OFFSET, &console_six_axis, sizeof(console_six_axis));
+    // Update seven six axis transfer memory
+    std::memcpy(transfer_memory, &seven_six_axis, sizeof(seven_six_axis));
+}
+
+void Controller_ConsoleSixAxis::OnLoadInputDevices() {
+    const auto player = Settings::values.players.GetValue()[0];
+    std::transform(player.motions.begin() + Settings::NativeMotion::MOTION_HID_BEGIN,
+                   player.motions.begin() + Settings::NativeMotion::MOTION_HID_END, motions.begin(),
+                   Input::CreateDevice<Input::MotionDevice>);
+}
+
+void Controller_ConsoleSixAxis::SetTransferMemoryPointer(u8* t_mem_1) {
+    is_transfer_memory_set = true;
+    transfer_memory = t_mem_1;
+};
+
+void Controller_ConsoleSixAxis::ResetTimestamp() {
+    auto& cur_entry = seven_six_axis.sevensixaxis_states[seven_six_axis.header.last_entry_index];
+    cur_entry.sampling_number = 0;
+    cur_entry.sampling_number2 = 0;
+}
+} // namespace Service::HID
diff --git a/src/core/hle/service/hid/controllers/console_sixaxis.h b/src/core/hle/service/hid/controllers/console_sixaxis.h
new file mode 100644
index 000000000..ac0501683
--- /dev/null
+++ b/src/core/hle/service/hid/controllers/console_sixaxis.h
@@ -0,0 +1,80 @@
+// Copyright 2021 yuzu Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include <array>
+#include "common/bit_field.h"
+#include "common/common_types.h"
+#include "common/quaternion.h"
+#include "core/frontend/input.h"
+#include "core/hle/service/hid/controllers/controller_base.h"
+
+namespace Service::HID {
+class Controller_ConsoleSixAxis final : public ControllerBase {
+public:
+    explicit Controller_ConsoleSixAxis(Core::System& system);
+    ~Controller_ConsoleSixAxis() override;
+
+    // Called when the controller is initialized
+    void OnInit() override;
+
+    // When the controller is released
+    void OnRelease() override;
+
+    // When the controller is requesting an update for the shared memory
+    void OnUpdate(const Core::Timing::CoreTiming& core_timing, u8* data, size_t size) override;
+
+    // Called when input devices should be loaded
+    void OnLoadInputDevices() override;
+
+    // Called on InitializeSevenSixAxisSensor
+    void SetTransferMemoryPointer(u8* t_mem_1);
+
+    // Called on ResetSevenSixAxisSensorTimestamp
+    void ResetTimestamp();
+
+private:
+    struct SevenSixAxisState {
+        INSERT_PADDING_WORDS(4); // unused
+        s64_le sampling_number{};
+        s64_le sampling_number2{};
+        u64 unknown{};
+        Common::Vec3f accel{};
+        Common::Vec3f gyro{};
+        Common::Quaternion<f32> quaternion{};
+    };
+    static_assert(sizeof(SevenSixAxisState) == 0x50, "SevenSixAxisState is an invalid size");
+
+    struct SevenSixAxisMemory {
+        CommonHeader header{};
+        std::array<SevenSixAxisState, 0x21> sevensixaxis_states{};
+    };
+    static_assert(sizeof(SevenSixAxisMemory) == 0xA70, "SevenSixAxisMemory is an invalid size");
+
+    struct ConsoleSharedMemory {
+        u64_le sampling_number{};
+        bool is_seven_six_axis_sensor_at_rest{};
+        f32 verticalization_error{};
+        Common::Vec3f gyro_bias{};
+    };
+    static_assert(sizeof(ConsoleSharedMemory) == 0x20, "ConsoleSharedMemory is an invalid size");
+
+    struct MotionDevice {
+        Common::Vec3f accel;
+        Common::Vec3f gyro;
+        Common::Vec3f rotation;
+        std::array<Common::Vec3f, 3> orientation;
+        Common::Quaternion<f32> quaternion;
+    };
+
+    using MotionArray =
+        std::array<std::unique_ptr<Input::MotionDevice>, Settings::NativeMotion::NUM_MOTIONS_HID>;
+    u8* transfer_memory;
+    MotionArray motions;
+    bool is_transfer_memory_set = false;
+    ConsoleSharedMemory console_six_axis{};
+    SevenSixAxisMemory seven_six_axis{};
+};
+} // namespace Service::HID
diff --git a/src/core/hle/service/hid/controllers/npad.cpp b/src/core/hle/service/hid/controllers/npad.cpp
index 113a41254..249c300f6 100644
--- a/src/core/hle/service/hid/controllers/npad.cpp
+++ b/src/core/hle/service/hid/controllers/npad.cpp
@@ -654,8 +654,8 @@ void Controller_NPad::OnMotionUpdate(const Core::Timing::CoreTiming& core_timing
                 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();
+                             motion_devices[e].rotation, motion_devices[e].orientation,
+                             motion_devices[e].quaternion) = device->GetStatus();
                     sixaxis_at_rest = sixaxis_at_rest && motion_devices[e].gyro.Length2() < 0.0001f;
                 }
             }
diff --git a/src/core/hle/service/hid/controllers/npad.h b/src/core/hle/service/hid/controllers/npad.h
index c3b07bd41..085f42c48 100644
--- a/src/core/hle/service/hid/controllers/npad.h
+++ b/src/core/hle/service/hid/controllers/npad.h
@@ -8,6 +8,7 @@
 #include <atomic>
 #include "common/bit_field.h"
 #include "common/common_types.h"
+#include "common/quaternion.h"
 #include "common/settings.h"
 #include "core/frontend/input.h"
 #include "core/hle/kernel/object.h"
@@ -467,6 +468,7 @@ private:
         Common::Vec3f gyro;
         Common::Vec3f rotation;
         std::array<Common::Vec3f, 3> orientation;
+        Common::Quaternion<f32> quaternion;
     };
 
     struct NfcXcdHandle {
diff --git a/src/core/hle/service/hid/hid.cpp b/src/core/hle/service/hid/hid.cpp
index 2aa1942cb..9c4bf6d16 100644
--- a/src/core/hle/service/hid/hid.cpp
+++ b/src/core/hle/service/hid/hid.cpp
@@ -26,6 +26,7 @@
 #include "core/hle/service/hid/xcd.h"
 #include "core/hle/service/service.h"
 
+#include "core/hle/service/hid/controllers/console_sixaxis.h"
 #include "core/hle/service/hid/controllers/controller_base.h"
 #include "core/hle/service/hid/controllers/debug_pad.h"
 #include "core/hle/service/hid/controllers/gesture.h"
@@ -67,7 +68,7 @@ IAppletResource::IAppletResource(Core::System& system_)
     MakeController<Controller_Stubbed>(HidController::UniquePad);
     MakeController<Controller_NPad>(HidController::NPad);
     MakeController<Controller_Gesture>(HidController::Gesture);
-    MakeController<Controller_Stubbed>(HidController::ConsoleSixAxisSensor);
+    MakeController<Controller_ConsoleSixAxis>(HidController::ConsoleSixAxisSensor);
 
     // Homebrew doesn't try to activate some controllers, so we activate them by default
     GetController<Controller_NPad>(HidController::NPad).ActivateController();
@@ -78,8 +79,6 @@ IAppletResource::IAppletResource(Core::System& system_)
     GetController<Controller_Stubbed>(HidController::CaptureButton).SetCommonHeaderOffset(0x5000);
     GetController<Controller_Stubbed>(HidController::InputDetector).SetCommonHeaderOffset(0x5200);
     GetController<Controller_Stubbed>(HidController::UniquePad).SetCommonHeaderOffset(0x5A00);
-    GetController<Controller_Stubbed>(HidController::ConsoleSixAxisSensor)
-        .SetCommonHeaderOffset(0x3C200);
 
     // Register update callbacks
     pad_update_event = Core::Timing::CreateEvent(
@@ -1404,8 +1403,9 @@ void Hid::ActivateConsoleSixAxisSensor(Kernel::HLERequestContext& ctx) {
     IPC::RequestParser rp{ctx};
     const auto applet_resource_user_id{rp.Pop<u64>()};
 
-    LOG_WARNING(Service_HID, "(STUBBED) called, applet_resource_user_id={}",
-                applet_resource_user_id);
+    applet_resource->ActivateController(HidController::ConsoleSixAxisSensor);
+
+    LOG_WARNING(Service_HID, "called, applet_resource_user_id={}", applet_resource_user_id);
 
     IPC::ResponseBuilder rb{ctx, 2};
     rb.Push(RESULT_SUCCESS);
@@ -1455,8 +1455,9 @@ void Hid::ActivateSevenSixAxisSensor(Kernel::HLERequestContext& ctx) {
     IPC::RequestParser rp{ctx};
     const auto applet_resource_user_id{rp.Pop<u64>()};
 
-    LOG_WARNING(Service_HID, "(STUBBED) called, applet_resource_user_id={}",
-                applet_resource_user_id);
+    applet_resource->ActivateController(HidController::ConsoleSixAxisSensor);
+
+    LOG_WARNING(Service_HID, "called, applet_resource_user_id={}", applet_resource_user_id);
 
     IPC::ResponseBuilder rb{ctx, 2};
     rb.Push(RESULT_SUCCESS);
@@ -1518,8 +1519,15 @@ void Hid::InitializeSevenSixAxisSensor(Kernel::HLERequestContext& ctx) {
     ASSERT_MSG(t_mem_1->GetSize() == 0x1000, "t_mem_1 has incorrect size");
     ASSERT_MSG(t_mem_2->GetSize() == 0x7F000, "t_mem_2 has incorrect size");
 
+    // Activate console six axis controller
+    applet_resource->GetController<Controller_ConsoleSixAxis>(HidController::ConsoleSixAxisSensor)
+        .ActivateController();
+
+    applet_resource->GetController<Controller_ConsoleSixAxis>(HidController::ConsoleSixAxisSensor)
+        .SetTransferMemoryPointer(t_mem_1->GetPointer());
+
     LOG_WARNING(Service_HID,
-                "(STUBBED) called, t_mem_1_handle=0x{:08X}, t_mem_2_handle=0x{:08X}, "
+                "called, t_mem_1_handle=0x{:08X}, t_mem_2_handle=0x{:08X}, "
                 "applet_resource_user_id={}",
                 t_mem_1_handle, t_mem_2_handle, applet_resource_user_id);
 
@@ -1542,8 +1550,10 @@ void Hid::ResetSevenSixAxisSensorTimestamp(Kernel::HLERequestContext& ctx) {
     IPC::RequestParser rp{ctx};
     const auto applet_resource_user_id{rp.Pop<u64>()};
 
-    LOG_WARNING(Service_HID, "(STUBBED) called, applet_resource_user_id={}",
-                applet_resource_user_id);
+    applet_resource->GetController<Controller_ConsoleSixAxis>(HidController::ConsoleSixAxisSensor)
+        .ResetTimestamp();
+
+    LOG_WARNING(Service_HID, "called, applet_resource_user_id={}", applet_resource_user_id);
 
     IPC::ResponseBuilder rb{ctx, 2};
     rb.Push(RESULT_SUCCESS);