Merge pull request #1549 from wwylele/acc_gyro

hid: implement accelerometer and gyroscope back-end

5 files changed, 235 insertions, 23 deletions

diff --git a/src/common/emu_window.h b/src/common/emu_window.h
index a0ae4c9fa..b6b7bfd26 100644
--- a/src/common/emu_window.h
+++ b/src/common/emu_window.h
@@ -121,6 +121,54 @@ public:
     }
 
     /**
+     * Gets the current accelerometer state (acceleration along each three axis).
+     * Axis explained:
+     *   +x is the same direction as LEFT on D-pad.
+     *   +y is normal to the touch screen, pointing outward.
+     *   +z is the same direction as UP on D-pad.
+     * Units:
+     *   1 unit of return value = 1/512 g (measured by hw test),
+     *   where g is the gravitational acceleration (9.8 m/sec2).
+     * @note This should be called by the core emu thread to get a state set by the window thread.
+     * @todo Implement accelerometer input in front-end.
+     * @return std::tuple of (x, y, z)
+     */
+    std::tuple<s16, s16, s16> GetAccelerometerState() const {
+        // stubbed
+        return std::make_tuple(0, -512, 0);
+    }
+
+    /**
+     * Gets the current gyroscope state (angular rates about each three axis).
+     * Axis explained:
+     *   +x is the same direction as LEFT on D-pad.
+     *   +y is normal to the touch screen, pointing outward.
+     *   +z is the same direction as UP on D-pad.
+     * Orientation is determined by right-hand rule.
+     * Units:
+     *   1 unit of return value = (1/coef) deg/sec,
+     *   where coef is the return value of GetGyroscopeRawToDpsCoefficient().
+     * @note This should be called by the core emu thread to get a state set by the window thread.
+     * @todo Implement gyroscope input in front-end.
+     * @return std::tuple of (x, y, z)
+     */
+    std::tuple<s16, s16, s16> GetGyroscopeState() const {
+        // stubbed
+        return std::make_tuple(0, 0, 0);
+    }
+
+    /**
+     * Gets the coefficient for units conversion of gyroscope state.
+     * The conversion formula is r = coefficient * v,
+     * where v is angular rate in deg/sec,
+     * and r is the gyroscope state.
+     * @return float-type coefficient
+     */
+    f32 GetGyroscopeRawToDpsCoefficient() const {
+        return 14.375f; // taken from hw test, and gyroscope's document
+    }
+
+    /**
      * Returns currently active configuration.
      * @note Accesses to the returned object need not be consistent because it may be modified in another thread
      */
diff --git a/src/core/hle/service/hid/hid.cpp b/src/core/hle/service/hid/hid.cpp
index cb4fd38e2..b27ab6d9b 100644
--- a/src/core/hle/service/hid/hid.cpp
+++ b/src/core/hle/service/hid/hid.cpp
@@ -33,6 +33,11 @@ static Kernel::SharedPtr<Kernel::Event> event_debug_pad;
 
 static u32 next_pad_index;
 static u32 next_touch_index;
+static u32 next_accelerometer_index;
+static u32 next_gyroscope_index;
+
+static int enable_accelerometer_count = 0; // positive means enabled
+static int enable_gyroscope_count = 0; // positive means enabled
 
 const std::array<Service::HID::PadState, Settings::NativeInput::NUM_INPUTS> pad_mapping = {{
     Service::HID::PAD_A, Service::HID::PAD_B, Service::HID::PAD_X, Service::HID::PAD_Y,
@@ -120,6 +125,58 @@ void Update() {
     // Signal both handles when there's an update to Pad or touch
     event_pad_or_touch_1->Signal();
     event_pad_or_touch_2->Signal();
+
+    // Update accelerometer
+    if (enable_accelerometer_count > 0) {
+        mem->accelerometer.index = next_accelerometer_index;
+        next_accelerometer_index = (next_accelerometer_index + 1) % mem->accelerometer.entries.size();
+
+        AccelerometerDataEntry& accelerometer_entry = mem->accelerometer.entries[mem->accelerometer.index];
+        std::tie(accelerometer_entry.x, accelerometer_entry.y, accelerometer_entry.z)
+            = VideoCore::g_emu_window->GetAccelerometerState();
+
+        // Make up "raw" entry
+        // TODO(wwylele):
+        // From hardware testing, the raw_entry values are approximately,
+        // but not exactly, as twice as corresponding entries (or with a minus sign).
+        // It may caused by system calibration to the accelerometer.
+        // Figure out how it works, or, if no game reads raw_entry,
+        // the following three lines can be removed and leave raw_entry unimplemented.
+        mem->accelerometer.raw_entry.x = -2 * accelerometer_entry.x;
+        mem->accelerometer.raw_entry.z = 2 * accelerometer_entry.y;
+        mem->accelerometer.raw_entry.y = -2 * accelerometer_entry.z;
+
+        // If we just updated index 0, provide a new timestamp
+        if (mem->accelerometer.index == 0) {
+            mem->accelerometer.index_reset_ticks_previous = mem->accelerometer.index_reset_ticks;
+            mem->accelerometer.index_reset_ticks = (s64)CoreTiming::GetTicks();
+        }
+
+        event_accelerometer->Signal();
+    }
+
+    // Update gyroscope
+    if (enable_gyroscope_count > 0) {
+        mem->gyroscope.index = next_gyroscope_index;
+        next_gyroscope_index = (next_gyroscope_index + 1) % mem->gyroscope.entries.size();
+
+        GyroscopeDataEntry& gyroscope_entry = mem->gyroscope.entries[mem->gyroscope.index];
+        std::tie(gyroscope_entry.x, gyroscope_entry.y, gyroscope_entry.z)
+            = VideoCore::g_emu_window->GetGyroscopeState();
+
+        // Make up "raw" entry
+        mem->gyroscope.raw_entry.x = gyroscope_entry.x;
+        mem->gyroscope.raw_entry.z = -gyroscope_entry.y;
+        mem->gyroscope.raw_entry.y = gyroscope_entry.z;
+
+        // If we just updated index 0, provide a new timestamp
+        if (mem->gyroscope.index == 0) {
+            mem->gyroscope.index_reset_ticks_previous = mem->gyroscope.index_reset_ticks;
+            mem->gyroscope.index_reset_ticks = (s64)CoreTiming::GetTicks();
+        }
+
+        event_gyroscope->Signal();
+    }
 }
 
 void GetIPCHandles(Service::Interface* self) {
@@ -139,40 +196,69 @@ void GetIPCHandles(Service::Interface* self) {
 void EnableAccelerometer(Service::Interface* self) {
     u32* cmd_buff = Kernel::GetCommandBuffer();
 
+    ++enable_accelerometer_count;
     event_accelerometer->Signal();
 
     cmd_buff[1] = RESULT_SUCCESS.raw;
 
-    LOG_WARNING(Service_HID, "(STUBBED) called");
+    LOG_DEBUG(Service_HID, "called");
 }
 
 void DisableAccelerometer(Service::Interface* self) {
     u32* cmd_buff = Kernel::GetCommandBuffer();
 
+    --enable_accelerometer_count;
     event_accelerometer->Signal();
 
     cmd_buff[1] = RESULT_SUCCESS.raw;
 
-    LOG_WARNING(Service_HID, "(STUBBED) called");
+    LOG_DEBUG(Service_HID, "called");
 }
 
 void EnableGyroscopeLow(Service::Interface* self) {
     u32* cmd_buff = Kernel::GetCommandBuffer();
 
+    ++enable_gyroscope_count;
     event_gyroscope->Signal();
 
     cmd_buff[1] = RESULT_SUCCESS.raw;
 
-    LOG_WARNING(Service_HID, "(STUBBED) called");
+    LOG_DEBUG(Service_HID, "called");
 }
 
 void DisableGyroscopeLow(Service::Interface* self) {
     u32* cmd_buff = Kernel::GetCommandBuffer();
 
+    --enable_gyroscope_count;
     event_gyroscope->Signal();
 
     cmd_buff[1] = RESULT_SUCCESS.raw;
 
+    LOG_DEBUG(Service_HID, "called");
+}
+
+void GetGyroscopeLowRawToDpsCoefficient(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    f32 coef = VideoCore::g_emu_window->GetGyroscopeRawToDpsCoefficient();
+    memcpy(&cmd_buff[2], &coef, 4);
+}
+
+void GetGyroscopeLowCalibrateParam(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+
+    const s16 param_unit = 6700; // an approximate value taken from hw
+    GyroscopeCalibrateParam param = {
+        { 0, param_unit, -param_unit },
+        { 0, param_unit, -param_unit },
+        { 0, param_unit, -param_unit },
+    };
+    memcpy(&cmd_buff[2], &param, sizeof(param));
+
     LOG_WARNING(Service_HID, "(STUBBED) called");
 }
 
diff --git a/src/core/hle/service/hid/hid.h b/src/core/hle/service/hid/hid.h
index 517f4f2ae..170d19ea8 100644
--- a/src/core/hle/service/hid/hid.h
+++ b/src/core/hle/service/hid/hid.h
@@ -78,6 +78,24 @@ struct TouchDataEntry {
 };
 
 /**
+ * Structure of a single entry of accelerometer state history within HID shared memory
+ */
+struct AccelerometerDataEntry {
+    s16 x;
+    s16 y;
+    s16 z;
+};
+
+/**
+ * Structure of a single entry of gyroscope state history within HID shared memory
+ */
+struct GyroscopeDataEntry {
+    s16 x;
+    s16 y;
+    s16 z;
+};
+
+/**
  * Structure of data stored in HID shared memory
  */
 struct SharedMem {
@@ -112,6 +130,46 @@ struct SharedMem {
 
         std::array<TouchDataEntry, 8> entries; ///< Last 8 touch entries, in pixel coordinates
     } touch;
+
+    /// Accelerometer data
+    struct {
+        s64 index_reset_ticks; ///< CPU tick count for when HID module updated entry index 0
+        s64 index_reset_ticks_previous; ///< Previous `index_reset_ticks`
+        u32 index; ///< Index of the last updated accelerometer entry
+
+        INSERT_PADDING_WORDS(0x1);
+
+        AccelerometerDataEntry raw_entry;
+        INSERT_PADDING_BYTES(2);
+
+        std::array<AccelerometerDataEntry, 8> entries;
+    } accelerometer;
+
+    /// Gyroscope data
+    struct {
+        s64 index_reset_ticks; ///< CPU tick count for when HID module updated entry index 0
+        s64 index_reset_ticks_previous; ///< Previous `index_reset_ticks`
+        u32 index; ///< Index of the last updated accelerometer entry
+
+        INSERT_PADDING_WORDS(0x1);
+
+        GyroscopeDataEntry raw_entry;
+        INSERT_PADDING_BYTES(2);
+
+        std::array<GyroscopeDataEntry, 32> entries;
+    } gyroscope;
+};
+
+/**
+ * Structure of calibrate params that GetGyroscopeLowCalibrateParam returns
+ */
+struct GyroscopeCalibrateParam {
+    struct {
+        // TODO (wwylele): figure out the exact meaning of these params
+        s16 zero_point;
+        s16 positive_unit_point;
+        s16 negative_unit_point;
+    } x, y, z;
 };
 
 // TODO: MSVC does not support using offsetof() on non-static data members even though this
@@ -222,6 +280,26 @@ void DisableGyroscopeLow(Interface* self);
  */
 void GetSoundVolume(Interface* self);
 
+/**
+ * HID::GetGyroscopeLowRawToDpsCoefficient service function
+ *  Inputs:
+ *      None
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ *      2 : float output value
+ */
+void GetGyroscopeLowRawToDpsCoefficient(Service::Interface* self);
+
+/**
+ * HID::GetGyroscopeLowCalibrateParam service function
+ *  Inputs:
+ *      None
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ *      2~6 (18 bytes) : struct GyroscopeCalibrateParam
+ */
+void GetGyroscopeLowCalibrateParam(Service::Interface* self);
+
 /// Checks for user input updates
 void Update();
 
diff --git a/src/core/hle/service/hid/hid_spvr.cpp b/src/core/hle/service/hid/hid_spvr.cpp
index c50f597eb..046e65b11 100644
--- a/src/core/hle/service/hid/hid_spvr.cpp
+++ b/src/core/hle/service/hid/hid_spvr.cpp
@@ -9,16 +9,16 @@ namespace Service {
 namespace HID {
 
 const Interface::FunctionInfo FunctionTable[] = {
-    {0x000A0000, GetIPCHandles,              "GetIPCHandles"},
-    {0x000B0000, nullptr,                    "StartAnalogStickCalibration"},
-    {0x000E0000, nullptr,                    "GetAnalogStickCalibrateParam"},
-    {0x00110000, EnableAccelerometer,        "EnableAccelerometer"},
-    {0x00120000, DisableAccelerometer,       "DisableAccelerometer"},
-    {0x00130000, EnableGyroscopeLow,         "EnableGyroscopeLow"},
-    {0x00140000, DisableGyroscopeLow,        "DisableGyroscopeLow"},
-    {0x00150000, nullptr,                    "GetGyroscopeLowRawToDpsCoefficient"},
-    {0x00160000, nullptr,                    "GetGyroscopeLowCalibrateParam"},
-    {0x00170000, GetSoundVolume,             "GetSoundVolume"},
+    {0x000A0000, GetIPCHandles,                      "GetIPCHandles"},
+    {0x000B0000, nullptr,                            "StartAnalogStickCalibration"},
+    {0x000E0000, nullptr,                            "GetAnalogStickCalibrateParam"},
+    {0x00110000, EnableAccelerometer,                "EnableAccelerometer"},
+    {0x00120000, DisableAccelerometer,               "DisableAccelerometer"},
+    {0x00130000, EnableGyroscopeLow,                 "EnableGyroscopeLow"},
+    {0x00140000, DisableGyroscopeLow,                "DisableGyroscopeLow"},
+    {0x00150000, GetGyroscopeLowRawToDpsCoefficient, "GetGyroscopeLowRawToDpsCoefficient"},
+    {0x00160000, GetGyroscopeLowCalibrateParam,      "GetGyroscopeLowCalibrateParam"},
+    {0x00170000, GetSoundVolume,                     "GetSoundVolume"},
 };
 
 HID_SPVR_Interface::HID_SPVR_Interface() {
diff --git a/src/core/hle/service/hid/hid_user.cpp b/src/core/hle/service/hid/hid_user.cpp
index bbdde2abb..bb157b83d 100644
--- a/src/core/hle/service/hid/hid_user.cpp
+++ b/src/core/hle/service/hid/hid_user.cpp
@@ -9,16 +9,16 @@ namespace Service {
 namespace HID {
 
 const Interface::FunctionInfo FunctionTable[] = {
-    {0x000A0000, GetIPCHandles,             "GetIPCHandles"},
-    {0x000B0000, nullptr,                   "StartAnalogStickCalibration"},
-    {0x000E0000, nullptr,                   "GetAnalogStickCalibrateParam"},
-    {0x00110000, EnableAccelerometer,       "EnableAccelerometer"},
-    {0x00120000, DisableAccelerometer,      "DisableAccelerometer"},
-    {0x00130000, EnableGyroscopeLow,        "EnableGyroscopeLow"},
-    {0x00140000, DisableGyroscopeLow,       "DisableGyroscopeLow"},
-    {0x00150000, nullptr,                   "GetGyroscopeLowRawToDpsCoefficient"},
-    {0x00160000, nullptr,                   "GetGyroscopeLowCalibrateParam"},
-    {0x00170000, GetSoundVolume,            "GetSoundVolume"},
+    {0x000A0000, GetIPCHandles,                      "GetIPCHandles"},
+    {0x000B0000, nullptr,                            "StartAnalogStickCalibration"},
+    {0x000E0000, nullptr,                            "GetAnalogStickCalibrateParam"},
+    {0x00110000, EnableAccelerometer,                "EnableAccelerometer"},
+    {0x00120000, DisableAccelerometer,               "DisableAccelerometer"},
+    {0x00130000, EnableGyroscopeLow,                 "EnableGyroscopeLow"},
+    {0x00140000, DisableGyroscopeLow,                "DisableGyroscopeLow"},
+    {0x00150000, GetGyroscopeLowRawToDpsCoefficient, "GetGyroscopeLowRawToDpsCoefficient"},
+    {0x00160000, GetGyroscopeLowCalibrateParam,      "GetGyroscopeLowCalibrateParam"},
+    {0x00170000, GetSoundVolume,                     "GetSoundVolume"},
 };
 
 HID_U_Interface::HID_U_Interface() {