VERY experimental Nintendo Switch 2 emulator
C++
input_common: joycon: Remove magic numbers from calibration protocol
Narr the Regf20ec6f69b72
diff --git a/src/input_common/helpers/joycon_protocol/calibration.cpp b/src/input_common/helpers/joycon_protocol/calibration.cppindex f6e7e97d59..d8f040f75b 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 missingValidateCalibration(calibration);@@ -49,33 +49,33 @@ DriverResult CalibrationProtocol::GetLeftJoyStickCalibration(JoyStickCalibrationDriverResult 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 missingValidateCalibration(calibration);@@ -85,39 +85,41 @@ DriverResult CalibrationProtocol::GetRightJoyStickCalibration(JoyStickCalibratioDriverResult 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& calibratiDriverResult CalibrationProtocol::GetRingCalibration(RingCalibration& calibration,s16 current_value) {+ constexpr s16 DefaultRingRange{800};+// TODO: Get default calibration form ring itselfif (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& calibratioreturn 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::Joycondiff --git a/src/input_common/helpers/joycon_protocol/calibration.h b/src/input_common/helpers/joycon_protocol/calibration.hindex afb52a36a4..c6fd0f7299 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 setvoid ValidateCalibration(JoyStickCalibration& calibration);++ /// Ensures that all motion calibration values are setvoid 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.cppindex 417d0dcc50..2f0ab6b2a7 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> buffereturn 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.hindex 903bcf4025..675eacf68b 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 joycondiff --git a/src/input_common/helpers/joycon_protocol/generic_functions.cpp b/src/input_common/helpers/joycon_protocol/generic_functions.cppindex 63cfb13690..4763ba3e65 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.hindex 182d2c15b7..14b07bfb52 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;