6 files changed, 202 insertions, 107 deletions

diff --git a/src/input_common/helpers/joycon_protocol/calibration.cpp b/src/input_common/helpers/joycon_protocol/calibration.cpp
index f6e7e97d5..d8f040f75 100644
--- a/src/input_common/helpers/joycon_protocol/calibration.cpp
+++ b/src/input_common/helpers/joycon_protocol/calibration.cpp
@@ -13,33 +13,33 @@ CalibrationProtocol::CalibrationProtocol(std::shared_ptr<JoyconHandle> handle)
 
 DriverResult CalibrationProtocol::GetLeftJoyStickCalibration(JoyStickCalibration& calibration) {
     ScopedSetBlocking sb(this);
-    std::vector<u8> buffer;
     DriverResult result{DriverResult::Success};
+    JoystickLeftSpiCalibration spi_calibration{};
+    bool has_user_calibration = false;
     calibration = {};
 
-    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);
-        }
+        result = HasUserCalibration(SpiAddress::USER_LEFT_MAGIC, has_user_calibration);
     }
 
-    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));
+    // Read User defined calibration
+    if (result == DriverResult::Success && has_user_calibration) {
+        result = ReadSPI(SpiAddress::USER_LEFT_DATA, spi_calibration);
     }
 
-    // Nintendo fix for drifting stick
-    // result = ReadSPI(0x60, 0x86 ,buffer, 16);
-    // calibration.deadzone = (u16)((buffer[4] << 8) & 0xF00 | buffer[3]);
+    // Read Factory calibration
+    if (result == DriverResult::Success && !has_user_calibration) {
+        result = ReadSPI(SpiAddress::FACT_LEFT_DATA, spi_calibration);
+    }
+
+    if (result == DriverResult::Success) {
+        calibration.x.center = GetXAxisCalibrationValue(spi_calibration.center);
+        calibration.y.center = GetYAxisCalibrationValue(spi_calibration.center);
+        calibration.x.min = GetXAxisCalibrationValue(spi_calibration.min);
+        calibration.y.min = GetYAxisCalibrationValue(spi_calibration.min);
+        calibration.x.max = GetXAxisCalibrationValue(spi_calibration.max);
+        calibration.y.max = GetYAxisCalibrationValue(spi_calibration.max);
+    }
 
     // Set a valid default calibration if data is missing
     ValidateCalibration(calibration);
@@ -49,33 +49,33 @@ DriverResult CalibrationProtocol::GetLeftJoyStickCalibration(JoyStickCalibration
 
 DriverResult CalibrationProtocol::GetRightJoyStickCalibration(JoyStickCalibration& calibration) {
     ScopedSetBlocking sb(this);
-    std::vector<u8> buffer;
     DriverResult result{DriverResult::Success};
+    JoystickRightSpiCalibration spi_calibration{};
+    bool has_user_calibration = false;
     calibration = {};
 
-    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);
-        }
+        result = HasUserCalibration(SpiAddress::USER_RIGHT_MAGIC, has_user_calibration);
     }
 
-    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));
+    // Read User defined calibration
+    if (result == DriverResult::Success && has_user_calibration) {
+        result = ReadSPI(SpiAddress::USER_RIGHT_DATA, spi_calibration);
+    }
+
+    // Read Factory calibration
+    if (result == DriverResult::Success && !has_user_calibration) {
+        result = ReadSPI(SpiAddress::FACT_RIGHT_DATA, spi_calibration);
     }
 
-    // Nintendo fix for drifting stick
-    // buffer = ReadSPI(0x60, 0x98 , 16);
-    // joystick.deadzone = (u16)((buffer[4] << 8) & 0xF00 | buffer[3]);
+    if (result == DriverResult::Success) {
+        calibration.x.center = GetXAxisCalibrationValue(spi_calibration.center);
+        calibration.y.center = GetYAxisCalibrationValue(spi_calibration.center);
+        calibration.x.min = GetXAxisCalibrationValue(spi_calibration.min);
+        calibration.y.min = GetYAxisCalibrationValue(spi_calibration.min);
+        calibration.x.max = GetXAxisCalibrationValue(spi_calibration.max);
+        calibration.y.max = GetYAxisCalibrationValue(spi_calibration.max);
+    }
 
     // Set a valid default calibration if data is missing
     ValidateCalibration(calibration);
@@ -85,39 +85,41 @@ DriverResult CalibrationProtocol::GetRightJoyStickCalibration(JoyStickCalibratio
 
 DriverResult CalibrationProtocol::GetImuCalibration(MotionCalibration& calibration) {
     ScopedSetBlocking sb(this);
-    std::vector<u8> buffer;
     DriverResult result{DriverResult::Success};
+    ImuSpiCalibration spi_calibration{};
+    bool has_user_calibration = false;
     calibration = {};
 
-    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);
-        }
+        result = HasUserCalibration(SpiAddress::USER_IMU_MAGIC, has_user_calibration);
+    }
+
+    // Read User defined calibration
+    if (result == DriverResult::Success && has_user_calibration) {
+        result = ReadSPI(SpiAddress::USER_IMU_DATA, spi_calibration);
+    }
+
+    // Read Factory calibration
+    if (result == DriverResult::Success && !has_user_calibration) {
+        result = ReadSPI(SpiAddress::FACT_IMU_DATA, spi_calibration);
     }
 
     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].offset = spi_calibration.accelerometer_offset[0];
+        calibration.accelerometer[1].offset = spi_calibration.accelerometer_offset[1];
+        calibration.accelerometer[2].offset = spi_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.accelerometer[0].scale = spi_calibration.accelerometer_scale[0];
+        calibration.accelerometer[1].scale = spi_calibration.accelerometer_scale[1];
+        calibration.accelerometer[2].scale = spi_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].offset = spi_calibration.gyroscope_offset[0];
+        calibration.gyro[1].offset = spi_calibration.gyroscope_offset[1];
+        calibration.gyro[2].offset = spi_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];
+        calibration.gyro[0].scale = spi_calibration.gyroscope_scale[0];
+        calibration.gyro[1].scale = spi_calibration.gyroscope_scale[1];
+        calibration.gyro[2].scale = spi_calibration.gyroscope_scale[2];
     }
 
     ValidateCalibration(calibration);
@@ -127,10 +129,12 @@ DriverResult CalibrationProtocol::GetImuCalibration(MotionCalibration& calibrati
 
 DriverResult CalibrationProtocol::GetRingCalibration(RingCalibration& calibration,
                                                      s16 current_value) {
+    constexpr s16 DefaultRingRange{800};
+
     // TODO: Get default calibration form ring itself
     if (ring_data_max == 0 && ring_data_min == 0) {
-        ring_data_max = current_value + 800;
-        ring_data_min = current_value - 800;
+        ring_data_max = current_value + DefaultRingRange;
+        ring_data_min = current_value - DefaultRingRange;
         ring_data_default = current_value;
     }
     ring_data_max = std::max(ring_data_max, current_value);
@@ -143,42 +147,72 @@ DriverResult CalibrationProtocol::GetRingCalibration(RingCalibration& calibratio
     return DriverResult::Success;
 }
 
+DriverResult CalibrationProtocol::HasUserCalibration(SpiAddress address,
+                                                     bool& has_user_calibration) {
+    MagicSpiCalibration spi_magic{};
+    const DriverResult result{ReadSPI(address, spi_magic)};
+    has_user_calibration = false;
+    if (result == DriverResult::Success) {
+        has_user_calibration = spi_magic.first == CalibrationMagic::USR_MAGIC_0 &&
+                               spi_magic.second == CalibrationMagic::USR_MAGIC_1;
+    }
+    return result;
+}
+
+u16 CalibrationProtocol::GetXAxisCalibrationValue(std::span<u8> block) const {
+    return static_cast<u16>(((block[1] & 0x0F) << 8) | block[0]);
+}
+
+u16 CalibrationProtocol::GetYAxisCalibrationValue(std::span<u8> block) const {
+    return static_cast<u16>((block[2] << 4) | (block[1] >> 4));
+}
+
 void CalibrationProtocol::ValidateCalibration(JoyStickCalibration& calibration) {
-    constexpr u16 DefaultStickCenter{2048};
-    constexpr u16 DefaultStickRange{1740};
+    constexpr u16 DefaultStickCenter{0x800};
+    constexpr u16 DefaultStickRange{0x6cc};
 
-    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;
+    calibration.x.center = ValidateValue(calibration.x.center, DefaultStickCenter);
+    calibration.x.max = ValidateValue(calibration.x.max, DefaultStickRange);
+    calibration.x.min = ValidateValue(calibration.x.min, DefaultStickRange);
+
+    calibration.y.center = ValidateValue(calibration.y.center, DefaultStickCenter);
+    calibration.y.max = ValidateValue(calibration.y.max, DefaultStickRange);
+    calibration.y.min = ValidateValue(calibration.y.min, DefaultStickRange);
+}
+
+void CalibrationProtocol::ValidateCalibration(MotionCalibration& calibration) {
+    constexpr s16 DefaultAccelerometerScale{0x4000};
+    constexpr s16 DefaultGyroScale{0x3be7};
+    constexpr s16 DefaultOffset{0};
+
+    for (auto& sensor : calibration.accelerometer) {
+        sensor.scale = ValidateValue(sensor.scale, DefaultAccelerometerScale);
+        sensor.offset = ValidateValue(sensor.offset, DefaultOffset);
     }
-    if (calibration.x.min == 0xFFF || calibration.x.min == 0) {
-        calibration.x.min = DefaultStickRange;
+    for (auto& sensor : calibration.gyro) {
+        sensor.scale = ValidateValue(sensor.scale, DefaultGyroScale);
+        sensor.offset = ValidateValue(sensor.offset, DefaultOffset);
     }
+}
 
-    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;
+u16 CalibrationProtocol::ValidateValue(u16 value, u16 default_value) const {
+    if (value == 0) {
+        return default_value;
     }
-    if (calibration.y.min == 0xFFF || calibration.y.min == 0) {
-        calibration.y.min = DefaultStickRange;
+    if (value == 0xFFF) {
+        return default_value;
     }
+    return value;
 }
 
-void CalibrationProtocol::ValidateCalibration(MotionCalibration& calibration) {
-    for (auto& sensor : calibration.accelerometer) {
-        if (sensor.scale == 0) {
-            sensor.scale = 0x4000;
-        }
+s16 CalibrationProtocol::ValidateValue(s16 value, s16 default_value) const {
+    if (value == 0) {
+        return default_value;
     }
-    for (auto& sensor : calibration.gyro) {
-        if (sensor.scale == 0) {
-            sensor.scale = 0x3be7;
-        }
+    if (value == 0xFFF) {
+        return default_value;
     }
+    return value;
 }
 
 } // namespace InputCommon::Joycon
diff --git a/src/input_common/helpers/joycon_protocol/calibration.h b/src/input_common/helpers/joycon_protocol/calibration.h
index afb52a36a..c6fd0f729 100644
--- a/src/input_common/helpers/joycon_protocol/calibration.h
+++ b/src/input_common/helpers/joycon_protocol/calibration.h
@@ -53,9 +53,27 @@ public:
     DriverResult GetRingCalibration(RingCalibration& calibration, s16 current_value);
 
 private:
+    /// Returns true if the specified address corresponds to the magic value of user calibration
+    DriverResult HasUserCalibration(SpiAddress address, bool& has_user_calibration);
+
+    /// Converts a raw calibration block to an u16 value containing the x axis value
+    u16 GetXAxisCalibrationValue(std::span<u8> block) const;
+
+    /// Converts a raw calibration block to an u16 value containing the y axis value
+    u16 GetYAxisCalibrationValue(std::span<u8> block) const;
+
+    /// Ensures that all joystick calibration values are set
     void ValidateCalibration(JoyStickCalibration& calibration);
+
+    /// Ensures that all motion calibration values are set
     void ValidateCalibration(MotionCalibration& calibration);
 
+    /// Returns the default value if the value is either zero or 0xFFF
+    u16 ValidateValue(u16 value, u16 default_value) const;
+
+    /// Returns the default value if the value is either zero or 0xFFF
+    s16 ValidateValue(s16 value, s16 default_value) const;
+
     s16 ring_data_max = 0;
     s16 ring_data_default = 0;
     s16 ring_data_min = 0;
diff --git a/src/input_common/helpers/joycon_protocol/common_protocol.cpp b/src/input_common/helpers/joycon_protocol/common_protocol.cpp
index 417d0dcc5..2f0ab6b2a 100644
--- a/src/input_common/helpers/joycon_protocol/common_protocol.cpp
+++ b/src/input_common/helpers/joycon_protocol/common_protocol.cpp
@@ -23,7 +23,7 @@ void JoyconCommonProtocol::SetNonBlocking() {
 
 DriverResult JoyconCommonProtocol::GetDeviceType(ControllerType& controller_type) {
     std::vector<u8> buffer;
-    const auto result = ReadSPI(CalAddr::DEVICE_TYPE, 1, buffer);
+    const auto result = ReadSPI(SpiAddress::DEVICE_TYPE, 1, buffer);
     controller_type = ControllerType::None;
 
     if (result == DriverResult::Success) {
@@ -148,7 +148,7 @@ DriverResult JoyconCommonProtocol::SendVibrationReport(std::span<const u8> buffe
     return SendData(local_buffer);
 }
 
-DriverResult JoyconCommonProtocol::ReadSPI(CalAddr addr, u8 size, std::vector<u8>& output) {
+DriverResult JoyconCommonProtocol::ReadSPI(SpiAddress addr, u8 size, std::vector<u8>& output) {
     constexpr std::size_t MaxTries = 10;
     std::size_t tries = 0;
     std::array<u8, 5> buffer = {0x00, 0x00, 0x00, 0x00, size};
diff --git a/src/input_common/helpers/joycon_protocol/common_protocol.h b/src/input_common/helpers/joycon_protocol/common_protocol.h
index 903bcf402..675eacf68 100644
--- a/src/input_common/helpers/joycon_protocol/common_protocol.h
+++ b/src/input_common/helpers/joycon_protocol/common_protocol.h
@@ -100,7 +100,26 @@ public:
      * @param size in bytes to be read
      * @returns output buffer containing the responce
      */
-    DriverResult ReadSPI(CalAddr addr, u8 size, std::vector<u8>& output);
+    DriverResult ReadSPI(SpiAddress addr, u8 size, std::vector<u8>& output);
+
+    template <typename Output>
+        requires(std::is_trivially_copyable_v<Output>)
+    DriverResult ReadSPI(SpiAddress addr, Output& output) {
+        std::vector<u8> buffer;
+        output = {};
+
+        const auto result = ReadSPI(addr, sizeof(Output), buffer);
+        if (result != DriverResult::Success) {
+            return result;
+        }
+
+        if (buffer.size() != sizeof(Output)) {
+            return DriverResult::WrongReply;
+        }
+
+        std::memcpy(&output, buffer.data(), sizeof(Output));
+        return DriverResult::Success;
+    }
 
     /**
      * Enables MCU chip on the joycon
diff --git a/src/input_common/helpers/joycon_protocol/generic_functions.cpp b/src/input_common/helpers/joycon_protocol/generic_functions.cpp
index 63cfb1369..4763ba3e6 100644
--- a/src/input_common/helpers/joycon_protocol/generic_functions.cpp
+++ b/src/input_common/helpers/joycon_protocol/generic_functions.cpp
@@ -72,7 +72,7 @@ DriverResult GenericProtocol::GetBattery(u32& battery_level) {
 DriverResult GenericProtocol::GetColor(Color& color) {
     ScopedSetBlocking sb(this);
     std::vector<u8> buffer;
-    const auto result = ReadSPI(CalAddr::COLOR_DATA, 12, buffer);
+    const auto result = ReadSPI(SpiAddress::COLOR_DATA, 12, buffer);
 
     color = {};
     if (result == DriverResult::Success) {
@@ -88,7 +88,7 @@ DriverResult GenericProtocol::GetColor(Color& color) {
 DriverResult GenericProtocol::GetSerialNumber(SerialNumber& serial_number) {
     ScopedSetBlocking sb(this);
     std::vector<u8> buffer;
-    const auto result = ReadSPI(CalAddr::SERIAL_NUMBER, 16, buffer);
+    const auto result = ReadSPI(SpiAddress::SERIAL_NUMBER, 16, buffer);
 
     serial_number = {};
     if (result == DriverResult::Success) {
diff --git a/src/input_common/helpers/joycon_protocol/joycon_types.h b/src/input_common/helpers/joycon_protocol/joycon_types.h
index 182d2c15b..14b07bfb5 100644
--- a/src/input_common/helpers/joycon_protocol/joycon_types.h
+++ b/src/input_common/helpers/joycon_protocol/joycon_types.h
@@ -159,13 +159,12 @@ enum class UsbSubCommand : u8 {
     SEND_UART = 0x92,
 };
 
-enum class CalMagic : u8 {
+enum class CalibrationMagic : u8 {
     USR_MAGIC_0 = 0xB2,
     USR_MAGIC_1 = 0xA1,
-    USRR_MAGI_SIZE = 2,
 };
 
-enum class CalAddr {
+enum class SpiAddress {
     SERIAL_NUMBER = 0X6000,
     DEVICE_TYPE = 0X6012,
     COLOR_EXIST = 0X601B,
@@ -396,10 +395,35 @@ struct MotionData {
     u64 delta_timestamp{};
 };
 
+// Output from SPI read command containing user calibration magic
+struct MagicSpiCalibration {
+    CalibrationMagic first;
+    CalibrationMagic second;
+};
+static_assert(sizeof(MagicSpiCalibration) == 0x2, "MagicSpiCalibration is an invalid size");
+
+// Output from SPI read command containing left joystick calibration
+struct JoystickLeftSpiCalibration {
+    std::array<u8, 3> max;
+    std::array<u8, 3> center;
+    std::array<u8, 3> min;
+};
+static_assert(sizeof(JoystickLeftSpiCalibration) == 0x9,
+              "JoystickLeftSpiCalibration is an invalid size");
+
+// Output from SPI read command containing right joystick calibration
+struct JoystickRightSpiCalibration {
+    std::array<u8, 3> center;
+    std::array<u8, 3> min;
+    std::array<u8, 3> max;
+};
+static_assert(sizeof(JoystickRightSpiCalibration) == 0x9,
+              "JoystickRightSpiCalibration is an invalid size");
+
 struct JoyStickAxisCalibration {
-    u16 max{1};
-    u16 min{1};
-    u16 center{0};
+    u16 max;
+    u16 min;
+    u16 center;
 };
 
 struct JoyStickCalibration {
@@ -407,6 +431,14 @@ struct JoyStickCalibration {
     JoyStickAxisCalibration y;
 };
 
+struct ImuSpiCalibration {
+    std::array<s16, 3> accelerometer_offset;
+    std::array<s16, 3> accelerometer_scale;
+    std::array<s16, 3> gyroscope_offset;
+    std::array<s16, 3> gyroscope_scale;
+};
+static_assert(sizeof(ImuSpiCalibration) == 0x18, "ImuSpiCalibration is an invalid size");
+
 struct RingCalibration {
     s16 default_value;
     s16 max_value;
@@ -488,14 +520,6 @@ struct InputReportNfcIr {
 static_assert(sizeof(InputReportNfcIr) == 0x29, "InputReportNfcIr is an invalid size");
 #pragma pack(pop)
 
-struct IMUCalibration {
-    std::array<s16, 3> accelerometer_offset;
-    std::array<s16, 3> accelerometer_scale;
-    std::array<s16, 3> gyroscope_offset;
-    std::array<s16, 3> gyroscope_scale;
-};
-static_assert(sizeof(IMUCalibration) == 0x18, "IMUCalibration is an invalid size");
-
 struct NFCReadBlock {
     u8 start;
     u8 end;