diff options
6 files changed, 215 insertions, 114 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..0ef240344 100644 --- a/src/input_common/helpers/joycon_protocol/common_protocol.cpp +++ b/src/input_common/helpers/joycon_protocol/common_protocol.cpp @@ -22,8 +22,8 @@ void JoyconCommonProtocol::SetNonBlocking() { } DriverResult JoyconCommonProtocol::GetDeviceType(ControllerType& controller_type) { - std::vector<u8> buffer; - const auto result = ReadSPI(CalAddr::DEVICE_TYPE, 1, buffer); + std::array<u8, 1> buffer{}; + const auto result = ReadRawSPI(SpiAddress::DEVICE_TYPE, buffer); controller_type = ControllerType::None; if (result == DriverResult::Success) { @@ -148,11 +148,13 @@ 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::ReadRawSPI(SpiAddress addr, std::span<u8> output) { + constexpr std::size_t HeaderSize = 20; constexpr std::size_t MaxTries = 10; + const auto size = output.size(); std::size_t tries = 0; - std::array<u8, 5> buffer = {0x00, 0x00, 0x00, 0x00, size}; - std::vector<u8> local_buffer(size + 20); + std::array<u8, 5> buffer = {0x00, 0x00, 0x00, 0x00, static_cast<u8>(size)}; + std::vector<u8> local_buffer{}; buffer[0] = static_cast<u8>(static_cast<u16>(addr) & 0x00FF); buffer[1] = static_cast<u8>((static_cast<u16>(addr) & 0xFF00) >> 8); @@ -167,8 +169,12 @@ DriverResult JoyconCommonProtocol::ReadSPI(CalAddr addr, u8 size, std::vector<u8 } } while (local_buffer[15] != buffer[0] || local_buffer[16] != buffer[1]); + if (local_buffer.size() < size + HeaderSize) { + return DriverResult::WrongReply; + } + // Remove header from output - output = std::vector<u8>(local_buffer.begin() + 20, local_buffer.begin() + 20 + size); + memcpy(output.data(), local_buffer.data() + HeaderSize, size); return DriverResult::Success; } diff --git a/src/input_common/helpers/joycon_protocol/common_protocol.h b/src/input_common/helpers/joycon_protocol/common_protocol.h index 903bcf402..75d3f20a4 100644 --- a/src/input_common/helpers/joycon_protocol/common_protocol.h +++ b/src/input_common/helpers/joycon_protocol/common_protocol.h @@ -97,10 +97,29 @@ public: /** * Reads the SPI memory stored on the joycon * @param Initial address location - * @param size in bytes to be read * @returns output buffer containing the responce */ - DriverResult ReadSPI(CalAddr addr, u8 size, std::vector<u8>& output); + DriverResult ReadRawSPI(SpiAddress addr, std::span<u8> output); + + /** + * Reads the SPI memory stored on the joycon + * @param Initial address location + * @returns output object containing the responce + */ + template <typename Output> + requires std::is_trivially_copyable_v<Output> DriverResult ReadSPI(SpiAddress addr, + Output& output) { + std::array<u8, sizeof(Output)> buffer; + output = {}; + + const auto result = ReadRawSPI(addr, buffer); + if (result != DriverResult::Success) { + return result; + } + + 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..484c208e6 100644 --- a/src/input_common/helpers/joycon_protocol/generic_functions.cpp +++ b/src/input_common/helpers/joycon_protocol/generic_functions.cpp @@ -71,8 +71,8 @@ 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); + std::array<u8, 12> buffer{}; + const auto result = ReadRawSPI(SpiAddress::COLOR_DATA, buffer); color = {}; if (result == DriverResult::Success) { @@ -87,8 +87,8 @@ 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); + std::array<u8, 16> buffer{}; + const auto result = ReadRawSPI(SpiAddress::SERIAL_NUMBER, 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; |