input_common: Use calibration from joycon

5 files changed, 231 insertions, 0 deletions

diff --git a/src/input_common/CMakeLists.txt b/src/input_common/CMakeLists.txt
index a60cecaf4..d4307351c 100644
--- a/src/input_common/CMakeLists.txt
+++ b/src/input_common/CMakeLists.txt
@@ -57,6 +57,8 @@ if (ENABLE_SDL2)
         drivers/sdl_driver.h
         helpers/joycon_driver.cpp
         helpers/joycon_driver.h
+        helpers/joycon_protocol/calibration.cpp
+        helpers/joycon_protocol/calibration.h
         helpers/joycon_protocol/common_protocol.cpp
         helpers/joycon_protocol/common_protocol.h
         helpers/joycon_protocol/generic_functions.cpp
diff --git a/src/input_common/helpers/joycon_driver.cpp b/src/input_common/helpers/joycon_driver.cpp
index 0de55578b..ac11be1c1 100644
--- a/src/input_common/helpers/joycon_driver.cpp
+++ b/src/input_common/helpers/joycon_driver.cpp
@@ -64,6 +64,7 @@ DriverResult JoyconDriver::InitializeDevice() {
     accelerometer_performance = Joycon::AccelerometerPerformance::HZ100;
 
     // Initialize HW Protocols
+    calibration_protocol = std::make_unique<CalibrationProtocol>(hidapi_handle);
     generic_protocol = std::make_unique<GenericProtocol>(hidapi_handle);
 
     // Get fixed joycon info
@@ -79,6 +80,9 @@ DriverResult JoyconDriver::InitializeDevice() {
     supported_features = GetSupportedFeatures();
 
     // Get Calibration data
+    calibration_protocol->GetLeftJoyStickCalibration(left_stick_calibration);
+    calibration_protocol->GetRightJoyStickCalibration(right_stick_calibration);
+    calibration_protocol->GetImuCalibration(motion_calibration);
 
     // Set led status
     generic_protocol->SetLedBlinkPattern(static_cast<u8>(1 + port));
diff --git a/src/input_common/helpers/joycon_driver.h b/src/input_common/helpers/joycon_driver.h
index deb50ec77..275c97b91 100644
--- a/src/input_common/helpers/joycon_driver.h
+++ b/src/input_common/helpers/joycon_driver.h
@@ -8,6 +8,7 @@
 #include <span>
 #include <thread>
 
+#include "input_common/helpers/joycon_protocol/calibration.h"
 #include "input_common/helpers/joycon_protocol/generic_functions.h"
 #include "input_common/helpers/joycon_protocol/joycon_types.h"
 
@@ -95,6 +96,7 @@ private:
     void ReadNfcIRMode(std::span<u8> buffer);
 
     // Protocol Features
+    std::unique_ptr<CalibrationProtocol> calibration_protocol = nullptr;
     std::unique_ptr<GenericProtocol> generic_protocol = nullptr;
 
     // Connection status
diff --git a/src/input_common/helpers/joycon_protocol/calibration.cpp b/src/input_common/helpers/joycon_protocol/calibration.cpp
new file mode 100644
index 000000000..5c29af545
--- /dev/null
+++ b/src/input_common/helpers/joycon_protocol/calibration.cpp
@@ -0,0 +1,169 @@
+// SPDX-FileCopyrightText: Copyright 2022 yuzu Emulator Project
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#include <cstring>
+
+#include "input_common/helpers/joycon_protocol/calibration.h"
+#include "input_common/helpers/joycon_protocol/joycon_types.h"
+
+namespace InputCommon::Joycon {
+
+CalibrationProtocol::CalibrationProtocol(std::shared_ptr<JoyconHandle> handle)
+    : JoyconCommonProtocol(handle) {}
+
+DriverResult CalibrationProtocol::GetLeftJoyStickCalibration(JoyStickCalibration& calibration) {
+    std::vector<u8> buffer;
+    DriverResult result{DriverResult::Success};
+    calibration = {};
+    SetBlocking();
+
+    result = ReadSPI(CalAddr::USER_LEFT_MAGIC, sizeof(u16), buffer);
+
+    if (result == DriverResult::Success) {
+        const bool has_user_calibration = buffer[0] == 0xB2 && buffer[1] == 0xA1;
+        if (has_user_calibration) {
+            result = ReadSPI(CalAddr::USER_LEFT_DATA, 9, buffer);
+        } else {
+            result = ReadSPI(CalAddr::FACT_LEFT_DATA, 9, buffer);
+        }
+    }
+
+    if (result == DriverResult::Success) {
+        calibration.x.max = static_cast<u16>(((buffer[1] & 0x0F) << 8) | buffer[0]);
+        calibration.y.max = static_cast<u16>((buffer[2] << 4) | (buffer[1] >> 4));
+        calibration.x.center = static_cast<u16>(((buffer[4] & 0x0F) << 8) | buffer[3]);
+        calibration.y.center = static_cast<u16>((buffer[5] << 4) | (buffer[4] >> 4));
+        calibration.x.min = static_cast<u16>(((buffer[7] & 0x0F) << 8) | buffer[6]);
+        calibration.y.min = static_cast<u16>((buffer[8] << 4) | (buffer[7] >> 4));
+    }
+
+    // Nintendo fix for drifting stick
+    // result = ReadSPI(0x60, 0x86 ,buffer, 16);
+    // calibration.deadzone = (u16)((buffer[4] << 8) & 0xF00 | buffer[3]);
+
+    // Set a valid default calibration if data is missing
+    ValidateCalibration(calibration);
+
+    SetNonBlocking();
+    return result;
+}
+
+DriverResult CalibrationProtocol::GetRightJoyStickCalibration(JoyStickCalibration& calibration) {
+    std::vector<u8> buffer;
+    DriverResult result{DriverResult::Success};
+    calibration = {};
+    SetBlocking();
+
+    result = ReadSPI(CalAddr::USER_RIGHT_MAGIC, sizeof(u16), buffer);
+
+    if (result == DriverResult::Success) {
+        const bool has_user_calibration = buffer[0] == 0xB2 && buffer[1] == 0xA1;
+        if (has_user_calibration) {
+            result = ReadSPI(CalAddr::USER_RIGHT_DATA, 9, buffer);
+        } else {
+            result = ReadSPI(CalAddr::FACT_RIGHT_DATA, 9, buffer);
+        }
+    }
+
+    if (result == DriverResult::Success) {
+        calibration.x.center = static_cast<u16>(((buffer[1] & 0x0F) << 8) | buffer[0]);
+        calibration.y.center = static_cast<u16>((buffer[2] << 4) | (buffer[1] >> 4));
+        calibration.x.min = static_cast<u16>(((buffer[4] & 0x0F) << 8) | buffer[3]);
+        calibration.y.min = static_cast<u16>((buffer[5] << 4) | (buffer[4] >> 4));
+        calibration.x.max = static_cast<u16>(((buffer[7] & 0x0F) << 8) | buffer[6]);
+        calibration.y.max = static_cast<u16>((buffer[8] << 4) | (buffer[7] >> 4));
+    }
+
+    // Nintendo fix for drifting stick
+    // buffer = ReadSPI(0x60, 0x98 , 16);
+    // joystick.deadzone = (u16)((buffer[4] << 8) & 0xF00 | buffer[3]);
+
+    // Set a valid default calibration if data is missing
+    ValidateCalibration(calibration);
+
+    SetNonBlocking();
+    return result;
+}
+
+DriverResult CalibrationProtocol::GetImuCalibration(MotionCalibration& calibration) {
+    std::vector<u8> buffer;
+    DriverResult result{DriverResult::Success};
+    calibration = {};
+    SetBlocking();
+
+    result = ReadSPI(CalAddr::USER_IMU_MAGIC, sizeof(u16), buffer);
+
+    if (result == DriverResult::Success) {
+        const bool has_user_calibration = buffer[0] == 0xB2 && buffer[1] == 0xA1;
+        if (has_user_calibration) {
+            result = ReadSPI(CalAddr::USER_IMU_DATA, sizeof(IMUCalibration), buffer);
+        } else {
+            result = ReadSPI(CalAddr::FACT_IMU_DATA, sizeof(IMUCalibration), buffer);
+        }
+    }
+
+    if (result == DriverResult::Success) {
+        IMUCalibration device_calibration{};
+        memcpy(&device_calibration, buffer.data(), sizeof(IMUCalibration));
+        calibration.accelerometer[0].offset = device_calibration.accelerometer_offset[0];
+        calibration.accelerometer[1].offset = device_calibration.accelerometer_offset[1];
+        calibration.accelerometer[2].offset = device_calibration.accelerometer_offset[2];
+
+        calibration.accelerometer[0].scale = device_calibration.accelerometer_scale[0];
+        calibration.accelerometer[1].scale = device_calibration.accelerometer_scale[1];
+        calibration.accelerometer[2].scale = device_calibration.accelerometer_scale[2];
+
+        calibration.gyro[0].offset = device_calibration.gyroscope_offset[0];
+        calibration.gyro[1].offset = device_calibration.gyroscope_offset[1];
+        calibration.gyro[2].offset = device_calibration.gyroscope_offset[2];
+
+        calibration.gyro[0].scale = device_calibration.gyroscope_scale[0];
+        calibration.gyro[1].scale = device_calibration.gyroscope_scale[1];
+        calibration.gyro[2].scale = device_calibration.gyroscope_scale[2];
+    }
+
+    ValidateCalibration(calibration);
+
+    SetNonBlocking();
+    return result;
+}
+
+void CalibrationProtocol::ValidateCalibration(JoyStickCalibration& calibration) {
+    constexpr u16 DefaultStickCenter{2048};
+    constexpr u16 DefaultStickRange{1740};
+
+    if (calibration.x.center == 0xFFF || calibration.x.center == 0) {
+        calibration.x.center = DefaultStickCenter;
+    }
+    if (calibration.x.max == 0xFFF || calibration.x.max == 0) {
+        calibration.x.max = DefaultStickRange;
+    }
+    if (calibration.x.min == 0xFFF || calibration.x.min == 0) {
+        calibration.x.min = DefaultStickRange;
+    }
+
+    if (calibration.y.center == 0xFFF || calibration.y.center == 0) {
+        calibration.y.center = DefaultStickCenter;
+    }
+    if (calibration.y.max == 0xFFF || calibration.y.max == 0) {
+        calibration.y.max = DefaultStickRange;
+    }
+    if (calibration.y.min == 0xFFF || calibration.y.min == 0) {
+        calibration.y.min = DefaultStickRange;
+    }
+}
+
+void CalibrationProtocol::ValidateCalibration(MotionCalibration& calibration) {
+    for (auto& sensor : calibration.accelerometer) {
+        if (sensor.scale == 0) {
+            sensor.scale = 0x4000;
+        }
+    }
+    for (auto& sensor : calibration.gyro) {
+        if (sensor.scale == 0) {
+            sensor.scale = 0x3be7;
+        }
+    }
+}
+
+} // namespace InputCommon::Joycon
diff --git a/src/input_common/helpers/joycon_protocol/calibration.h b/src/input_common/helpers/joycon_protocol/calibration.h
new file mode 100644
index 000000000..38214eed4
--- /dev/null
+++ b/src/input_common/helpers/joycon_protocol/calibration.h
@@ -0,0 +1,54 @@
+// SPDX-FileCopyrightText: Copyright 2022 yuzu Emulator Project
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+// Based on dkms-hid-nintendo implementation, CTCaer joycon toolkit and dekuNukem reverse
+// engineering https://github.com/nicman23/dkms-hid-nintendo/blob/master/src/hid-nintendo.c
+// https://github.com/CTCaer/jc_toolkit
+// https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering
+
+#pragma once
+
+#include <vector>
+
+#include "input_common/helpers/joycon_protocol/common_protocol.h"
+
+namespace InputCommon::Joycon {
+enum class DriverResult;
+struct JoyStickCalibration;
+struct IMUCalibration;
+struct JoyconHandle;
+} // namespace InputCommon::Joycon
+
+namespace InputCommon::Joycon {
+
+/// Driver functions related to retrieving calibration data from the device
+class CalibrationProtocol final : private JoyconCommonProtocol {
+public:
+    CalibrationProtocol(std::shared_ptr<JoyconHandle> handle);
+
+    /**
+     * Sends a request to obtain the left stick calibration from memory
+     * @param is_factory_calibration if true factory values will be returned
+     * @returns JoyStickCalibration of the left joystick
+     */
+    DriverResult GetLeftJoyStickCalibration(JoyStickCalibration& calibration);
+
+    /**
+     * Sends a request to obtain the right stick calibration from memory
+     * @param is_factory_calibration if true factory values will be returned
+     * @returns JoyStickCalibration of the right joystick
+     */
+    DriverResult GetRightJoyStickCalibration(JoyStickCalibration& calibration);
+
+    /**
+     * Sends a request to obtain the motion calibration from memory
+     * @returns ImuCalibration of the motion sensor
+     */
+    DriverResult GetImuCalibration(MotionCalibration& calibration);
+
+private:
+    void ValidateCalibration(JoyStickCalibration& calibration);
+    void ValidateCalibration(MotionCalibration& calibration);
+};
+
+} // namespace InputCommon::Joycon