diff --git a/src/components/bluepad32/bt/uni_bt.c b/src/components/bluepad32/bt/uni_bt.c --- a/src/components/bluepad32/bt/uni_bt.c +++ b/src/components/bluepad32/bt/uni_bt.c @@ -73,6 +73,12 @@ #include "uni_log.h" #include "uni_property.h" +#ifdef SWITCH_PICO_HAPTICS_EXPERIMENT +// The opt-in PCM sender owns its CAN_SEND_NOW event, not the generic HID FIFO. +extern bool uni_platform_on_l2cap_can_send_now(uni_hid_device_t* device, + uint16_t local_cid); +#endif + // globals bd_addr_t uni_local_bd_addr; @@ -100,17 +106,21 @@ }; static void bluetooth_del_keys(void) { - if (IS_ENABLED(UNI_ENABLE_BREDR)) - uni_bt_bredr_delete_bonded_keys(); - if (IS_ENABLED(UNI_ENABLE_BLE)) - uni_bt_le_delete_bonded_keys(); +#ifdef ENABLE_CLASSIC + uni_bt_bredr_delete_bonded_keys(); +#endif +#ifdef ENABLE_BLE + uni_bt_le_delete_bonded_keys(); +#endif } static void bluetooth_list_keys(void) { - if (IS_ENABLED(UNI_ENABLE_BREDR)) - uni_bt_bredr_list_bonded_keys(); - if (IS_ENABLED(UNI_ENABLE_BLE)) - uni_bt_le_list_bonded_keys(); +#ifdef ENABLE_CLASSIC + uni_bt_bredr_list_bonded_keys(); +#endif +#ifdef ENABLE_BLE + uni_bt_le_list_bonded_keys(); +#endif } static void start_scan(void) { @@ -172,9 +182,9 @@ // Device cannot be used after delete. d = NULL; - if (IS_ENABLED(UNI_ENABLE_BLE) && type == GAP_CONNECTION_LE) + if (IS_ENABLED(ENABLE_BLE) && type == GAP_CONNECTION_LE) uni_bt_le_on_hci_disconnection_complete(channel, packet, size); - else if (IS_ENABLED(UNI_ENABLE_BREDR) && type == GAP_CONNECTION_ACL) + else if (IS_ENABLED(ENABLE_CLASSIC) && type == GAP_CONNECTION_ACL) uni_bt_bredr_on_hci_disconnection_complete(channel, packet, size); else loge("on_hci_disconnection_complete: Unknown GAP connection type: %d\n", type); @@ -473,6 +483,10 @@ if (device == NULL) { loge("--->>> CANNOT FIND DEVICE"); } else { +#ifdef SWITCH_PICO_HAPTICS_EXPERIMENT + if (uni_platform_on_l2cap_can_send_now(device, local_cid)) + break; +#endif uni_hid_device_send_queued_reports(device); } break; diff --git a/src/components/bluepad32/bt/uni_bt_bredr.c b/src/components/bluepad32/bt/uni_bt_bredr.c --- a/src/components/bluepad32/bt/uni_bt_bredr.c +++ b/src/components/bluepad32/bt/uni_bt_bredr.c @@ -28,7 +28,7 @@ #define INQUIRY_REMOTE_NAME_TIMEOUT_MS 4500 _Static_assert(INQUIRY_REMOTE_NAME_TIMEOUT_MS < HID_DEVICE_CONNECTION_TIMEOUT_MS, "Timeout too big"); -static bool bt_bredr_enabled = true; +static bool bt_bredr_enabled = IS_ENABLED(UNI_ENABLE_BREDR); static void l2cap_create_control_connection(uni_hid_device_t* d) { uint8_t status; @@ -62,6 +62,8 @@ } void uni_bt_bredr_scan_start(void) { + if (!bt_bredr_enabled) + return; uint8_t status; status = gap_inquiry_periodic_start(uni_bt_get_gap_inquiry_length(), uni_bt_get_gap_max_periodic_length(), @@ -147,6 +149,8 @@ } void uni_bt_bredr_setup(void) { + if (!bt_bredr_enabled) + return; int security_level = uni_bt_get_gap_security_level(); gap_set_security_level(security_level); @@ -183,7 +187,7 @@ } void uni_bt_bredr_set_enabled(bool enabled) { - bt_bredr_enabled = enabled; + bt_bredr_enabled = enabled && IS_ENABLED(UNI_ENABLE_BREDR); } bool uni_bt_bredr_is_enabled(void) { @@ -423,13 +427,14 @@ status = l2cap_event_channel_opened_get_status(packet); if (status) { logi("L2CAP Connection failed: 0x%02x.\n", status); - // Practice showed that if the connection fails, just disconnect/remove - // so that the connection can start again. + // Channel-open failures also include transient page timeouts when a + // paired controller powers down or is temporarily unreachable. Keep + // the persistent key so the controller can reconnect later. Users can + // remove genuinely stale keys through the explicit pairing reset. if (status == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY) { logi("Probably GAP-security-related issues. Set GAP security to 2\n"); } - logi("Removing key for device: %s.\n", bd_addr_to_str(address)); - gap_drop_link_key_for_bd_addr(device->conn.btaddr); + logi("Removing failed device instance for: %s; preserving link key.\n", bd_addr_to_str(address)); uni_hid_device_disconnect(device); uni_hid_device_delete(device); /* 'device' is destroyed, don't use */ @@ -562,6 +567,17 @@ void uni_bt_bredr_on_gap_inquiry_result(uint16_t channel, const uint8_t* packet, uin logi(", name '%s'", name_buffer); } logi("\n"); + // Reconnect discovery is not a pairing request. Avoid paging nearby + // unpaired devices outside the explicit bondable window: their connection + // attempts compete with live controller input and native PCM for airtime. + if (!gap_get_bondable_mode()) { + link_key_t key; + link_key_type_t type; + if (!gap_get_link_key_for_bd_addr(addr, key, &type)) { + return; + } + } + supported = uni_hid_device_on_device_discovered(addr, name_buffer, cod, rssi) == UNI_ERROR_SUCCESS; if (supported) { diff --git a/src/components/bluepad32/bt/uni_bt_le.c b/src/components/bluepad32/bt/uni_bt_le.c --- a/src/components/bluepad32/bt/uni_bt_le.c +++ b/src/components/bluepad32/bt/uni_bt_le.c @@ -68,6 +68,7 @@ #include "bt/uni_bt_conn.h" #include "bt/uni_bt_defines.h" #include "parser/uni_hid_parser.h" +#include "parser/uni_hid_parser_switch2.h" #include "uni_common.h" #include "uni_config.h" #include "uni_hid_device.h" @@ -93,7 +94,7 @@ gap_connect(addr, addr_type); } -static void resume_scanning_hint(void) { +void uni_bt_le_resume_scanning_if_enabled(void) { // Resume scanning, only if it was scanning before connecting if (is_scanning) { gap_start_scan(); @@ -107,7 +108,7 @@ uni_hid_device_t* device; device = uni_hid_device_get_instance_for_connection_handle(con_handle); - if (device) { + if (device && !uni_hid_parser_switch2_is_ble_device(device)) { status = hids_client_disconnect(device->hids_cid); if (status != ERROR_CODE_SUCCESS) { loge("Failed to disconnect HIDS client for hids_cid=%d, status=%d\n", device->hids_cid, status); @@ -118,7 +119,7 @@ if (gap_get_connection_type(con_handle) != GAP_CONNECTION_INVALID) gap_disconnect(con_handle); - resume_scanning_hint(); + uni_bt_le_resume_scanning_if_enabled(); } static void get_advertisement_data(const uint8_t* adv_data, uint8_t adv_size, uint16_t* appearance, char* name) { @@ -297,7 +298,7 @@ uni_hid_device_connect(device); uni_hid_device_set_ready(device); - resume_scanning_hint(); + uni_bt_le_resume_scanning_if_enabled(); break; default: loge("HID service client connection failed, err 0x%02x.\n", status); @@ -560,10 +561,22 @@ type = hci_event_packet_get_type(packet); switch (type) { case SM_EVENT_JUST_WORKS_REQUEST: + con_handle = sm_event_just_works_request_get_handle(packet); + device = uni_hid_device_get_instance_for_connection_handle(con_handle); + if (uni_hid_parser_switch2_is_ble_device(device)) { + hog_disconnect(con_handle); + break; + } logi("Just works requested\n"); sm_just_works_confirm(sm_event_just_works_request_get_handle(packet)); break; case SM_EVENT_NUMERIC_COMPARISON_REQUEST: + con_handle = sm_event_numeric_comparison_request_get_handle(packet); + device = uni_hid_device_get_instance_for_connection_handle(con_handle); + if (uni_hid_parser_switch2_is_ble_device(device)) { + hog_disconnect(con_handle); + break; + } logi("Confirming numeric comparison: %" PRIu32 "\n", sm_event_numeric_comparison_request_get_passkey(packet)); sm_numeric_comparison_confirm(sm_event_passkey_display_number_get_handle(packet)); @@ -598,6 +611,12 @@ break; case SM_EVENT_REENCRYPTION_COMPLETE: con_handle = sm_event_reencryption_complete_get_handle(packet); + device = uni_hid_device_get_instance_for_connection_handle(con_handle); + if (uni_hid_parser_switch2_is_ble_device(device)) { + // Proprietary Switch 2 GATT must never enter HOGP or re-pair SMP. + hog_disconnect(con_handle); + break; + } switch (sm_event_reencryption_complete_get_status(packet)) { case ERROR_CODE_SUCCESS: logi("Re-encryption complete, success\n"); @@ -630,6 +649,10 @@ con_handle = sm_event_pairing_complete_get_handle(packet); if (!device) { loge("SM_EVENT_PAIRING_COMPLETE: Invalid device for addr %s\n", bd_addr_to_str(addr)); + hog_disconnect(con_handle); + break; + } + if (uni_hid_parser_switch2_is_ble_device(device)) { hog_disconnect(con_handle); break; } @@ -700,7 +723,18 @@ con_handle = hci_subevent_le_connection_complete_get_connection_handle(packet); logi("Using con_handle: %#x\n", con_handle); + if (uni_hid_parser_switch2_is_ble_device(device) && + hci_subevent_le_connection_complete_get_status(packet) != ERROR_CODE_SUCCESS) { + // A failed event has no valid connection handle to disconnect. + uni_hid_device_delete(device); + uni_bt_le_resume_scanning_if_enabled(); + break; + } uni_hid_device_set_connection_handle(device, con_handle); + if (uni_hid_parser_switch2_is_ble_device(device)) { + uni_hid_parser_switch2_on_le_connected(device); + break; + } sm_request_pairing(con_handle); // Resume scanning @@ -739,6 +773,8 @@ if (device->conn.protocol != UNI_BT_CONN_PROTOCOL_BLE) // Abort on non BLE connections return; + if (uni_hid_parser_switch2_is_ble_device(device)) + return; // Switch 2 uses its own application-level pairing and GATT setup. logi("Connection encrypted: %u\n", hci_event_encryption_change_get_encryption_enabled(packet)); if (hci_event_encryption_change_get_encryption_enabled(packet) == 0) { @@ -758,7 +794,11 @@ appearance = 0; name[0] = 0; - ARG_UNUSED(size); + if (packet == NULL || size < 12 || + (uint16_t)gap_event_advertising_report_get_data_length(packet) > size - 12u) + return; + if (uni_bt_le_switch2_handle_advertisement(packet, size)) + return; gap_event_advertising_report_get_address(packet, addr); if (uni_hid_device_get_instance_for_address(addr)) { @@ -826,7 +866,7 @@ ARG_UNUSED(packet); ARG_UNUSED(size); - resume_scanning_hint(); + uni_bt_le_resume_scanning_if_enabled(); } void uni_bt_le_list_bonded_keys(void) { @@ -874,7 +914,14 @@ logi(".\n"); } +void uni_bt_le_set_background_scan(bool enabled) { + gap_set_scan_parameters(0 /* type: passive */, enabled ? 160 : 48 /* interval */, + enabled ? 16 : 48 /* window */); +} + void uni_bt_le_setup(void) { + if (!IS_ENABLED(UNI_ENABLE_BLE)) + return; // register for events from Security Manager sm_event_callback_registration.callback = &uni_sm_packet_handler; sm_add_event_handler(&sm_event_callback_registration); @@ -921,11 +968,11 @@ // scan_parameters_service_client_init(); device_information_service_client_init(); - gap_set_scan_parameters(0 /* type: passive */, 48 /* interval */, 48 /* window */); + uni_bt_le_set_background_scan(false); } void uni_bt_le_scan_start(void) { - if (!ble_enabled) + if (!IS_ENABLED(UNI_ENABLE_BLE) || !ble_enabled) return; gap_start_scan(); @@ -950,6 +997,9 @@ void uni_bt_le_set_enabled(bool enabled) { // Called from different Task. Don't call BTstack functions. + // A build-time radio exclusion must not rewrite the saved mixed-mode preference. + if (!IS_ENABLED(UNI_ENABLE_BLE)) + return; uni_property_value_t val; val.u8 = enabled; @@ -959,6 +1009,8 @@ } bool uni_bt_le_is_enabled() { + if (!IS_ENABLED(UNI_ENABLE_BLE)) + return false; // Expensive call. Avoid calling it from this same file. // Called from "uni_bt_setup" uni_property_value_t val; diff --git a/src/components/bluepad32/bt/uni_bt_service.c b/src/components/bluepad32/bt/uni_bt_service.c --- a/src/components/bluepad32/bt/uni_bt_service.c +++ b/src/components/bluepad32/bt/uni_bt_service.c @@ -12,6 +12,7 @@ #include "bt/uni_bt_service.gatt.h" #include "controller/uni_gamepad.h" #include "uni_common.h" +#include "uni_config.h" #include "uni_log.h" #include "uni_system.h" #include "uni_version.h" @@ -433,6 +434,8 @@ * Finally, it configures the advertisements. */ void uni_bt_service_init(void) { + if (!IS_ENABLED(UNI_ENABLE_BLE)) + return; logi("Starting Bluepad32 BLE service UUID: 4627C4A4-AC00-46B9-B688-AFC5C1BF7F63\n"); // Setup ATT server. @@ -464,6 +467,7 @@ } void uni_bt_service_set_enabled(bool enabled) { + enabled = enabled && IS_ENABLED(UNI_ENABLE_BLE); if (enabled == service_enabled) return; diff --git a/src/components/bluepad32/controller/uni_gamepad.c b/src/components/bluepad32/controller/uni_gamepad.c --- a/src/components/bluepad32/controller/uni_gamepad.c +++ b/src/components/bluepad32/controller/uni_gamepad.c @@ -35,6 +35,9 @@ {CONTROLLER_TYPE_SwitchJoyConRight, "Switch JoyCon Right"}, {CONTROLLER_TYPE_SwitchJoyConPair, "Switch JoyCon Pair"}, {CONTROLLER_TYPE_SwitchInputOnlyController, "Switch Input Only"}, + {CONTROLLER_TYPE_Switch2ProController, "Switch 2 Pro"}, + {CONTROLLER_TYPE_Switch2JoyConLeft, "Joy-Con 2 Left"}, + {CONTROLLER_TYPE_Switch2JoyConRight, "Joy-Con 2 Right"}, {CONTROLLER_TYPE_MobileTouch, "Mobile Touch"}, {CONTROLLER_TYPE_XInputSwitchController, "XInput Switch"}, {CONTROLLER_TYPE_PS5Controller, "DualSense"}, diff --git a/src/components/bluepad32/include/bt/uni_bt_le.h b/src/components/bluepad32/include/bt/uni_bt_le.h --- a/src/components/bluepad32/include/bt/uni_bt_le.h +++ b/src/components/bluepad32/include/bt/uni_bt_le.h @@ -25,6 +25,8 @@ void uni_bt_le_scan_start(void); void uni_bt_le_scan_stop(void); +void uni_bt_le_set_background_scan(bool enabled); +void uni_bt_le_resume_scanning_if_enabled(void); // Called from uni_hid_device_disconnect() void uni_bt_le_disconnect(uni_hid_device_t* d); diff --git a/src/components/bluepad32/include/controller/uni_controller_list.h b/src/components/bluepad32/include/controller/uni_controller_list.h --- a/src/components/bluepad32/include/controller/uni_controller_list.h +++ b/src/components/bluepad32/include/controller/uni_controller_list.h @@ -568,6 +568,9 @@ // * ZhiXu Gamepad Wireless // * Sunwaytek Wireless Motion Controller for Nintendo Switch { MAKE_CONTROLLER_ID( 0x057e, 0x2009 ), k_eControllerType_SwitchProController, NULL }, // Nintendo Switch Pro Controller + { MAKE_CONTROLLER_ID( 0x057e, 0x2069 ), k_eControllerType_Switch2ProController, NULL }, + { MAKE_CONTROLLER_ID( 0x057e, 0x2067 ), k_eControllerType_Switch2JoyConLeft, NULL }, + { MAKE_CONTROLLER_ID( 0x057e, 0x2066 ), k_eControllerType_Switch2JoyConRight, NULL }, //{ MAKE_CONTROLLER_ID( 0x057e, 0x2017 ), k_eControllerType_SwitchProController, NULL }, // Nintendo Online SNES Controller //{ MAKE_CONTROLLER_ID( 0x057e, 0x2019 ), k_eControllerType_SwitchProController, NULL }, // Nintendo Online N64 Controller //{ MAKE_CONTROLLER_ID( 0x057e, 0x201e ), k_eControllerType_SwitchProController, NULL }, // Nintendo Online SEGA Genesis Controller diff --git a/src/components/bluepad32/include/controller/uni_controller_type.h b/src/components/bluepad32/include/controller/uni_controller_type.h --- a/src/components/bluepad32/include/controller/uni_controller_type.h +++ b/src/components/bluepad32/include/controller/uni_controller_type.h @@ -70,6 +70,9 @@ k_eControllerType_OUYAController = 55, // (Bluepad32) k_eControllerType_PSMoveController = 56, // (Bluepad32) k_eControllerType_AtariJoystick = 57, // (Bluepad32) + k_eControllerType_Switch2ProController = 58, + k_eControllerType_Switch2JoyConLeft = 59, + k_eControllerType_Switch2JoyConRight = 60, k_eControllerType_LastController, // Don't add game controllers below this enumeration - this enumeration can // change value @@ -120,6 +123,9 @@ #define CONTROLLER_TYPE_OUYAController k_eControllerType_OUYAController #define CONTROLLER_TYPE_PSMoveController k_eControllerType_PSMoveController #define CONTROLLER_TYPE_AtariJoystick k_eControllerType_AtariJoystick +#define CONTROLLER_TYPE_Switch2ProController k_eControllerType_Switch2ProController +#define CONTROLLER_TYPE_Switch2JoyConLeft k_eControllerType_Switch2JoyConLeft +#define CONTROLLER_TYPE_Switch2JoyConRight k_eControllerType_Switch2JoyConRight #define CONTROLLER_TYPE_LastController k_eControllerType_LastController #define CONTROLLER_TYPE_GenericKeyboard k_eControllertype_GenericKeyboard #define CONTROLLER_TYPE_GenericMouse k_eControllertype_GenericMouse diff --git a/src/components/bluepad32/include/parser/uni_hid_parser_psmove.h b/src/components/bluepad32/include/parser/uni_hid_parser_psmove.h --- a/src/components/bluepad32/include/parser/uni_hid_parser_psmove.h +++ b/src/components/bluepad32/include/parser/uni_hid_parser_psmove.h @@ -14,6 +14,8 @@ void uni_hid_parser_psmove_setup(struct uni_hid_device_s* d); void uni_hid_parser_psmove_init_report(struct uni_hid_device_s* d); void uni_hid_parser_psmove_parse_input_report(struct uni_hid_device_s* d, const uint8_t* report, uint16_t len); +void uni_hid_parser_psmove_parse_feature_report( + struct uni_hid_device_s* d, const uint8_t* report, uint16_t len); void uni_hid_parser_psmove_set_lightbar_color(struct uni_hid_device_s* d, uint8_t r, uint8_t g, uint8_t b); void uni_hid_parser_psmove_play_dual_rumble(struct uni_hid_device_s* d, uint16_t start_delay_ms, diff --git a/src/components/bluepad32/include/parser/uni_hid_parser_switch.h b/src/components/bluepad32/include/parser/uni_hid_parser_switch.h --- a/src/components/bluepad32/include/parser/uni_hid_parser_switch.h +++ b/src/components/bluepad32/include/parser/uni_hid_parser_switch.h @@ -23,4 +23,22 @@ bool uni_hid_parser_switch_does_name_match(struct uni_hid_device_s* d, const char* name); void uni_hid_parser_switch_device_dump(struct uni_hid_device_s* d); +// BTstack-thread-only native owner. Qualification belongs to the caller: +// type and firmware are unverified bytes from a successful device-info reply. +bool uni_hid_parser_switch_native_info(struct uni_hid_device_s* d, + uint8_t* type, uint8_t* firmware_hi, + uint8_t* firmware_lo); +bool uni_hid_parser_switch_native_acquire(struct uni_hid_device_s* d); +// Retires compatibility timers and requests neutral; no audio is queued. +void uni_hid_parser_switch_native_release(struct uni_hid_device_s* d); +// Exactly 11 bytes on interrupt L2CAP. False means NOT submitted, never queued. +// Caller must retry/resynchronize; true is transport acceptance, not playback. +bool uni_hid_parser_switch_native_send(struct uni_hid_device_s* d, const uint8_t rumble[8]); + +// Internal device-send/lifecycle hooks, including queued subcommand retries. +// Returns the L2CAP status, or success when discarding an obsolete writer. +int uni_hid_parser_switch_send_report(struct uni_hid_device_s* d, uint16_t cid, + const uint8_t* report, uint16_t len); +void uni_hid_parser_switch_teardown(struct uni_hid_device_s* d); + #endif // UNI_HID_PARSER_SWITCH_H diff --git a/src/components/bluepad32/include/uni.h b/src/components/bluepad32/include/uni.h --- a/src/components/bluepad32/include/uni.h +++ b/src/components/bluepad32/include/uni.h @@ -34,6 +34,7 @@ #include "parser/uni_hid_parser_ds5.h" #include "parser/uni_hid_parser_keyboard.h" #include "parser/uni_hid_parser_mouse.h" +#include "parser/uni_hid_parser_switch2.h" #include "parser/uni_hid_parser_xboxone.h" #include "platform/uni_platform.h" #include "uni_circular_buffer.h" diff --git a/src/components/bluepad32/include/uni_config.h b/src/components/bluepad32/include/uni_config.h --- a/src/components/bluepad32/include/uni_config.h +++ b/src/components/bluepad32/include/uni_config.h @@ -7,8 +7,15 @@ #include "sdkconfig.h" -#if defined(CONFIG_TARGET_POSIX) || defined(CONFIG_TARGET_PICO_W) || defined(CONFIG_IDF_TARGET_ESP32) -// Pico W, original ESP32 and Posix all support both BR/EDR and BLE +#if defined(CONFIG_TARGET_PICO_W) +#if SWITCH_PICO_ENABLE_CLASSIC +#define UNI_ENABLE_BREDR 1 +#endif +#if SWITCH_PICO_ENABLE_BLE +#define UNI_ENABLE_BLE 1 +#endif +#elif defined(CONFIG_TARGET_POSIX) || defined(CONFIG_IDF_TARGET_ESP32) +// Original ESP32 and Posix support both BR/EDR and BLE. #define UNI_ENABLE_BREDR 1 #define UNI_ENABLE_BLE 1 #elif defined(CONFIG_IDF_TARGET_ESP32S3) || defined(CONFIG_IDF_TARGET_ESP32C3) || \ diff --git a/src/components/bluepad32/parser/uni_hid_parser_ds4.c b/src/components/bluepad32/parser/uni_hid_parser_ds4.c --- a/src/components/bluepad32/parser/uni_hid_parser_ds4.c +++ b/src/components/bluepad32/parser/uni_hid_parser_ds4.c @@ -10,6 +10,9 @@ // https://github.com/chrippa/ds4drv/blob/master/ds4drv/device.py #include "parser/uni_hid_parser_ds4.h" +#if SWITCH2_BRIDGE_FULL_INPUT +#include "parser/uni_hid_parser_native_motion.h" +#endif #include @@ -69,6 +72,9 @@ struct ds4_calibration_data gyro_calib_data[3]; struct ds4_calibration_data accel_calib_data[3]; +#if SWITCH2_BRIDGE_FULL_INPUT + bool accel_calibrated, gyro_calibrated; +#endif // Prev Touchpad values, to convert them from absolute // coordinates into relative ones. @@ -220,6 +226,9 @@ void uni_hid_parser_ds4_setup(struct uni_hid_device_s* d) { ds4_instance_t* ins = get_ds4_instance(d); memset(ins, 0, sizeof(*ins)); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_reset(d); +#endif // Default values for Accel / Gyro calibration data, until calibration is supported. for (size_t i = 0; i < ARRAY_SIZE(ins->accel_calib_data); i++) { @@ -263,6 +272,9 @@ } void uni_hid_parser_ds4_init_report(uni_hid_device_t* d) { +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_begin(d); +#endif uni_controller_t* ctl = &d->controller; memset(ctl, 0, sizeof(*ctl)); @@ -279,6 +291,10 @@ void uni_hid_parser_ds4_parse_feature_report(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { ds4_instance_t* ins = get_ds4_instance(d); +#if SWITCH2_BRIDGE_FULL_INPUT + if (!report || !len) + return; +#endif uint8_t report_id = report[0]; switch (report_id) { @@ -286,6 +302,14 @@ int speed_2x; int range_2g; +#if SWITCH2_BRIDGE_FULL_INPUT + if (len != DS4_FEATURE_REPORT_CALIBRATION_SIZE) + break; + ins->accel_calibrated = true; + ins->gyro_calibrated = true; + uni_hid_parser_native_motion_accel(d, NULL); + uni_hid_parser_native_motion_gyro(d, NULL); +#endif if (len != DS4_FEATURE_REPORT_CALIBRATION_SIZE) { loge("DS4: Unexpected calibration size: got %d, want: %d\n", len, DS4_FEATURE_REPORT_CALIBRATION_SIZE); /* fallthrough */ @@ -297,25 +321,34 @@ // Set gyroscope calibration and normalization parameters. // Data values will be normalized to 1/DS_GYRO_RES_PER_DEG_S degree/s. speed_2x = r->gyro_speed_plus + r->gyro_speed_minus; - ins->gyro_calib_data[0].bias = 0; + ins->gyro_calib_data[0].bias = r->gyro_pitch_bias; ins->gyro_calib_data[0].sens_numer = speed_2x * DS4_GYRO_RES_PER_DEG_S; ins->gyro_calib_data[0].sens_denom = abs(r->gyro_pitch_plus - r->gyro_pitch_bias) + abs(r->gyro_pitch_minus + r->gyro_pitch_bias); - ins->gyro_calib_data[1].bias = 0; + ins->gyro_calib_data[1].bias = r->gyro_yaw_bias; ins->gyro_calib_data[1].sens_numer = speed_2x * DS4_GYRO_RES_PER_DEG_S; ins->gyro_calib_data[1].sens_denom = abs(r->gyro_yaw_plus - r->gyro_yaw_bias) + abs(r->gyro_yaw_minus - r->gyro_yaw_bias); - ins->gyro_calib_data[2].bias = 0; + ins->gyro_calib_data[2].bias = r->gyro_roll_bias; ins->gyro_calib_data[2].sens_numer = speed_2x * DS4_GYRO_RES_PER_DEG_S; ins->gyro_calib_data[2].sens_denom = abs(r->gyro_roll_plus - r->gyro_roll_bias) + abs(r->gyro_roll_minus - r->gyro_roll_bias); +#if SWITCH2_BRIDGE_FULL_INPUT + ins->gyro_calib_data[0].sens_denom = + abs(r->gyro_pitch_plus - r->gyro_pitch_bias) + abs(r->gyro_pitch_minus - r->gyro_pitch_bias); + ins->gyro_calibrated = speed_2x > 0; +#endif // Sanity check gyro calibration data. This is needed to prevent crashes // during report handling of virtual, clone or broken devices not implementing // calibration data properly. for (size_t i = 0; i < ARRAY_SIZE(ins->gyro_calib_data); i++) { +#if SWITCH2_BRIDGE_FULL_INPUT + if (ins->gyro_calib_data[i].sens_denom <= 0) + ins->gyro_calibrated = false; +#endif if (ins->gyro_calib_data[i].sens_denom == 0) { loge("Invalid gyro calibration data for axis (%d), disabling calibration for axis = %d\n", i); ins->gyro_calib_data[i].bias = 0; @@ -345,6 +378,10 @@ // during report handling of virtual, clone or broken devices not implementing calibration // data properly. for (size_t i = 0; i < ARRAY_SIZE(ins->accel_calib_data); i++) { +#if SWITCH2_BRIDGE_FULL_INPUT + if (ins->accel_calib_data[i].sens_denom <= 0) + ins->accel_calibrated = false; +#endif if (ins->accel_calib_data[i].sens_denom == 0) { loge("Invalid accelerometer calibration data for axis (%d), disabling calibration for axis=%d\n", i); @@ -357,6 +394,10 @@ break; } case DS4_FEATURE_REPORT_FIRMWARE_VERSION: { +#if SWITCH2_BRIDGE_FULL_INPUT + if (len != DS4_FEATURE_REPORT_FIRMWARE_VERSION_SIZE) + break; +#endif if (len != DS4_FEATURE_REPORT_FIRMWARE_VERSION_SIZE) { loge("DS4: Unexpected firmware version size: got %d, want: %d\n", len, DS4_FEATURE_REPORT_FIRMWARE_VERSION_SIZE); @@ -476,7 +517,7 @@ // Gyro for (size_t i = 0; i < ARRAY_SIZE(r->gyro); i++) { - int32_t raw_data = (int16_t)r->gyro[i]; + int32_t raw_data = (int16_t)r->gyro[i] - ins->gyro_calib_data[i].bias; int32_t calib_data = mult_frac(ins->gyro_calib_data[i].sens_numer, raw_data, ins->gyro_calib_data[i].sens_denom); ctl->gamepad.gyro[i] = calib_data; @@ -484,7 +525,7 @@ // Accel for (size_t i = 0; i < ARRAY_SIZE(r->accel); i++) { - int32_t raw_data = (int16_t)r->accel[i]; + int32_t raw_data = (int16_t)r->accel[i] - ins->accel_calib_data[i].bias; int32_t calib_data = mult_frac(ins->accel_calib_data[i].sens_numer, raw_data, ins->accel_calib_data[i].sens_denom); ctl->gamepad.accel[i] = calib_data; @@ -500,12 +541,35 @@ } void uni_hid_parser_ds4_parse_input_report(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_begin(d); + if (!report || !len) + return; +#endif if (report[0] == 0x11 && len == 78) { +#if SWITCH2_BRIDGE_FULL_INPUT + const uint8_t transaction = 0xa1; + uint32_t crc = uni_crc32_le(UINT32_MAX, &transaction, 1); + crc = ~uni_crc32_le(crc, report, len - 4); + if (crc != little_endian_read_32(report, len - 4)) + return; +#endif const ds4_input_report_11_t* r = (ds4_input_report_11_t*)&report[3]; ds4_parse_input_report_11(d, r); +#if SWITCH2_BRIDGE_FULL_INPUT + ds4_instance_t* ins = get_ds4_instance(d); + uni_hid_parser_native_motion_accept(d); + if (uni_hid_parser_native_motion_fresh(d, r->sensor_timestamp, UINT16_MAX)) { + uni_hid_parser_native_motion_accel(d, ins->accel_calibrated ? d->controller.gamepad.accel : NULL); + uni_hid_parser_native_motion_gyro(d, ins->gyro_calibrated ? d->controller.gamepad.gyro : NULL); + } +#endif } else if (report[0] == 0x01 && len == 10) { const ds4_input_report_01_t* r = (ds4_input_report_01_t*)&report[1]; ds4_parse_input_report_01(d, r); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_accept(d); +#endif } else { loge("DS4: Unexpected report type and len: report id=0x%02x, len=%d\n", report[0], len); } diff --git a/src/components/bluepad32/include/parser/uni_hid_parser_ds5.h b/src/components/bluepad32/include/parser/uni_hid_parser_ds5.h --- a/src/components/bluepad32/include/parser/uni_hid_parser_ds5.h +++ b/src/components/bluepad32/include/parser/uni_hid_parser_ds5.h @@ -7,6 +7,7 @@ #define UNI_HID_PARSER_DS5_H #include +#include #include "parser/uni_hid_parser.h" @@ -33,6 +34,16 @@ uint8_t weak_magnitude, uint8_t strong_magnitude); void uni_hid_parser_ds5_device_dump(struct uni_hid_device_s* d); + +#if SWITCH2_BRIDGE_FULL_INPUT +// Bounded native bridge writer: false means not submitted (never queued). +// The existing duration timer retries OFF for at most 2 seconds, then closes +// a stuck connection without deleting its bond. No application ACK exists. +bool uni_hid_parser_ds5_bridge_rumble(struct uni_hid_device_s* d, uint16_t duration_ms, + uint8_t right, uint8_t left); +// Disconnect/delete retire delayed and duration timers before parser memory reuse. +void uni_hid_parser_ds5_bridge_teardown(struct uni_hid_device_s* d); +#endif // Unique to DualSense. Not part of the "hid_parser" interface // Warning: Adaptive trigger API is experimental. It might change in the future without further notice. diff --git a/src/components/bluepad32/parser/uni_hid_parser_ds5.c b/src/components/bluepad32/parser/uni_hid_parser_ds5.c --- a/src/components/bluepad32/parser/uni_hid_parser_ds5.c +++ b/src/components/bluepad32/parser/uni_hid_parser_ds5.c @@ -9,6 +9,9 @@ // https://controllers.fandom.com/wiki/Sony_DualSense #include "parser/uni_hid_parser_ds5.h" +#if SWITCH2_BRIDGE_FULL_INPUT +#include "parser/uni_hid_parser_native_motion.h" +#endif #include @@ -106,6 +109,10 @@ uint32_t fw_version; uint16_t update_version; bool use_vibration2; +#if SWITCH2_BRIDGE_FULL_INPUT + bool calibration_valid; + uint32_t bridge_stop_deadline_ms; +#endif struct ds5_calibration_data gyro_calib_data[3]; struct ds5_calibration_data accel_calib_data[3]; @@ -228,7 +235,18 @@ _Static_assert(sizeof(ds5_feature_report_calibration_t) == DS5_FEATURE_REPORT_CALIBRATION_SIZE, "Invalid size"); static ds5_instance_t* get_ds5_instance(uni_hid_device_t* d); +#if SWITCH2_BRIDGE_FULL_INPUT +// Bluetooth HID CRC includes its transaction byte, omitted by the parser API. +static bool ds5_bridge_crc_valid(const uint8_t* report, uint16_t len, uint8_t transaction) { + uint32_t crc = uni_crc32_le(0xffffffff, &transaction, 1); + crc = ~uni_crc32_le(crc, report, len - 4); + const uint8_t* expected = &report[len - 4]; + return crc == ((uint32_t)expected[0] | ((uint32_t)expected[1] << 8) | + ((uint32_t)expected[2] << 16) | ((uint32_t)expected[3] << 24)); +} +#endif static void ds5_send_output_report(uni_hid_device_t* d, ds5_output_report_t* out); +static void ds5_prepare_output_report(uni_hid_device_t* d, ds5_output_report_t* out); static void ds5_send_enable_lightbar_report(uni_hid_device_t* d); static void ds5_request_pairing_info_report(uni_hid_device_t* d); static void ds5_request_firmware_version_report(uni_hid_device_t* d); @@ -390,6 +408,9 @@ void uni_hid_parser_ds5_init_report(uni_hid_device_t* d) { uni_controller_t* ctl = &d->controller; +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_begin(d); +#endif memset(ctl, 0, sizeof(*ctl)); ctl->klass = UNI_CONTROLLER_CLASS_GAMEPAD; @@ -406,6 +427,9 @@ void uni_hid_parser_ds5_setup(uni_hid_device_t* d) { ds5_instance_t* ins = get_ds5_instance(d); memset(ins, 0, sizeof(*ins)); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_reset(d); +#endif // Default values for Accel / Gyro calibration data, until calibration is supported. for (size_t i = 0; i < ARRAY_SIZE(ins->accel_calib_data); i++) { @@ -423,10 +447,19 @@ void uni_hid_parser_ds5_parse_feature_report(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { ds5_instance_t* ins = get_ds5_instance(d); +#if SWITCH2_BRIDGE_FULL_INPUT + if (report == NULL || len == 0) + return; +#endif uint8_t report_id = report[0]; switch (report_id) { case DS5_FEATURE_REPORT_PAIRING_INFO: +#if SWITCH2_BRIDGE_FULL_INPUT + if (len != DS5_FEATURE_REPORT_PAIRING_INFO_SIZE || + !ds5_bridge_crc_valid(report, len, 0xa3)) + break; +#endif if (len != DS5_FEATURE_REPORT_PAIRING_INFO_SIZE) { loge("DS5: Unexpected pairing info size: got %d, want: %d\n", len, DS5_FEATURE_REPORT_PAIRING_INFO_SIZE); @@ -444,6 +477,11 @@ break; case DS5_FEATURE_REPORT_FIRMWARE_VERSION: { +#if SWITCH2_BRIDGE_FULL_INPUT + if (len != DS5_FEATURE_REPORT_FIRMWARE_VERSION_SIZE || + !ds5_bridge_crc_valid(report, len, 0xa3)) + break; +#endif if (len != DS5_FEATURE_REPORT_FIRMWARE_VERSION_SIZE) { loge("DS5: Unexpected firmware version size: got %d, want: %d\n", len, DS5_FEATURE_REPORT_FIRMWARE_VERSION_SIZE); @@ -476,6 +514,11 @@ int speed_2x; int range_2g; +#if SWITCH2_BRIDGE_FULL_INPUT + if (len != DS5_FEATURE_REPORT_CALIBRATION_SIZE || + !ds5_bridge_crc_valid(report, len, 0xa3)) + break; +#endif if (len != DS5_FEATURE_REPORT_CALIBRATION_SIZE) { loge("DS5: Unexpected calibration size: got %d, want: %d\n", len, DS5_FEATURE_REPORT_CALIBRATION_SIZE); /* fallthrough */ @@ -487,25 +530,39 @@ // Set gyroscope calibration and normalization parameters. // Data values will be normalized to 1/DS_GYRO_RES_PER_DEG_S degree/s. speed_2x = r->gyro_speed_plus + r->gyro_speed_minus; - ins->gyro_calib_data[0].bias = 0; +#if SWITCH2_BRIDGE_FULL_INPUT + ins->calibration_valid = speed_2x > 0; + uni_hid_parser_native_motion_accel(d, NULL); + uni_hid_parser_native_motion_gyro(d, NULL); +#endif + ins->gyro_calib_data[0].bias = r->gyro_pitch_bias; ins->gyro_calib_data[0].sens_numer = speed_2x * DS5_GYRO_RES_PER_DEG_S; ins->gyro_calib_data[0].sens_denom = abs(r->gyro_pitch_plus - r->gyro_pitch_bias) + abs(r->gyro_pitch_minus + r->gyro_pitch_bias); - ins->gyro_calib_data[1].bias = 0; + ins->gyro_calib_data[1].bias = r->gyro_yaw_bias; ins->gyro_calib_data[1].sens_numer = speed_2x * DS5_GYRO_RES_PER_DEG_S; ins->gyro_calib_data[1].sens_denom = abs(r->gyro_yaw_plus - r->gyro_yaw_bias) + abs(r->gyro_yaw_minus - r->gyro_yaw_bias); - ins->gyro_calib_data[2].bias = 0; + ins->gyro_calib_data[2].bias = r->gyro_roll_bias; ins->gyro_calib_data[2].sens_numer = speed_2x * DS5_GYRO_RES_PER_DEG_S; ins->gyro_calib_data[2].sens_denom = abs(r->gyro_roll_plus - r->gyro_roll_bias) + abs(r->gyro_roll_minus - r->gyro_roll_bias); +#if SWITCH2_BRIDGE_FULL_INPUT + // Both extrema are measured relative to the same factory bias. + ins->gyro_calib_data[0].sens_denom = + abs(r->gyro_pitch_plus - r->gyro_pitch_bias) + abs(r->gyro_pitch_minus - r->gyro_pitch_bias); +#endif // Sanity check gyro calibration data. This is needed to prevent crashes // during report handling of virtual, clone or broken devices not implementing // calibration data properly. for (size_t i = 0; i < ARRAY_SIZE(ins->gyro_calib_data); i++) { +#if SWITCH2_BRIDGE_FULL_INPUT + if (ins->gyro_calib_data[i].sens_denom <= 0) + ins->calibration_valid = false; +#endif if (ins->gyro_calib_data[i].sens_denom == 0) { loge("Invalid gyro calibration data for axis (%d), disabling calibration for axis = %d\n", i); ins->gyro_calib_data[i].bias = 0; @@ -535,6 +592,10 @@ // during report handling of virtual, clone or broken devices not implementing calibration // data properly. for (size_t i = 0; i < ARRAY_SIZE(ins->accel_calib_data); i++) { +#if SWITCH2_BRIDGE_FULL_INPUT + if (ins->accel_calib_data[i].sens_denom <= 0) + ins->calibration_valid = false; +#endif if (ins->accel_calib_data[i].sens_denom == 0) { loge("Invalid accelerometer calibration data for axis (%d), disabling calibration for axis=%d\n", i); @@ -557,6 +618,11 @@ void uni_hid_parser_ds5_parse_input_report(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { ds5_instance_t* ins = get_ds5_instance(d); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_begin(d); + if (report == NULL || len != 78 || !ds5_bridge_crc_valid(report, len, 0xa1)) + return; +#endif // Don't process reports until state is ready. Prevents possible div-by-0 on calibration // and ignores other warnings. if (ins->state != DS5_STATE_READY) @@ -617,12 +683,12 @@ ctl->gamepad.buttons |= BUTTON_THUMB_R; // Thumb R if (r->buttons[2] & 0x01) ctl->gamepad.misc_buttons |= MISC_BUTTON_SYSTEM; // PS - if (r->buttons[2] & 0x04) - ctl->gamepad.misc_buttons |= MISC_BUTTON_CAPTURE; // "mute" button + if (r->buttons[2] & 0x02) + ctl->gamepad.misc_buttons |= MISC_BUTTON_CAPTURE; // Touchpad click // Gyro for (size_t i = 0; i < ARRAY_SIZE(r->gyro); i++) { - int32_t raw_data = (int16_t)r->gyro[i]; + int32_t raw_data = (int16_t)r->gyro[i] - ins->gyro_calib_data[i].bias; int32_t calib_data = mult_frac(ins->gyro_calib_data[i].sens_numer, raw_data, ins->gyro_calib_data[i].sens_denom); ctl->gamepad.gyro[i] = calib_data; @@ -630,12 +696,21 @@ // Accel for (size_t i = 0; i < ARRAY_SIZE(r->accel); i++) { - int32_t raw_data = (int16_t)r->accel[i]; + int32_t raw_data = (int16_t)r->accel[i] - ins->accel_calib_data[i].bias; int32_t calib_data = mult_frac(ins->accel_calib_data[i].sens_numer, raw_data, ins->accel_calib_data[i].sens_denom); ctl->gamepad.accel[i] = calib_data; } +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_accept(d); + uni_hid_parser_native_motion_accel(d, NULL); + uni_hid_parser_native_motion_gyro(d, NULL); + if (uni_hid_parser_native_motion_fresh(d, r->sensor_timestamp, UINT32_MAX) && ins->calibration_valid) { + uni_hid_parser_native_motion_accel(d, ctl->gamepad.accel); + uni_hid_parser_native_motion_gyro(d, ctl->gamepad.gyro); + } +#endif // Value goes from 0 to 10. Make it from 0 to 250. // The +1 is to avoid having a value of 0, which means "battery unavailable". ctl->battery = (r->status & DS5_STATUS_BATTERY_CAPACITY) * 25 + 1; @@ -721,6 +796,68 @@ } } +#if SWITCH2_BRIDGE_FULL_INPUT +static void on_ds5_bridge_rumble_off(btstack_timer_source_t* timer) { + uni_hid_device_t* d = timer->context; + ds5_instance_t* ins = get_ds5_instance(d); + if (uni_hid_parser_ds5_bridge_rumble(d, 0, 0, 0)) + return; + if ((int32_t)(btstack_run_loop_get_time_ms() - ins->bridge_stop_deadline_ms) >= 0) { + // A stuck output path must not leave a live vibrating connection. + // Disconnect preserves its bond; lifecycle teardown retires this timer. + uni_hid_device_disconnect(d); + return; + } + btstack_run_loop_set_timer(timer, 5); + btstack_run_loop_add_timer(timer); +} + +bool uni_hid_parser_ds5_bridge_rumble(uni_hid_device_t* d, uint16_t duration_ms, + uint8_t right, uint8_t left) { + if (d == NULL || d->report_parser.setup != uni_hid_parser_ds5_setup || duration_ms > 1000) + return false; + ds5_instance_t* ins = get_ds5_instance(d); + if (ins->state != DS5_STATE_READY || d->conn.interrupt_cid == 0 || + !l2cap_can_send_packet_now(d->conn.interrupt_cid)) + return false; + ds5_output_report_t out = { + .valid_flag0 = DS5_FLAG0_HAPTICS_SELECT, + .motor_right = duration_ms == 0 ? 0 : right, + .motor_left = duration_ms == 0 ? 0 : left, + }; + if (ins->use_vibration2) + out.valid_flag2 |= DS5_FLAG2_COMPATIBLE_VIBRATION2; + else + out.valid_flag0 |= DS5_FLAG0_COMPATIBLE_VIBRATION; + ds5_prepare_output_report(d, &out); + // Never put a timed ON packet in the generic FIFO. A later retry evaluates + // the current cue phase; it cannot drain a backlog of expired vibration. + if (l2cap_send(d->conn.interrupt_cid, (uint8_t*)&out, sizeof(out)) != ERROR_CODE_SUCCESS) + return false; + btstack_run_loop_remove_timer(&ins->rumble_timer_delayed_start); + btstack_run_loop_remove_timer(&ins->rumble_timer_duration); + ins->rumble_state = duration_ms == 0 ? DS5_STATE_RUMBLE_DISABLED : DS5_STATE_RUMBLE_IN_PROGRESS; + if (duration_ms != 0) { + ins->bridge_stop_deadline_ms = btstack_run_loop_get_time_ms() + duration_ms + 2000; + ins->rumble_timer_duration.context = d; + ins->rumble_timer_duration.process = on_ds5_bridge_rumble_off; + btstack_run_loop_set_timer(&ins->rumble_timer_duration, duration_ms); + btstack_run_loop_add_timer(&ins->rumble_timer_duration); + } + return true; // Source transport submission, never an application ACK. +} + +void uni_hid_parser_ds5_bridge_teardown(uni_hid_device_t* d) { + if (d == NULL || d->report_parser.setup != uni_hid_parser_ds5_setup) + return; + ds5_instance_t* ins = get_ds5_instance(d); + btstack_run_loop_remove_timer(&ins->rumble_timer_delayed_start); + btstack_run_loop_remove_timer(&ins->rumble_timer_duration); + ins->rumble_state = DS5_STATE_RUMBLE_DISABLED; + uni_hid_parser_native_motion_forget(d); +} +#endif + void uni_hid_parser_ds5_device_dump(uni_hid_device_t* d) { ds5_instance_t* ins = get_ds5_instance(d); logi("\tDS5: FW version: %#x, HW version: %#x, update version: %#x, use vibration2: %d\n", ins->fw_version, @@ -734,7 +871,7 @@ return (ds5_instance_t*)&d->parser_data[0]; } -static void ds5_send_output_report(uni_hid_device_t* d, ds5_output_report_t* out) { +static void ds5_prepare_output_report(uni_hid_device_t* d, ds5_output_report_t* out) { ds5_instance_t* ins = get_ds5_instance(d); out->transaction_type = (HID_MESSAGE_TYPE_DATA << 4) | HID_REPORT_TYPE_OUTPUT; @@ -748,7 +885,10 @@ ins->output_seq = 0; out->crc32 = ~uni_crc32_le(0xffffffff, (uint8_t*)out, sizeof(*out) - 4); - +} + +static void ds5_send_output_report(uni_hid_device_t* d, ds5_output_report_t* out) { + ds5_prepare_output_report(d, out); uni_hid_device_send_intr_report(d, (uint8_t*)out, sizeof(*out)); } diff --git a/src/components/bluepad32/parser/uni_hid_parser_psmove.c b/src/components/bluepad32/parser/uni_hid_parser_psmove.c --- a/src/components/bluepad32/parser/uni_hid_parser_psmove.c +++ b/src/components/bluepad32/parser/uni_hid_parser_psmove.c @@ -8,6 +8,10 @@ */ #include "parser/uni_hid_parser_psmove.h" +#include "parser/uni_hid_parser_imu.h" +#if SWITCH2_BRIDGE_FULL_INPUT +#include "parser/uni_hid_parser_native_motion.h" +#endif #include @@ -27,11 +31,6 @@ PSMOVE_FSM_LED_UPDATED, // LED updated } psmove_fsm_t; -typedef enum psmove_model { - PSMOVE_MODEL_UNK, - PSMOVE_MODEL_ZCM1, - PSMOVE_MODEL_ZCM2, -} psmove_model_t; typedef enum { PSMOVE_STATE_RUMBLE_DISABLED, @@ -41,9 +40,10 @@ // psmove_instance_t represents data used by the psmove driver instance. typedef struct psmove_instance_s { - psmove_model_t model; + uni_psmove_imu_model_t model; psmove_fsm_t state; uint8_t led_rgb[3]; + uni_psmove_imu_calibration_t imu_calibration; btstack_timer_source_t rumble_timer_duration; btstack_timer_source_t rumble_timer_delayed_start; @@ -127,8 +127,12 @@ static void on_psmove_set_rumble_on(btstack_timer_source_t* ts); static void on_psmove_set_rumble_off(btstack_timer_source_t* ts); static void psmove_play_dual_rumble_now(uni_hid_device_t* d, uint16_t duration_ms, uint8_t magnitude); +static void psmove_request_calibration_report(uni_hid_device_t* d); void uni_hid_parser_psmove_init_report(uni_hid_device_t* d) { +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_begin(d); +#endif uni_controller_t* ctl = &d->controller; memset(ctl, 0, sizeof(*ctl)); @@ -136,6 +140,12 @@ } void uni_hid_parser_psmove_parse_input_report(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_begin(d); + // Both model-specific layouts carry the low clock byte at offset 43. + if (!report || len < sizeof(psmove_input_zcm2_report_t)) + return; +#endif // printf_hexdump(report, len); // FIXME: parse ZCM1 / ZCM2 parts. For the moment only the "common" is being parsed. @@ -154,6 +164,12 @@ } uni_controller_t* ctl = &d->controller; + psmove_instance_t* ins = get_psmove_instance(d); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_accept(d); + bool fresh = uni_hid_parser_native_motion_fresh( + d, ((uint32_t)r->time_high << 8) | report[43], UINT16_MAX); +#endif // Buttons if (r->buttons[0] & 0x01) @@ -187,16 +203,50 @@ ctl->gamepad.throttle = r->trigger * 4; - ctl->gamepad.accel[0] = r->accel_x; - ctl->gamepad.accel[1] = r->accel_y; - ctl->gamepad.accel[2] = r->accel_z; - - ctl->gamepad.gyro[0] = r->gyro_x; - ctl->gamepad.gyro[1] = r->gyro_y; - ctl->gamepad.gyro[2] = r->gyro_z; + const uint16_t accel_first[3] = {r->accel_x, r->accel_y, r->accel_z}; + const uint16_t accel_second[3] = { + r->accel_x2, r->accel_y2, r->accel_z2}; + const uint16_t gyro_first[3] = {r->gyro_x, r->gyro_y, r->gyro_z}; + const uint16_t gyro_second[3] = { + r->gyro_x2, r->gyro_y2, r->gyro_z2}; + uni_imu_fixed_sample_t motion; + if (uni_psmove_normalize_imu( + ins->model, &ins->imu_calibration, accel_first, accel_second, + gyro_first, gyro_second, &motion)) { + memcpy(ctl->gamepad.accel, motion.accel, sizeof(motion.accel)); + memcpy(ctl->gamepad.gyro, motion.gyro, sizeof(motion.gyro)); +#if SWITCH2_BRIDGE_FULL_INPUT + if (fresh) { + uni_hid_parser_native_motion_accel(d, motion.accel); + uni_hid_parser_native_motion_gyro(d, motion.gyro); + } +#endif + } if (r->battery <= 5) ctl->battery = r->battery * 51; +} + +void uni_hid_parser_psmove_parse_feature_report( + uni_hid_device_t* d, const uint8_t* report, uint16_t len) { + psmove_instance_t* ins = get_psmove_instance(d); + const uni_psmove_calibration_result_t result = + uni_psmove_add_calibration_report( + &ins->imu_calibration, ins->model, report, len); +#if SWITCH2_BRIDGE_FULL_INPUT + // A changed calibration bank is not itself a new physical sample. + if (result != UNI_PSMOVE_CALIBRATION_INVALID) { + uni_hid_parser_native_motion_accel(d, NULL); + uni_hid_parser_native_motion_gyro(d, NULL); + } +#endif + if (result == UNI_PSMOVE_CALIBRATION_INCOMPLETE) { + psmove_request_calibration_report(d); + } else if (result == UNI_PSMOVE_CALIBRATION_COMPLETE) { + logi("psmove: IMU calibration ready\n"); + } else if (result == UNI_PSMOVE_CALIBRATION_INVALID) { + loge("psmove: invalid IMU calibration; motion disabled\n"); + } } void uni_hid_parser_psmove_play_dual_rumble(struct uni_hid_device_s* d, @@ -258,28 +308,40 @@ void uni_hid_parser_psmove_setup(struct uni_hid_device_s* d) { psmove_instance_t* ins = get_psmove_instance(d); memset(ins, 0, sizeof(*ins)); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_reset(d); +#endif switch (d->product_id) { case ZCM1_PID: - ins->model = PSMOVE_MODEL_ZCM1; + ins->model = UNI_PSMOVE_IMU_MODEL_ZCM1; logi("psmove: Detected ZCM1 model\n"); break; case ZCM2_PID: - ins->model = PSMOVE_MODEL_ZCM2; + ins->model = UNI_PSMOVE_IMU_MODEL_ZCM2; logi("psmove: Detected ZCM2 model\n"); break; default: - loge("psmove: Unknown PSMove PID = %#x, assuming ZCM1\n", ins->model); - ins->model = PSMOVE_MODEL_ZCM1; - break; - } - + loge("psmove: Unknown PSMove PID = %#x, assuming ZCM1\n", d->product_id); + ins->model = UNI_PSMOVE_IMU_MODEL_ZCM1; + break; + } + + psmove_request_calibration_report(d); uni_hid_device_set_ready_complete(d); } // // Helpers // +static void psmove_request_calibration_report(uni_hid_device_t* d) { + static const uint8_t report[] = { + ((HID_MESSAGE_TYPE_GET_REPORT << 4) | HID_REPORT_TYPE_FEATURE), + 0x10, + }; + uni_hid_device_send_ctrl_report(d, report, sizeof(report)); +} + static psmove_instance_t* get_psmove_instance(uni_hid_device_t* d) { return (psmove_instance_t*)&d->parser_data[0]; } diff --git a/src/components/bluepad32/parser/uni_hid_parser_switch.c b/src/components/bluepad32/parser/uni_hid_parser_switch.c --- a/src/components/bluepad32/parser/uni_hid_parser_switch.c +++ b/src/components/bluepad32/parser/uni_hid_parser_switch.c @@ -7,6 +7,9 @@ // https://github.com/DanielOgorchock/linux/blob/ogorchock/drivers/hid/hid-nintendo.c #include "parser/uni_hid_parser_switch.h" +#if SWITCH2_BRIDGE_FULL_INPUT +#include "parser/uni_hid_parser_native_motion.h" +#endif #include @@ -51,13 +54,22 @@ static const int16_t DEFAULT_ACCEL_SCALE = 16384; static const int16_t DEFAULT_GYRO_OFFSET = 0; static const int16_t DEFAULT_GYRO_SCALE = 13371; -#define SWITCH_IMU_PREC_RANGE_SCALE 1000 +#define SWITCH_IMU_GYRO_RES_PER_DEG_S 1024 +#define SWITCH_IMU_ACCEL_RES_PER_G 8192 #define SWITCH_FACTORY_IMU_CAL_DATA_SIZE 24 static const uint16_t SWITCH_FACTORY_IMU_CAL_DATA_ADDR = 0x6020; #define SWITCH_DUMP_ROM_DATA_SIZE 24 // Max size is 24 #define SWITCH_SETUP_TIMEOUT_MS 800 +#define SWITCH_RUMBLE_REFRESH_MS 40 +#define SWITCH_RUMBLE_LOW_FREQUENCY_HZ 453 +#define SWITCH_RUMBLE_LEFT_HIGH_FREQUENCY_HZ 135 +#define SWITCH_RUMBLE_RIGHT_HIGH_FREQUENCY_HZ 99 +#define SWITCH_RUMBLE_AMPLITUDE_MAX 1003 +#define SWITCH_RUMBLE_MAGNITUDE_MAX UINT8_MAX +#define SWITCH_RUMBLE_AMPLITUDE_ROUNDING_BIAS \ + (SWITCH_RUMBLE_MAGNITUDE_MAX / 2) #if ENABLE_SPI_FLASH_DUMP static const uint32_t SWITCH_DUMP_ROM_DATA_ADDR_START = 0x20000; static const uint32_t SWITCH_DUMP_ROM_DATA_ADDR_END = 0x30000; @@ -72,6 +84,7 @@ STATE_READ_FACTORY_IMU_CALIBRATION, // Factory IMU calibration info STATE_SET_FULL_REPORT, // Request report 0x30 STATE_ENABLE_IMU, // Enable/Disable gyro/accel + STATE_ENABLE_RUMBLE, // Enable controller vibration STATE_DUMP_FLASH, // Dump SPI Flash memory STATE_UPDATE_LED, // Update LEDs STATE_READY, // Gamepad setup ready! @@ -111,6 +124,7 @@ SUBCMD_SPI_FLASH_READ = 0x10, SUBCMD_SET_PLAYER_LEDS = 0x30, SUBCMD_ENABLE_IMU = 0x40, + SUBCMD_ENABLE_RUMBLE = 0x48, }; typedef enum { @@ -137,7 +151,12 @@ // Although technically, we can use one timer for delay and duration, easier to debug/maintain if we have two. btstack_timer_source_t rumble_timer_duration; btstack_timer_source_t rumble_timer_delayed_start; + btstack_timer_source_t rumble_timer_refresh; switch_state_rumble_t rumble_state; + bool native_owned; + bool device_info_valid; + uint8_t packet_num; + uint8_t effective_rumble[8]; btstack_timer_source_t setup_timer; @@ -164,6 +183,9 @@ int32_t imu_cal_accel_divisor[3]; int32_t imu_cal_gyro_divisor[3]; +#if SWITCH2_BRIDGE_FULL_INPUT + bool accel_calibrated, gyro_calibrated; +#endif // Debug only int debug_fd; // File descriptor where dump is saved @@ -322,6 +344,7 @@ static void fsm_read_factory_imu_calibration(struct uni_hid_device_s* d); static void fsm_set_full_report(struct uni_hid_device_s* d); static void fsm_enable_imu(struct uni_hid_device_s* d); +static void fsm_enable_rumble(struct uni_hid_device_s* d); static void fsm_update_led(struct uni_hid_device_s* d); static void fsm_ready(struct uni_hid_device_s* d); static void process_reply_read_spi_dump(struct uni_hid_device_s* d, const uint8_t* data, int len); @@ -333,23 +356,36 @@ static void process_reply_spi_flash_read(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len); static void process_reply_set_player_leds(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len); static void process_reply_enable_imu(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len); +static void process_reply_enable_rumble(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len); static int32_t calibrate_axis(int32_t v, switch_cal_stick_t cal); static void set_led(uni_hid_device_t* d, uint8_t leds); static void on_switch_set_rumble_on(btstack_timer_source_t* ts); static void on_switch_set_rumble_off(btstack_timer_source_t* ts); +static void on_switch_refresh_rumble(btstack_timer_source_t* ts); static void switch_stop_rumble_now(uni_hid_device_t* d); +static void switch_send_dual_rumble_now(uni_hid_device_t* d, + uint8_t weak_magnitude, + uint8_t strong_magnitude); static void switch_play_dual_rumble_now(uni_hid_device_t* d, uint16_t duration_ms, uint8_t weak_magnitude, uint8_t strong_magnitude); static void switch_setup_timeout_callback(btstack_timer_source_t* ts); static void parse_stick_calibration(switch_cal_stick_t* x, switch_cal_stick_t* y, const uint8_t* data, bool is_left); +static const uint8_t switch_neutral_rumble[8] = { + 0x00, 0x01, 0x40, 0x40, 0x00, 0x01, 0x40, 0x40, +}; +static void switch_cancel_rumble_timers(switch_instance_t* ins); void uni_hid_parser_switch_setup(struct uni_hid_device_s* d) { switch_instance_t* ins = get_switch_instance(d); memset(ins, 0, sizeof(*ins)); - +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_reset(d); +#endif + + memcpy(ins->effective_rumble, switch_neutral_rumble, sizeof(ins->effective_rumble)); ins->state = STATE_SETUP; ins->mode = SWITCH_MODE_NONE; // In case the controller doesn't answer to SUBCMD_REQ_DEV_INFO command, set a default one. @@ -389,11 +425,19 @@ } void uni_hid_parser_switch_init_report(uni_hid_device_t* d) { +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_begin(d); +#endif ARG_UNUSED(d); // Nothing } void uni_hid_parser_switch_parse_input_report(struct uni_hid_device_s* d, const uint8_t* report, uint16_t len) { +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_begin(d); + if (!report) + return; +#endif if (len < 12) { loge("Nintendo Switch: Invalid packet len; got %d, want >= 12\n", len); return; @@ -408,6 +452,9 @@ break; case SWITCH_INPUT_BUTTON_EVENT: parse_report_3f(d, report, len); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_accept(d); +#endif break; default: loge("Nintendo Switch: unsupported report id: 0x%02x\n", report[0]); @@ -451,6 +498,10 @@ break; case STATE_ENABLE_IMU: logd("STATE_ENABLE_IMU\n"); + fsm_enable_rumble(d); + break; + case STATE_ENABLE_RUMBLE: + logd("STATE_ENABLE_RUMBLE\n"); fsm_dump_rom(d); break; case STATE_DUMP_FLASH: @@ -619,6 +670,11 @@ SWITCH_FACTORY_IMU_CAL_DATA_SIZE); return; } +#if SWITCH2_BRIDGE_FULL_INPUT + ins->accel_calibrated = ins->gyro_calibrated = true; + uni_hid_parser_native_motion_accel(d, NULL); + uni_hid_parser_native_motion_gyro(d, NULL); +#endif for (int i = 0; i < 3; i++) { int j = i * 2; @@ -635,6 +691,12 @@ // FIXME: move to its own function ins->imu_cal_accel_divisor[i] = ins->cal_accel.scale[i] - ins->cal_accel.offset[i]; ins->imu_cal_gyro_divisor[i] = ins->cal_gyro.scale[i] - ins->cal_gyro.offset[i]; +#if SWITCH2_BRIDGE_FULL_INPUT + if (ins->imu_cal_accel_divisor[i] <= 0) + ins->accel_calibrated = false; + if (ins->imu_cal_gyro_divisor[i] <= 0) + ins->gyro_calibrated = false; +#endif } logi( @@ -648,7 +710,8 @@ // Reply to SUBCMD_REQ_DEV_INFO static void process_reply_req_dev_info(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len) { - ARG_UNUSED(len); + if (len < (int)sizeof(*r) + 3 || !(r->ack & 0x80)) + return; switch_instance_t* ins = get_switch_instance(d); if (ins->state > STATE_SETUP && ins->mode == SWITCH_MODE_NONE) { bool enable_imu; @@ -669,6 +732,7 @@ ins->firmware_version_hi = r->data[0]; ins->firmware_version_lo = r->data[1]; ins->controller_type = r->data[2]; + ins->device_info_valid = true; logi("Switch: Firmware version: %d.%d. Controller type=%d\n", r->data[0], r->data[1], r->data[2]); } @@ -725,6 +789,12 @@ ARG_UNUSED(r); ARG_UNUSED(len); } +static void process_reply_enable_rumble(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len) { + ARG_UNUSED(d); + ARG_UNUSED(r); + ARG_UNUSED(len); +} + // Process 0x21 input report: SWITCH_INPUT_SUBCMD_REPLY static void process_input_subcmd_reply(struct uni_hid_device_s* d, const uint8_t* report, int len) { @@ -733,6 +803,23 @@ // 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 // 00 const struct switch_report_21_s* r = (const struct switch_report_21_s*)report; + if (len < (int)sizeof(*r)) + return; +#if SWITCH2_BRIDGE_FULL_INPUT + if (!(r->ack & 0x80)) + return; + if (r->subcmd_id == SUBCMD_REQ_DEV_INFO && len < (int)sizeof(*r) + 3) + return; + if (r->subcmd_id == SUBCMD_SPI_FLASH_READ) { + if (len < (int)sizeof(*r) + 5 || len < (int)sizeof(*r) + 5 + r->data[4]) + return; + // An unrelated/stale SPI response cannot certify IMU calibration. + if (get_switch_instance(d)->state == STATE_READ_FACTORY_IMU_CALIBRATION && + (r->data[4] != SWITCH_FACTORY_IMU_CAL_DATA_SIZE || + little_endian_read_32(r->data, 0) != SWITCH_FACTORY_IMU_CAL_DATA_ADDR)) + return; + } +#endif if ((r->ack & 0b10000000) == 0) { loge("Switch: Error, subcommand id=0x%02x was not successful.\n", r->subcmd_id); } @@ -751,6 +838,9 @@ break; case SUBCMD_ENABLE_IMU: process_reply_enable_imu(d, r, len); + break; + case SUBCMD_ENABLE_RUMBLE: + process_reply_enable_rumble(d, r, len); break; default: loge("Switch: Error, unexpected subcmd_id=0x%02x in report 0x21\n", r->subcmd_id); @@ -823,19 +913,26 @@ switch_instance_t* ins = get_switch_instance(d); uni_controller_t* ctl = &d->controller; - int accel[3]; - int gyro[3]; + int32_t accel[3]; + int32_t gyro[3]; for (int i = 0; i < 3; i++) { - if (ins->imu_cal_accel_divisor[i] == 0) - accel[i] = r->accel[i]; - else - accel[i] = (r->accel[i] * ins->cal_accel.scale[i]) / ins->imu_cal_accel_divisor[i]; - gyro[i] = mult_frac((SWITCH_IMU_PREC_RANGE_SCALE * (r->gyro[i] - ins->cal_gyro.offset[i])), - ins->cal_gyro.scale[i], ins->imu_cal_gyro_divisor[i]); - } - - // Right joycon has Y and Z axes negated. + if (ins->imu_cal_accel_divisor[i] == 0) { + accel[i] = r->accel[i] * 2; + } else { + accel[i] = mult_frac(r->accel[i], 4 * SWITCH_IMU_ACCEL_RES_PER_G, ins->imu_cal_accel_divisor[i]); + } + + if (ins->imu_cal_gyro_divisor[i] == 0) { + gyro[i] = mult_frac(r->gyro[i], 936 * SWITCH_IMU_GYRO_RES_PER_DEG_S, DEFAULT_GYRO_SCALE); + } else { + gyro[i] = mult_frac(r->gyro[i] - ins->cal_gyro.offset[i], + 936 * SWITCH_IMU_GYRO_RES_PER_DEG_S, + ins->imu_cal_gyro_divisor[i]); + } + } + + // Right Joy-Con has native Y and Z axes negated. if (ins->controller_type == SWITCH_CONTROLLER_TYPE_JCR) { accel[1] = -accel[1]; accel[2] = -accel[2]; @@ -843,10 +940,13 @@ gyro[2] = -gyro[2]; } - for (int i = 0; i < 3; i++) { - ctl->gamepad.accel[i] = accel[i]; - ctl->gamepad.gyro[i] = gyro[i]; - } + // Match SDL3's PlayStation-oriented sensor coordinate convention. + ctl->gamepad.accel[0] = -accel[1]; + ctl->gamepad.accel[1] = accel[2]; + ctl->gamepad.accel[2] = -accel[0]; + ctl->gamepad.gyro[0] = -gyro[1]; + ctl->gamepad.gyro[1] = gyro[2]; + ctl->gamepad.gyro[2] = -gyro[0]; } // Process 0x30 input report: SWITCH_INPUT_IMU_DATA @@ -856,6 +956,10 @@ // 9D FF 72 FD 01 00 72 10 35 00 C1 FF 9B FF 75 FD FF FF 6C 10 34 00 C2 FF // 9A FF +#if SWITCH2_BRIDGE_FULL_INPUT + if (len < 3 + (int)sizeof(struct switch_report_30_s)) + return; +#endif ARG_UNUSED(len); switch_instance_t* ins = get_switch_instance(d); @@ -887,6 +991,14 @@ // We just take the latest one. If it is not accurate enough, we can average them. if (ins->mode == SWITCH_MODE_IMU) parse_imu(d, &r->imu[2]); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_accept(d); + if (ins->mode == SWITCH_MODE_IMU && ins->controller_type != SWITCH_CONTROLLER_TYPE_SNES && + uni_hid_parser_native_motion_fresh(d, report[1], UINT8_MAX)) { + uni_hid_parser_native_motion_accel(d, ins->accel_calibrated ? ctl->gamepad.accel : NULL); + uni_hid_parser_native_motion_gyro(d, ins->gyro_calibrated ? ctl->gamepad.gyro : NULL); + } +#endif } // Shared both by Switch Pro Controller and Switch SNES. @@ -1172,6 +1284,18 @@ req->data[0] = (ins->mode == SWITCH_MODE_IMU); send_subcmd(d, req, sizeof(out)); } +static void fsm_enable_rumble(struct uni_hid_device_s* d) { + switch_instance_t* ins = get_switch_instance(d); + ins->state = STATE_ENABLE_RUMBLE; + + uint8_t out[sizeof(struct switch_subcmd_request) + 1] = {0}; + struct switch_subcmd_request* req = (struct switch_subcmd_request*)&out[0]; + req->report_id = OUTPUT_RUMBLE_AND_SUBCMD; + req->subcmd_id = SUBCMD_ENABLE_RUMBLE; + req->data[0] = 0x01; + send_subcmd(d, req, sizeof(out)); +} + static void fsm_update_led(struct uni_hid_device_s* d) { switch_instance_t* ins = get_switch_instance(d); @@ -1203,6 +1327,13 @@ return rumble_freqs[i]; } +static uint16_t switch_magnitude_to_amp(uint8_t magnitude) { + return (uint16_t)( + ((uint32_t)magnitude * SWITCH_RUMBLE_AMPLITUDE_MAX + + SWITCH_RUMBLE_AMPLITUDE_ROUNDING_BIAS) / + SWITCH_RUMBLE_MAGNITUDE_MAX); +} + static struct switch_rumble_amp_data find_rumble_amp(uint16_t amp) { unsigned int i = 0; if (amp > rumble_amps[0].amp) { @@ -1253,12 +1384,15 @@ } switch_instance_t* ins = get_switch_instance(d); + if (ins->native_owned || ins->state == STATE_UNINIT) + return; switch (ins->rumble_state) { case SWITCH_STATE_RUMBLE_DELAYED: btstack_run_loop_remove_timer(&ins->rumble_timer_delayed_start); break; case SWITCH_STATE_RUMBLE_IN_PROGRESS: btstack_run_loop_remove_timer(&ins->rumble_timer_duration); + btstack_run_loop_remove_timer(&ins->rumble_timer_refresh); break; default: // Do nothing @@ -1338,12 +1472,160 @@ } static void send_subcmd(uni_hid_device_t* d, struct switch_subcmd_request* r, int len) { - static uint8_t packet_num = 0; - r->packet_num = packet_num++; - if (packet_num > 0x0f) - packet_num = 0; + // Counter and effective rumble are refreshed by the device-send hook at + // actual submission, not here: this report may spend time in the queue. r->transaction_type = (HID_MESSAGE_TYPE_DATA << 4) | HID_REPORT_TYPE_OUTPUT; uni_hid_device_send_intr_report(d, (const uint8_t*)r, len); +} + +static void switch_cancel_rumble_timers(switch_instance_t* ins) { + btstack_run_loop_remove_timer(&ins->rumble_timer_duration); + btstack_run_loop_remove_timer(&ins->rumble_timer_delayed_start); + btstack_run_loop_remove_timer(&ins->rumble_timer_refresh); + ins->rumble_timer_duration.context = NULL; + ins->rumble_timer_delayed_start.context = NULL; + ins->rumble_timer_refresh.context = NULL; + ins->rumble_state = SWITCH_STATE_RUMBLE_DISABLED; +} + +// Preserve queued setup/LED subcommands, but retire every old rumble writer +// across ownership changes. Compact in place with a fixed queue-size bound. +static void switch_discard_queued_rumble(uni_hid_device_t* d) { + uni_circular_buffer_t* queue = &d->outgoing_buffer; + int read_idx = queue->head_idx; + int write_idx = read_idx; + for (int n = 0; n < UNI_CIRCULAR_BUFFER_SIZE && read_idx != queue->tail_idx; ++n) { + const uni_circular_buffer_data_t* entry = &queue->buffer[read_idx]; + const bool rumble = entry->cid == d->conn.interrupt_cid && + entry->data_len >= 2 && entry->data[0] == 0xa2 && + entry->data[1] == OUTPUT_RUMBLE_ONLY; + if (!rumble) { + if (write_idx != read_idx) + queue->buffer[write_idx] = *entry; + write_idx = (write_idx + 1) % UNI_CIRCULAR_BUFFER_SIZE; + } + read_idx = (read_idx + 1) % UNI_CIRCULAR_BUFFER_SIZE; + } + queue->tail_idx = write_idx; +} + +// Both writers use this at the actual L2CAP boundary. Failed sends neither +// consume a counter nor change the effective actuator-word cache. +static int switch_submit_report(uni_hid_device_t* d, uint16_t cid, uint8_t* report, uint16_t len) { + switch_instance_t* ins = get_switch_instance(d); + report[2] = ins->packet_num; + const int status = l2cap_send(cid, report, len); + if (status == ERROR_CODE_SUCCESS) { + ins->packet_num = (ins->packet_num + 1) & 0x0f; + memcpy(ins->effective_rumble, &report[3], sizeof(ins->effective_rumble)); + } + return status; +} + +int uni_hid_parser_switch_send_report(uni_hid_device_t* d, uint16_t cid, + const uint8_t* report, uint16_t len) { + switch_instance_t* ins = get_switch_instance(d); + if (ins->state == STATE_UNINIT) + return ERROR_CODE_SUCCESS; // A retired device must not enqueue work. + if (cid != d->conn.interrupt_cid || len < 11 || report[0] != 0xa2 || + (report[1] != OUTPUT_RUMBLE_ONLY && report[1] != OUTPUT_RUMBLE_AND_SUBCMD)) + return l2cap_send(cid, (uint8_t*)report, len); + if (ins->native_owned && report[1] == OUTPUT_RUMBLE_ONLY) + return ERROR_CODE_SUCCESS; // Only native_send may write this owner. + if (len > UNI_CIRCULAR_BUFFER_DATA_SIZE) + return ERROR_CODE_COMMAND_DISALLOWED; + + uint8_t outgoing[UNI_CIRCULAR_BUFFER_DATA_SIZE]; + memcpy(outgoing, report, len); + if (report[1] == OUTPUT_RUMBLE_AND_SUBCMD) { + // Repeating the SAME last accepted actuator word is Nintendo's hold + // behavior, including compressed words. Never use the queued copy: + // replaying an earlier word after intervening audio reapplies deltas. + // Do not synthesize an absolute endpoint here either: it changes the + // repeated-word history. Physical acceptance is model-qualified. + memcpy(&outgoing[3], ins->effective_rumble, sizeof(ins->effective_rumble)); + if (len >= 13 && outgoing[11] == SUBCMD_SET_PLAYER_LEDS) + outgoing[12] = ins->gamepad_seat & 0x0f; + } + return switch_submit_report(d, cid, outgoing, len); +} + +bool uni_hid_parser_switch_native_info(uni_hid_device_t* d, + uint8_t* type, uint8_t* firmware_hi, + uint8_t* firmware_lo) { + if (!d || d->report_parser.setup != uni_hid_parser_switch_setup) + return false; + switch_instance_t* ins = get_switch_instance(d); + if (ins->state != STATE_READY || !ins->device_info_valid) + return false; + if (type) + *type = ins->controller_type; + if (firmware_hi) + *firmware_hi = ins->firmware_version_hi; + if (firmware_lo) + *firmware_lo = ins->firmware_version_lo; + return true; +} + +bool uni_hid_parser_switch_native_acquire(uni_hid_device_t* d) { + uint8_t type; + if (!uni_hid_parser_switch_native_info(d, &type, NULL, NULL) || + type < SWITCH_CONTROLLER_TYPE_JCL || type > SWITCH_CONTROLLER_TYPE_PRO || + !d->conn.connected || !d->conn.interrupt_cid) + return false; + switch_instance_t* ins = get_switch_instance(d); + switch_cancel_rumble_timers(ins); + switch_discard_queued_rumble(d); + ins->native_owned = true; + return true; +} + +bool uni_hid_parser_switch_native_send(uni_hid_device_t* d, const uint8_t rumble[8]) { + if (!d || !rumble || d->report_parser.setup != uni_hid_parser_switch_setup) + return false; + switch_instance_t* ins = get_switch_instance(d); + if (!ins->native_owned || ins->state != STATE_READY || !d->conn.connected || + !d->conn.interrupt_cid || !l2cap_can_send_packet_now(d->conn.interrupt_cid)) + return false; + uint8_t report[11] = {0xa2, OUTPUT_RUMBLE_ONLY, 0}; + memcpy(&report[3], rumble, 8); + return switch_submit_report(d, d->conn.interrupt_cid, report, sizeof(report)) == ERROR_CODE_SUCCESS; +} + +void uni_hid_parser_switch_native_release(uni_hid_device_t* d) { + if (!d || d->report_parser.setup != uni_hid_parser_switch_setup) + return; + switch_instance_t* ins = get_switch_instance(d); + if (!ins->native_owned) + return; + switch_cancel_rumble_timers(ins); + switch_discard_queued_rumble(d); + ins->native_owned = false; + if (ins->state == STATE_READY && d->conn.connected && d->conn.interrupt_cid && + memcmp(ins->effective_rumble, switch_neutral_rumble, sizeof(ins->effective_rumble)) != 0) { + // A neutral stop may queue under congestion, never an audio command. + // The owner normally stops first; never queue a redundant delayed stop. + struct switch_subcmd_request req = {.report_id = OUTPUT_RUMBLE_ONLY}; + memcpy(req.rumble_left, switch_neutral_rumble, 4); + memcpy(req.rumble_right, &switch_neutral_rumble[4], 4); + send_subcmd(d, &req, 11); + } +} + +void uni_hid_parser_switch_teardown(uni_hid_device_t* d) { + if (!d || d->report_parser.setup != uni_hid_parser_switch_setup) + return; + switch_instance_t* ins = get_switch_instance(d); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_forget(d); +#endif + switch_cancel_rumble_timers(ins); + btstack_run_loop_remove_timer(&ins->setup_timer); + ins->setup_timer.context = NULL; + ins->native_owned = false; + ins->device_info_valid = false; + ins->state = STATE_UNINIT; + uni_circular_buffer_reset(&d->outgoing_buffer); } static int32_t calibrate_axis(int32_t v, switch_cal_stick_t cal) { @@ -1364,8 +1646,9 @@ static void switch_stop_rumble_now(uni_hid_device_t* d) { switch_instance_t* ins = get_switch_instance(d); - // No need to protect it with a mutex since it runs in the same main thread - assert(ins->rumble_state == SWITCH_STATE_RUMBLE_IN_PROGRESS); + if (ins->native_owned || ins->state == STATE_UNINIT) + return; + switch_cancel_rumble_timers(ins); ins->rumble_state = SWITCH_STATE_RUMBLE_DISABLED; struct switch_subcmd_request req = {0}; @@ -1379,11 +1662,31 @@ send_subcmd(d, (struct switch_subcmd_request*)&req, sizeof(req) - 1); } +static void switch_send_dual_rumble_now(uni_hid_device_t* d, + uint8_t weak_magnitude, + uint8_t strong_magnitude) { + struct switch_subcmd_request req = { + .report_id = OUTPUT_RUMBLE_ONLY, + }; + // Fixed frequencies match the standard Switch LRA envelope and the + // 8BitDo Switch-mode implementation. Magnitudes control amplitude only. + switch_encode_rumble(req.rumble_left, SWITCH_RUMBLE_LOW_FREQUENCY_HZ, + SWITCH_RUMBLE_LEFT_HIGH_FREQUENCY_HZ, + switch_magnitude_to_amp(weak_magnitude)); + switch_encode_rumble(req.rumble_right, SWITCH_RUMBLE_LOW_FREQUENCY_HZ, + SWITCH_RUMBLE_RIGHT_HIGH_FREQUENCY_HZ, + switch_magnitude_to_amp(strong_magnitude)); + // Rumble request don't include the last byte of "switch_subcmd_request": subcmd_id + send_subcmd(d, &req, sizeof(req) - 1); +} + static void switch_play_dual_rumble_now(uni_hid_device_t* d, uint16_t duration_ms, uint8_t weak_magnitude, uint8_t strong_magnitude) { switch_instance_t* ins = get_switch_instance(d); + if (ins->native_owned || ins->state == STATE_UNINIT) + return; if (duration_ms == 0) { if (ins->rumble_state != SWITCH_STATE_RUMBLE_DISABLED) @@ -1391,14 +1694,17 @@ return; } - struct switch_subcmd_request req = { - .report_id = OUTPUT_RUMBLE_ONLY, - }; - switch_encode_rumble(req.rumble_left, weak_magnitude << 2, weak_magnitude, 500); - switch_encode_rumble(req.rumble_right, strong_magnitude << 2, strong_magnitude, 500); - - // Rumble request don't include the last byte of "switch_subcmd_request": subcmd_id - send_subcmd(d, &req, sizeof(req) - 1); + ins->rumble_weak_magnitude = weak_magnitude; + ins->rumble_strong_magnitude = strong_magnitude; + switch_send_dual_rumble_now(d, weak_magnitude, strong_magnitude); + + // Refresh active rumble for Switch-compatible controllers that do not + // retain a single output packet, including 8BitDo Switch mode. + ins->rumble_timer_refresh.process = &on_switch_refresh_rumble; + ins->rumble_timer_refresh.context = d; + btstack_run_loop_set_timer(&ins->rumble_timer_refresh, + SWITCH_RUMBLE_REFRESH_MS); + btstack_run_loop_add_timer(&ins->rumble_timer_refresh); // Set timer to turn off rumble ins->rumble_timer_duration.process = &on_switch_set_rumble_off; @@ -1410,19 +1716,52 @@ static void on_switch_set_rumble_on(btstack_timer_source_t* ts) { uni_hid_device_t* d = ts->context; - switch_instance_t* ins = get_switch_instance(d); + if (!d || d->report_parser.setup != uni_hid_parser_switch_setup) + return; + switch_instance_t* ins = get_switch_instance(d); + if (ts != &ins->rumble_timer_delayed_start || ins->native_owned || + ins->rumble_state != SWITCH_STATE_RUMBLE_DELAYED) + return; + ts->context = NULL; switch_play_dual_rumble_now(d, ins->rumble_duration_ms, ins->rumble_weak_magnitude, ins->rumble_strong_magnitude); } +static void on_switch_refresh_rumble(btstack_timer_source_t* ts) { + uni_hid_device_t* d = btstack_run_loop_get_timer_context(ts); + if (!d || d->report_parser.setup != uni_hid_parser_switch_setup) + return; + switch_instance_t* ins = get_switch_instance(d); + if (ts != &ins->rumble_timer_refresh || ins->native_owned || + ins->rumble_state != SWITCH_STATE_RUMBLE_IN_PROGRESS) { + return; + } + switch_send_dual_rumble_now( + d, (uint8_t)ins->rumble_weak_magnitude, + (uint8_t)ins->rumble_strong_magnitude); + btstack_run_loop_set_timer(&ins->rumble_timer_refresh, + SWITCH_RUMBLE_REFRESH_MS); + btstack_run_loop_add_timer(&ins->rumble_timer_refresh); +} + static void on_switch_set_rumble_off(btstack_timer_source_t* ts) { uni_hid_device_t* d = btstack_run_loop_get_timer_context(ts); + if (!d || d->report_parser.setup != uni_hid_parser_switch_setup) + return; + switch_instance_t* ins = get_switch_instance(d); + if (ts != &ins->rumble_timer_duration || ins->native_owned || + ins->rumble_state != SWITCH_STATE_RUMBLE_IN_PROGRESS) + return; switch_stop_rumble_now(d); } void switch_setup_timeout_callback(btstack_timer_source_t* ts) { uni_hid_device_t* d = btstack_run_loop_get_timer_context(ts); - switch_instance_t* ins = get_switch_instance(d); + if (!d || d->report_parser.setup != uni_hid_parser_switch_setup) + return; + switch_instance_t* ins = get_switch_instance(d); + if (ts != &ins->setup_timer || ins->state == STATE_UNINIT) + return; logi("Switch: setup timer timeout, failed state: 0x%02x\n", ins->state); process_fsm(d); } diff --git a/src/components/bluepad32/include/parser/uni_hid_parser_wii.h b/src/components/bluepad32/include/parser/uni_hid_parser_wii.h --- a/src/components/bluepad32/include/parser/uni_hid_parser_wii.h +++ b/src/components/bluepad32/include/parser/uni_hid_parser_wii.h @@ -8,1 +8,2 @@ +#include #include @@ -46,3 +47,17 @@ void uni_hid_parser_wii_request_report_type(struct uni_hid_device_s* d, wii_report_type_t report_type); + +// Calibrated physical acceleration, independent of gyro/IR output selection. +// Sequence advances only on a fresh accelerometer report, never status/acks. +bool uni_hid_parser_wii_accel_snapshot(struct uni_hid_device_s* d, + int32_t accel[3], uint32_t* sequence); +// Independent calibrated MotionPlus stream; status/ACK/Nunchuk never advance it. +bool uni_hid_parser_wii_gyro_snapshot(struct uni_hid_device_s* d, + int32_t gyro[3], uint32_t* sequence); +void uni_hid_parser_wii_teardown(struct uni_hid_device_s* d); +// The void rumble hook ignores commands during topology setup. +bool uni_hid_parser_wii_rumble_ready(struct uni_hid_device_s* d); +// Independent Nunchuk stream; never includes interleaved MotionPlus samples. +bool uni_hid_parser_wii_nunchuk_accel_snapshot(struct uni_hid_device_s* d, + int32_t accel[3], uint32_t* sequence); #endif // UNI_HID_PARSER_WII_H diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/components/bluepad32/parser/uni_hid_parser_wii.c --- a/src/components/bluepad32/parser/uni_hid_parser_wii.c +++ b/src/components/bluepad32/parser/uni_hid_parser_wii.c @@ -5,6 +5,8 @@ // Technical info taken from: // http://wiibrew.org/wiki/Wiimote // https://github.com/dvdhrm/xwiimote/blob/master/doc/PROTOCOL +// https://wiibrew.org/wiki/Wiimote/Extension_Controllers/Wii_Motion_Plus +// Calibration signs: Dolphin WiimoteEmu/MotionPlus.cpp; axes: SDL_hidapi_wii.c. #include #include @@ -19,6 +21,13 @@ #endif // ENABLE_EEPROM_DUMP #include "parser/uni_hid_parser_wii.h" +#include "parser/uni_hid_parser_imu.h" +#if SWITCH2_BRIDGE_FULL_INPUT +#include "parser/uni_hid_parser_native_motion.h" +#endif +#if SWITCH_PICO_WII_IR +#include "parser/uni_hid_parser_wii_ir.h" +#endif #include "controller/uni_controller.h" #include "hid_usage.h" @@ -82,10 +91,23 @@ WII_FSM_EXT_DID_INIT, // Extension initialized WII_FSM_EXT_DID_NO_ENCRYPTION, // Extension no encryption WII_FSM_EXT_DID_READ_REGISTER, // Extension read register + WII_FSM_NUNCHUK_CALIBRATION, + WII_FSM_ACCEL_CALIBRATION, + WII_FSM_ACCEL_CALIBRATION_BACKUP, + WII_FSM_MP_PROBE, + WII_FSM_MP_INIT, + WII_FSM_MP_CALIBRATION_FAST, + WII_FSM_MP_CALIBRATION_SLOW, + WII_FSM_MP_ACTIVATE, + WII_FSM_MP_VERIFY, + WII_FSM_TOPOLOGY_STATUS, WII_FSM_BALANCE_BOARD_READ_CALIBRATION, WII_FSM_BALANCE_BOARD_READ_CALIBRATION2, WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION, WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION2, +#if SWITCH_PICO_WII_IR + WII_FSM_IR_SETUP, +#endif WII_FSM_DEV_GUESSED, // Device type guessed WII_FSM_DEV_ASSIGNED, // Device type assigned WII_FSM_LED_UPDATED, // After a device was assigned, update LEDs. @@ -104,16 +126,6 @@ WII_READ_FROM_REGISTERS = 0x04, } wii_read_type_t; -// nunchuk_t represents the data provided by the Nunchuk. -typedef struct nunchuk_s { - int sx; // Analog stick X - int sy; // Analog stick Y - int ax; // Accelerometer X - int ay; // Accelerometer Y - int az; // Accelerometer Z - bool bc; // Button C - bool bz; // Button Z -} nunchuk_t; // balance_board_t represents the data provided by the Balance Board. typedef struct balance_board_s { @@ -144,7 +156,6 @@ // wii_instance_t represents data used by the Wii driver instance. typedef struct wii_instance_s { uint8_t state; - uint8_t register_address; wii_mode_t mode; /* horizontal, accel, vertical, rumble, etc.. */ enum wii_devtype dev_type; enum wii_exttype ext_type; @@ -159,6 +170,39 @@ uint16_t rumble_duration_ms; balance_board_calibration_t balance_board_calibration; + + uint16_t read_address; + uint8_t read_size; + uint8_t report_type; + // Keep all independent validity flags within the fixed 256-byte instance. + bool motion_setup_done : 1; + bool accel_calibrated : 1; + bool mp_calibrated : 1; + bool mp_active : 1; + bool ready : 1; + bool ext_connected : 1; + bool topology_pending : 1; + bool gyro_valid : 1; + uint8_t ext_retries; + uint16_t accel_zero[3]; + uint16_t accel_span[3]; + // Two factory blocks, fast then slow; preserve all 16 bits of calibration. + uint8_t mp_calibration[32]; + int32_t accel[3]; + uint32_t accel_sequence; + int32_t gyro[3]; + uint32_t gyro_sequence; + int16_t nunchuk_x; + int16_t nunchuk_y; + uint16_t nunchuk_buttons; + // Per-axis factory max/min/center, read before MotionPlus maps A4. + uint8_t nunchuk_stick_calibration[6]; + bool nunchuk_accel_calibrated; + bool nunchuk_accel_valid; + uint16_t nunchuk_accel_zero[3]; + uint16_t nunchuk_accel_span[3]; + int32_t nunchuk_accel[3]; + uint32_t nunchuk_accel_sequence; // Debug only int debug_fd; // File descriptor where dump is saved @@ -177,7 +221,6 @@ static void process_drm_kae(uni_hid_device_t* d, const uint8_t* report, uint16_t len); static void process_drm_kee(uni_hid_device_t* d, const uint8_t* report, uint16_t len); static void process_drm_e(uni_hid_device_t* d, const uint8_t* report, uint16_t len); -static nunchuk_t process_nunchuk(const uint8_t* e, uint16_t len); static udraw_tablet_t process_udraw_tablet(const uint8_t* e, uint16_t len); static balance_board_t process_balance_board(uni_hid_device_t* d, const uint8_t* e, uint16_t len); @@ -196,6 +239,43 @@ static void on_wii_set_rumble_on(btstack_timer_source_t* ts); static void on_wii_set_rumble_off(btstack_timer_source_t* ts); static void wii_play_dual_rumble_now(struct uni_hid_device_s* d, uint16_t duration_ms); + +static void wii_write_register(uni_hid_device_t* d, uint32_t address, uint8_t value); + +#if SWITCH_PICO_WII_IR +enum wii_ir_step { + WII_IR_ENABLE1, + WII_IR_ENABLE2, + WII_IR_START, + WII_IR_SENSITIVITY1, + WII_IR_SENSITIVITY2 = WII_IR_SENSITIVITY1 + 9, + WII_IR_MODE = WII_IR_SENSITIVITY2 + 2, + WII_IR_FINISH, + WII_IR_DONE, + WII_IR_DISABLE1, + WII_IR_DISABLE2, + WII_IR_STOPPED, +}; + +typedef struct { + btstack_timer_source_t timer; + uni_wii_ir_snapshot_t snapshot; + bd_addr_t address; + hci_con_handle_t handle; + uint8_t step; + uint8_t mode; + uint8_t expected_ack; + bool initialized; + bool failed; +} wii_ir_t; + +// Device objects are reusable static slots. Keep timers outside parser_data, +// which device deletion clears without unlinking parser-owned timers. +static wii_ir_t wii_ir_devices[CONFIG_BLUEPAD32_MAX_DEVICES]; +static void wii_ir_invalidate(uni_hid_device_t* d); +static void wii_ir_on_timer(btstack_timer_source_t* ts); +static void process_drm_ir(uni_hid_device_t* d, const uint8_t* report, uint16_t len, bool basic); +#endif // Constants static const char* wii_devtype_names[] = { @@ -217,124 +297,456 @@ // process_ functions -// Defined here: http://wiibrew.org/wiki/Wiimote#0x20:_Status +static void wii_setup_read(uni_hid_device_t* d, enum wii_fsm state, wii_read_type_t type, + uint32_t address, uint8_t size) { + get_wii_instance(d)->state = state; + wii_read_mem(d, type, address, size); +} + +static bool wii_is_motion_plus(const uint8_t* id) { + // Like SDL/Linux, identify the data format/type suffix, not a vendor prefix. + return (id[4] == 0 || id[4] == 4 || id[4] == 5 || id[4] == 7) && id[5] == 5; +} + +static void wii_clear_extension(uni_hid_device_t* d) { + wii_instance_t* ins = get_wii_instance(d); + d->controller.gamepad.axis_x = 0; + d->controller.gamepad.axis_y = 0; + d->controller.gamepad.axis_rx = 0; + d->controller.gamepad.axis_ry = 0; + d->controller.gamepad.buttons &= ~ins->nunchuk_buttons; + ins->nunchuk_x = 0; + ins->nunchuk_y = 0; + ins->nunchuk_buttons = 0; + ins->nunchuk_accel_calibrated = false; + ins->nunchuk_accel_valid = false; + // Keep the sequence monotonic across hotplug, even if no snapshot observes + // the detached interval before the replacement's first real report. + memset(ins->nunchuk_accel, 0, sizeof(ins->nunchuk_accel)); + // Documented typical physical gate: center 128, travel +/-96. + // Keep clones with unreadable calibration usable; never learn from a held stick. + static const uint8_t nominal_stick[] = {224, 32, 128, 224, 32, 128}; + memcpy(ins->nunchuk_stick_calibration, nominal_stick, sizeof(nominal_stick)); + ins->ext_type = WII_EXT_NONE; + d->controller_subtype = ins->mode == WII_MODE_VERTICAL ? + CONTROLLER_SUBTYPE_WIIMOTE_VERTICAL : CONTROLLER_SUBTYPE_WIIMOTE_HORIZONTAL; +} + +static void wii_discover_extension(uni_hid_device_t* d) { + wii_instance_t* ins = get_wii_instance(d); +#if SWITCH_PICO_WII_IR + wii_ir_invalidate(d); +#endif + wii_clear_extension(d); + ins->topology_pending = false; + ins->ext_retries = 0; + ins->motion_setup_done = false; + // A400F0 unmaps an active MP and exposes its downstream extension. + // Do not decode in-flight extension bytes until the new topology is ready. + ins->mp_active = false; + ins->gyro_valid = false; + ins->mp_calibrated = false; + wii_fsm_ext_init(d); +} + +static bool wii_retry_extension(uni_hid_device_t* d) { + wii_instance_t* ins = get_wii_instance(d); + // Reply-paced transactions allow an initializing/partially inserted + // extension to settle without a timer that could outlive this device. + // Re-read after successful initialization rather than repeatedly resetting + // it, which could restart the initialization on each attempt. + if (ins->ext_connected && ins->ext_retries++ < 3) { + if (ins->state == WII_FSM_EXT_DID_READ_REGISTER) + wii_fsm_ext_read_register(d); + else + wii_fsm_ext_init(d); + return true; + } + return false; +} + +static void wii_confirm_topology(uni_hid_device_t* d) { + wii_instance_t* ins = get_wii_instance(d); + if (!ins->mp_active) { + ins->gyro_valid = false; + memset(ins->gyro, 0, sizeof(ins->gyro)); + memset(d->controller.gamepad.gyro, 0, sizeof(d->controller.gamepad.gyro)); + } + ins->state = WII_FSM_TOPOLOGY_STATUS; + const uint8_t report[] = {0xa2, WIIPROTO_REQ_SREQ, ins->rumble_state == WII_STATE_RUMBLE_IN_PROGRESS}; + uni_hid_device_send_intr_report(d, report, sizeof(report)); +} + +#if SWITCH_PICO_WII_IR +static wii_ir_t* wii_ir_for_device(uni_hid_device_t* d) { + if (!d || d->report_parser.setup != uni_hid_parser_wii_setup) + return NULL; + int idx = uni_hid_device_get_idx_for_instance(d); + if (idx < 0 || idx >= CONFIG_BLUEPAD32_MAX_DEVICES) + return NULL; + wii_ir_t* ir = &wii_ir_devices[idx]; + if (ir->handle != d->conn.handle || bd_addr_cmp(ir->address, d->conn.btaddr) != 0) + return NULL; + return ir; +} + +static void wii_ir_reset(uni_hid_device_t* d) { + int idx = uni_hid_device_get_idx_for_instance(d); + if (idx < 0 || idx >= CONFIG_BLUEPAD32_MAX_DEVICES) + return; + wii_ir_t* ir = &wii_ir_devices[idx]; + btstack_run_loop_remove_timer(&ir->timer); + memset(ir, 0, sizeof(*ir)); + ir->handle = d->conn.handle; + bd_addr_copy(ir->address, d->conn.btaddr); + ir->timer.process = wii_ir_on_timer; + ir->timer.context = ir; +} + +static void wii_ir_invalidate(uni_hid_device_t* d) { + wii_ir_t* ir = wii_ir_for_device(d); + if (!ir) + return; + ir->initialized = false; + ir->snapshot.valid_mask = 0; + // Do not rewind sequence on extension hotplug: the next real IR packet + // must remain distinguishable from the last packet of the old topology. +} + +static void wii_ir_wait(wii_ir_t* ir, uint32_t delay_ms) { + btstack_run_loop_remove_timer(&ir->timer); + btstack_run_loop_set_timer(&ir->timer, delay_ms); + btstack_run_loop_add_timer(&ir->timer); +} + +static void wii_ir_fail(uni_hid_device_t* d, wii_ir_t* ir) { + loge("Wii: IR setup failed at step %u; retaining controller-only reports\n", ir->step); + wii_ir_invalidate(d); + ir->failed = true; // No repeated camera failures until the next connection. + ir->expected_ack = 0; + ir->step = WII_IR_DISABLE1; + wii_ir_wait(ir, 50); +} + +static void wii_ir_complete(uni_hid_device_t* d, wii_ir_t* ir) { + ir->initialized = !ir->failed; + if (ir->initialized) + logi("Wii: IR camera ready, mode %u sensitivity %u\n", ir->mode, SWITCH_PICO_WII_IR_SENSITIVITY_LEVEL); + wii_instance_t* ins = get_wii_instance(d); + // A physical extension can change while the camera owns the write FSM. + // Reconcile after the last ACK, without competing memory transactions. + if (ins->topology_pending) + wii_discover_extension(d); + else if (ins->mp_active) + wii_setup_read(d, WII_FSM_MP_VERIFY, WII_READ_FROM_REGISTERS, 0xa400fa, 6); + else + wii_confirm_topology(d); +} + +static void wii_ir_on_timer(btstack_timer_source_t* ts) { + wii_ir_t* ir = ts->context; + int idx = ir - wii_ir_devices; + uni_hid_device_t* d = uni_hid_device_get_instance_for_idx(idx); + // Deletion clears the device but not this timer; slot reuse calls reset. + // Never let a late timer operate on another parser or Bluetooth session. + if (wii_ir_for_device(d) != ir || get_wii_instance(d)->state != WII_FSM_IR_SETUP || + !d->conn.interrupt_cid) + return; + if (ir->expected_ack) { + if (ir->step < WII_IR_DISABLE1) { + wii_ir_fail(d, ir); + } else { + // A broken camera must not hold controller setup indefinitely. + ir->expected_ack = 0; + ir->step++; + wii_ir_wait(ir, 50); + } + return; + } + if (ir->step == WII_IR_DONE || ir->step == WII_IR_STOPPED) { + wii_ir_complete(d, ir); + return; + } + uint8_t rumble = get_wii_instance(d)->rumble_state == WII_STATE_RUMBLE_IN_PROGRESS; + if (ir->step == WII_IR_ENABLE1 || ir->step == WII_IR_ENABLE2 || + ir->step == WII_IR_DISABLE1 || ir->step == WII_IR_DISABLE2) { + bool first = ir->step == WII_IR_ENABLE1 || ir->step == WII_IR_DISABLE1; + bool enable = ir->step < WII_IR_DISABLE1; + ir->expected_ack = first ? WIIPROTO_REQ_IR1 : WIIPROTO_REQ_IR2; + const uint8_t report[] = { + 0xa2, ir->expected_ack, 0x02 | (enable ? 0x04 : 0) | rumble, + }; + uni_hid_device_send_intr_report(d, report, sizeof(report)); + } else { + // Standard Wii sensitivity preset. Write every byte separately, with at + // least 50 ms after each ACK, including both camera-enable commands. + static const uint8_t sensitivity1[] = {0x02, 0x00, 0x00, 0x71, 0x01, 0x00, SWITCH_PICO_WII_IR_SENSITIVITY_LEVEL == 2 ? 0x96 : 0xaa, 0x00, SWITCH_PICO_WII_IR_SENSITIVITY_LEVEL == 2 ? 0xb4 : 0x64}; + static const uint8_t sensitivity2[] = {SWITCH_PICO_WII_IR_SENSITIVITY_LEVEL == 2 ? 0xb3 : 0x63, SWITCH_PICO_WII_IR_SENSITIVITY_LEVEL == 2 ? 0x04 : 0x03}; + uint32_t address = 0xb00030; + uint8_t value = 0x08; + if (ir->step >= WII_IR_SENSITIVITY1 && ir->step < WII_IR_SENSITIVITY2) { + uint8_t offset = ir->step - WII_IR_SENSITIVITY1; + address = 0xb00000 + offset; + value = sensitivity1[offset]; + } else if (ir->step >= WII_IR_SENSITIVITY2 && ir->step < WII_IR_MODE) { + uint8_t offset = ir->step - WII_IR_SENSITIVITY2; + address = 0xb0001a + offset; + value = sensitivity2[offset]; + } else if (ir->step == WII_IR_MODE) { + address = 0xb00033; + value = ir->mode; + } + ir->expected_ack = WIIPROTO_REQ_WMEM; + wii_write_register(d, address, value); + } + wii_ir_wait(ir, 500); +} + +static void wii_ir_ack(uni_hid_device_t* d, uint8_t command, uint8_t error) { + wii_ir_t* ir = wii_ir_for_device(d); + if (!ir || !ir->expected_ack || command != ir->expected_ack) + return; + if (error && ir->step < WII_IR_DISABLE1) { + wii_ir_fail(d, ir); + return; + } + ir->expected_ack = 0; + ir->step++; + wii_ir_wait(ir, 50); +} + +static bool wii_ir_start(uni_hid_device_t* d) { + wii_instance_t* ins = get_wii_instance(d); + wii_ir_t* ir = wii_ir_for_device(d); + if (!ir || ir->failed || ins->dev_type == WII_DEVTYPE_PRO_CONTROLLER || + (ins->ext_type != WII_EXT_NONE && ins->ext_type != WII_EXT_NUNCHUK)) + return false; + bool extension = ins->mp_active || ins->ext_type == WII_EXT_NUNCHUK; + uint8_t mode = extension ? 1 : 3; + if (ir->initialized && ir->mode == mode) + return false; + wii_ir_invalidate(d); + ir->mode = mode; + ir->step = WII_IR_ENABLE1; + ir->expected_ack = 0; + ins->state = WII_FSM_IR_SETUP; + // Keep buttons, accelerometer and six-byte extension decoding alive while + // camera setup is pending, using a format independent of the IR registers. + uni_hid_parser_wii_request_report_type(d, extension ? WII_REPORT_TYPE_KAE : WII_REPORT_TYPE_KA); + wii_ir_wait(ir, 50); + return true; +} + +bool uni_hid_parser_wii_ir_snapshot(uni_hid_device_t* d, uni_wii_ir_snapshot_t* out) { + wii_ir_t* ir = wii_ir_for_device(d); + if (!out || !ir || !ir->initialized) + return false; + *out = ir->snapshot; + return true; +} +#endif + +static void wii_finish_motion_setup(uni_hid_device_t* d) { + wii_instance_t* ins = get_wii_instance(d); + ins->motion_setup_done = true; + ins->read_size = 0; + logi("Wii: factory accel=%d, MotionPlus calibrated=%d active=%d, Nunchuk=%d\n", + ins->accel_calibrated, ins->mp_calibrated, ins->mp_active, ins->ext_type == WII_EXT_NUNCHUK); + wii_confirm_topology(d); +} + +static void wii_probe_motion_plus(uni_hid_device_t* d) { + // An inactive built-in or external MP does NOT set the status extension bit. + wii_setup_read(d, WII_FSM_MP_PROBE, WII_READ_FROM_REGISTERS, 0xa600fa, 6); +} + +static bool wii_read_accel_points(uint16_t zero_points[3], uint16_t spans[3], const uint8_t* c) { + // Dolphin WiimoteCommon::AccelCalibrationPoint is shared by the Remote + // EEPROM and Nunchuk A40020: XYZ high bytes, then XYZ low bits at 4/2/0. + for (int i = 0; i < 3; i++) { + int shift = 4 - 2 * i; + int zero = (c[i] << 2) | ((c[3] >> shift) & 3); + int one = (c[i + 4] << 2) | ((c[7] >> shift) & 3); + // Sensitivity is about 100 counts/g (Remote) or 200 (Nunchuk). + if (zero == 0 || zero == 1023 || one - zero < 16 || one - zero > 512) + return false; + zero_points[i] = zero; + spans[i] = one - zero; + } + return true; +} + +static bool wii_read_accel_calibration(wii_instance_t* ins, const uint8_t* c) { + uint8_t checksum = 0x55; + for (int i = 0; i < 9; i++) + checksum += c[i]; + return checksum == c[9] && wii_read_accel_points(ins->accel_zero, ins->accel_span, c); +} + +static uint16_t wii_be16(const uint8_t* p) { + return ((uint16_t)p[0] << 8) | p[1]; +} + +static bool wii_check_mp_calibration(const uint8_t* c) { + // IEEE CRC32 over both 14-byte payloads, excluding the split checksum. + uint32_t crc = UINT32_MAX; + for (int block = 0; block < 2; block++) { + const uint8_t* b = c + block * 16; + for (int i = 0; i < 14; i++) { + crc ^= b[i]; + for (int bit = 0; bit < 8; bit++) + crc = (crc >> 1) ^ ((crc & 1) ? 0xedb88320u : 0); + } + if (b[12] == 0 || b[12] == 0xff) + return false; + for (int axis = 0; axis < 3; axis++) { + int zero = wii_be16(b + axis * 2); + int scale = wii_be16(b + 6 + axis * 2); + int span = scale - zero; + if (zero == 0 || zero == 65535 || scale == 0 || scale == 65535 || + (span > -256 && span < 256)) + return false; + } + } + uint32_t expected = ((uint32_t)wii_be16(c + 14) << 16) | wii_be16(c + 30); + return ~crc == expected; +} + +static void wii_setup_read_error(uni_hid_device_t* d) { + wii_instance_t* ins = get_wii_instance(d); + logi("Wii: setup read failed in state %u\n", ins->state); + switch (ins->state) { + case WII_FSM_NUNCHUK_CALIBRATION: + // Retain nominal stick calibration, but still initialize remote motion. + ins->nunchuk_accel_calibrated = false; + ins->nunchuk_accel_valid = false; + ins->state = WII_FSM_DEV_GUESSED; + wii_process_fsm(d); + return; + case WII_FSM_ACCEL_CALIBRATION: + wii_setup_read(d, WII_FSM_ACCEL_CALIBRATION_BACKUP, WII_READ_FROM_MEM, 0x20, 10); + return; + case WII_FSM_ACCEL_CALIBRATION_BACKUP: + wii_probe_motion_plus(d); + return; + case WII_FSM_EXT_DID_READ_REGISTER: + if (wii_retry_extension(d)) + return; + ins->ext_type = WII_EXT_NONE; + ins->state = WII_FSM_DEV_GUESSED; + wii_process_fsm(d); + return; + case WII_FSM_MP_VERIFY: + // A real MP removal differs from its synthetic disconnect status: + // its mapped ID is no longer readable. + memset(ins->gyro, 0, sizeof(ins->gyro)); + memset(d->controller.gamepad.gyro, 0, sizeof(d->controller.gamepad.gyro)); + wii_discover_extension(d); + return; + case WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION: + case WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION2: + // Never run the weight interpolator with incomplete calibration. + ins->ext_type = WII_EXT_NONE; + break; + default: + break; + } + wii_finish_motion_setup(d); +} + static void process_req_status(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { - if (len < 7) { - loge("Wii: Unexpected report length; got %d, want >= 7\n", len); - return; - } - wii_instance_t* ins = get_wii_instance(d); - uint8_t flags = report[3] & 0x0f; // LF (leds / flags) + if (len < 7) + return; + wii_instance_t* ins = get_wii_instance(d); + // Wii Remote status contains a voltage-derived level; use SDL's capacity bands. + if (ins->dev_type != WII_DEVTYPE_PRO_CONTROLLER) { + const unsigned raw = report[6]; + const unsigned percent = raw > 178 ? 100 : raw > 51 ? 70 : raw > 13 ? 20 : 5; + d->controller.battery = (uint8_t)((percent * 255u + 50u) / 100u); + } + bool connected = (report[3] & 2) != 0; if (ins->state == WII_FSM_DID_REQ_STATUS) { - if (d->product_id == 0x0306) { - // We are positive that this is a Wii Remote 1st gen - ins->state = WII_FSM_DEV_GUESSED; - ins->dev_type = WII_DEVTYPE_REMOTE; - } else if (d->product_id == 0x0330) { - // It can be either a Wii Remote 2nd gen or a Wii U Pro Controller - if ((flags & 0x02) == 0) { - // If there are no extensions, then we are sure it is a Wii Remote MP. - ins->state = WII_FSM_DEV_GUESSED; - ins->dev_type = WII_DEVTYPE_REMOTE_MP; - } else { - // Otherwise, it can be either a Wii Remote MP with a Nunchuk or a - // Wii U Pro controller. - ins->state = WII_FSM_DEV_UNK; - } + ins->dev_type = d->product_id == 0x0330 ? WII_DEVTYPE_REMOTE_MP : WII_DEVTYPE_REMOTE; + ins->ext_connected = connected; + // Motion is always enabled; only "+" selects a different orientation. + if (report[1] & 0x10) + ins->mode = WII_MODE_VERTICAL; + // Also reset/probe A4 when the status bit is clear: inactive MP does + // not set it, and an attachment can race the initial status request. + wii_discover_extension(d); + } else if (ins->state == WII_FSM_TOPOLOGY_STATUS) { + // Reconcile status after the serialized setup, not while F0/FE writes + // are producing their own unplug/plug notifications. + if (!ins->mp_active && ins->ext_connected != connected && + ins->dev_type != WII_DEVTYPE_PRO_CONTROLLER) { + ins->ext_connected = connected; + wii_discover_extension(d); + } else { + wii_fsm_assign_device(d); } - - if ((flags & 0x02) != 0) { - // Extension detected: Nunchuk? - // Regardless of the previous FSM state, we overwrite it with "query - // extension". - logi("Wii: extension found.\n"); - ins->state = WII_FSM_EXT_UNK; - ins->ext_type = WII_EXT_UNK; - } else { - logi("Wii: No extensions found.\n"); - ins->ext_type = WII_EXT_NONE; + } else if (ins->state == WII_FSM_LED_UPDATED) { + // Every status stops the input stream, even when topology is unchanged. + uni_hid_parser_wii_request_report_type(d, ins->report_type); + if (ins->dev_type == WII_DEVTYPE_PRO_CONTROLLER) + return; + if (ins->mp_active) { + // A late activation or deactivation status is not proof of removal. + wii_setup_read(d, WII_FSM_MP_VERIFY, WII_READ_FROM_REGISTERS, 0xa400fa, 6); + } else if (ins->ext_connected != connected) { + ins->ext_connected = connected; + wii_discover_extension(d); } - - if (report[2] & 0x08) { - // Wii Remote only: Enter "accel mode" if "A" is pressed. - ins->mode = WII_MODE_ACCEL; - } else if (report[1] & 0x10) { - // Wii Remote only: Enter "vertical mode" if "+" is pressed. - ins->mode = WII_MODE_VERTICAL; - } - - wii_process_fsm(d); - } + } +#if SWITCH_PICO_WII_IR + if (ins->state == WII_FSM_IR_SETUP) { + // Restoring input does not issue a competing read or register write. + uni_hid_parser_wii_request_report_type(d, ins->report_type); + } +#endif + // All other states own a pending read/write. Never replace their FSM state + // or start a second transaction in response to a transitional status. } // Defined here: http://wiibrew.org/wiki/Wiimote#0x21:_Read_Memory_Data static void process_req_data_read_register(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { - uint8_t se = report[3]; // SE: size and error - uint8_t s = se >> 4; // size - uint8_t e = se & 0x0f; // error - if (e) { - loge("Wii: error reading memory: 0x%02x\n.", e); - return; - } - - wii_instance_t* ins = get_wii_instance(d); - - // We are expecting to read 6 bytes from 0xXX00fa - if (s == 5 && report[4] == 0x00 && report[5] == 0xfa) { - // This contains the read memory from register 0xa?00fa - // Data is in report[6]..report[11] - - // Try to guess device type. - if (report[10] == 0x01 && report[11] == 0x20) { - // Pro Controller: 00 00 a4 20 01 20 - ins->dev_type = WII_DEVTYPE_PRO_CONTROLLER; - ins->ext_type = WII_EXT_U_PRO_CONTROLLER; - } else if (ins->dev_type == WII_DEVTYPE_UNK) { - if (d->product_id == 0x0330) { - ins->dev_type = WII_DEVTYPE_REMOTE_MP; - } else if (d->product_id == 0x0306) { - ins->dev_type = WII_DEVTYPE_REMOTE; - } else { - loge("Wii: Unknown product id: 0x%04x\n", d->product_id); - } - } - - // Try to guess extension type. - if (ins->ext_type == WII_EXT_UNK) { - if (report[10] == 0x00 && report[11] == 0x00) { - // Nunchuck: 00 00 a4 20 00 00 - ins->ext_type = WII_EXT_NUNCHUK; - // If a Nunchuck is attached, WiiMode is treated as vertical mode - ins->mode = WII_MODE_VERTICAL; - } else if (report[10] == 0x04 && report[11] == 0x02) { - // Balance Board: 00 00 a4 20 04 02 - ins->ext_type = WII_EXT_BALANCE_BOARD; - } else if (report[10] == 0x01 && report[11] == 0x01) { - // Classic / Classic Pro: 0? 00 a4 20 01 01 - ins->ext_type = WII_EXT_CLASSIC_CONTROLLER; - } else if (report[10] == 0x01 && report[11] == 0x12) { - // Wii uDraw Tablet: FF 00 A4 20 01 12 - ins->ext_type = WII_EXT_UDRAW_TABLET; - // WiiMote is attached vertically to the uDraw Tablet - ins->mode = WII_MODE_VERTICAL; - } else { - loge("Wii: Unknown extension: %#x %#x\n", report[10], report[11]); - printf_hexdump(report, len); - } - } - - if (ins->ext_type == WII_EXT_BALANCE_BOARD) { - ins->state = WII_FSM_BALANCE_BOARD_READ_CALIBRATION; - } else { - ins->state = WII_FSM_DEV_GUESSED; - } - - logi("Wii: Device: %s, Extension: %s\n", wii_devtype_names[ins->dev_type], wii_exttype_names[ins->ext_type]); - wii_process_fsm(d); - } else { - loge("Wii: invalid response"); - printf_hexdump(report, len); - } + ARG_UNUSED(len); + wii_instance_t* ins = get_wii_instance(d); + const uint8_t* c = report + 6; + ins->ext_type = WII_EXT_NONE; + if (c[4] == 0xff && c[5] == 0xff) { + if (wii_retry_extension(d)) + return; + } else if (c[4] == 0 && c[5] == 0) { + // Vendor/version bytes vary. The upstream parser, SDL, and Linux all + // recognize Nunchuk from the format/type suffix after initialization. + ins->ext_type = WII_EXT_NUNCHUK; + } else if (c[4] == 1 && c[5] == 0x20) { + ins->dev_type = WII_DEVTYPE_PRO_CONTROLLER; + ins->ext_type = WII_EXT_U_PRO_CONTROLLER; + } else if (c[4] == 4 && c[5] == 2) { + ins->ext_type = WII_EXT_BALANCE_BOARD; + } else if (c[4] == 1 && c[5] == 1) { + ins->ext_type = WII_EXT_CLASSIC_CONTROLLER; + } else if (c[4] == 1 && c[5] == 0x12) { + ins->ext_type = WII_EXT_UDRAW_TABLET; + } else if (wii_is_motion_plus(c)) { + // A previously active MP can still be remapping A4 after F0 was acked. + if (wii_retry_extension(d)) + return; + } + if (ins->ext_type != WII_EXT_NONE) + ins->ext_connected = true; + else + logi("Wii: unidentified extension %02x %02x %02x %02x %02x %02x\n", + c[0], c[1], c[2], c[3], c[4], c[5]); + if (ins->ext_type == WII_EXT_NUNCHUK) { + wii_setup_read(d, WII_FSM_NUNCHUK_CALIBRATION, WII_READ_FROM_REGISTERS, 0xa40020, 16); + return; + } + ins->state = ins->ext_type == WII_EXT_BALANCE_BOARD ? + WII_FSM_BALANCE_BOARD_READ_CALIBRATION : WII_FSM_DEV_GUESSED; + logi("Wii: Device: %s, Extension: %s\n", wii_devtype_names[ins->dev_type], wii_exttype_names[ins->ext_type]); + wii_process_fsm(d); } static void process_req_data_read_calibration_data(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { @@ -444,17 +856,90 @@ // Defined here: http://wiibrew.org/wiki/Wiimote#0x21:_Read_Memory_Data static void process_req_data(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { - if (len < 22) { - loge("Wii: invalid req_data length: got %d, want >= 22\n", len); - printf_hexdump(report, len); - return; - } - - wii_instance_t* ins = get_wii_instance(d); + if (len < 22) + return; + wii_instance_t* ins = get_wii_instance(d); + uint16_t address = wii_be16(report + 4); + uint8_t size = (report[3] >> 4) + 1; + uint8_t error = report[3] & 0x0f; + // Read replies omit the address bank, so allow only one outstanding read. + // An error reply has no meaningful payload size, but still echoes address. + if (!ins->read_size || address != ins->read_address || (!error && size != ins->read_size)) + return; + ins->read_size = 0; + if (error) { + wii_setup_read_error(d); + return; + } + const uint8_t* c = report + 6; switch (ins->state) { case WII_FSM_EXT_DID_READ_REGISTER: process_req_data_read_register(d, report, len); break; + case WII_FSM_NUNCHUK_CALIBRATION: { + uint8_t checksum = 0x55; + for (int i = 0; i < 14; i++) + checksum += c[i]; + const bool checksum_valid = c[14] == checksum && c[15] == (uint8_t)(checksum + 0x55); + ins->nunchuk_accel_valid = false; + ins->nunchuk_accel_calibrated = checksum_valid && + wii_read_accel_points(ins->nunchuk_accel_zero, ins->nunchuk_accel_span, c); + if (!ins->nunchuk_accel_calibrated) + logi("Wii: invalid Nunchuk accelerometer calibration; Nunchuk motion disabled\n"); + // Stick validity is independent of accelerometer spans. + bool stick_valid = checksum_valid; + for (int i = 8; i <= 11; i += 3) + stick_valid = stick_valid && c[i + 2] - c[i + 1] >= 16 && c[i] - c[i + 2] >= 16; + if (stick_valid) + memcpy(ins->nunchuk_stick_calibration, c + 8, sizeof(ins->nunchuk_stick_calibration)); + else + logi("Wii: invalid Nunchuk stick calibration; using nominal gate\n"); + ins->state = WII_FSM_DEV_GUESSED; + wii_process_fsm(d); + break; + } + case WII_FSM_MP_VERIFY: + if (!wii_is_motion_plus(c) || c[4] == 0) { + memset(ins->gyro, 0, sizeof(ins->gyro)); + memset(d->controller.gamepad.gyro, 0, sizeof(d->controller.gamepad.gyro)); + wii_discover_extension(d); + } else if (ins->topology_pending) { + wii_discover_extension(d); + } else { + wii_fsm_assign_device(d); + } + break; + case WII_FSM_ACCEL_CALIBRATION: + case WII_FSM_ACCEL_CALIBRATION_BACKUP: + ins->accel_calibrated = wii_read_accel_calibration(ins, c); + if (!ins->accel_calibrated && ins->state == WII_FSM_ACCEL_CALIBRATION) + wii_setup_read(d, WII_FSM_ACCEL_CALIBRATION_BACKUP, WII_READ_FROM_MEM, 0x20, 10); + else + wii_probe_motion_plus(d); + break; + case WII_FSM_MP_PROBE: + if (wii_is_motion_plus(c)) { + ins->state = WII_FSM_MP_INIT; + wii_write_register(d, 0xa600f0, 0x55); + } else { + wii_finish_motion_setup(d); + } + break; + case WII_FSM_MP_CALIBRATION_FAST: + memcpy(ins->mp_calibration, c, 16); + wii_setup_read(d, WII_FSM_MP_CALIBRATION_SLOW, WII_READ_FROM_REGISTERS, 0xa60030, 16); + break; + case WII_FSM_MP_CALIBRATION_SLOW: + memcpy(ins->mp_calibration + 16, c, 16); + ins->mp_calibrated = wii_check_mp_calibration(ins->mp_calibration); + if (ins->mp_calibrated) { + ins->state = WII_FSM_MP_ACTIVATE; + wii_write_register(d, 0xa600fe, ins->ext_type == WII_EXT_NUNCHUK ? 5 : 4); + } else { + loge("Wii: invalid MotionPlus factory calibration; gyro disabled\n"); + wii_finish_motion_setup(d); + } + break; case WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION: process_req_data_read_calibration_data(d, report, len); break; @@ -465,7 +950,6 @@ process_req_data_dump_eeprom(d, report, len); break; default: - loge("process_req_data. Unknown FSM state: 0x%02x\n", ins->state); break; } } @@ -473,29 +957,44 @@ // Defined here: // http://wiibrew.org/wiki/Wiimote#0x22:_Acknowledge_output_report.2C_return_function_result static void process_req_return(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { - if (len < 5) { - loge("Invalid len report for process_req_return: got %d, want >= 5\n", len); - } - if (report[3] == WIIPROTO_REQ_WMEM) { - wii_instance_t* ins = get_wii_instance(d); - // Status != 0: Error. Probably invalid register - if (report[4] != 0) { - if (ins->register_address == 0xa6) { - loge("Failed to read registers from 0xa6... mmmm\n"); - ins->state = WII_FSM_SETUP; - } else { - // If it failed to read registers with 0xa4, then try with 0xa6. - // If 0xa6 works Ok, it is safe to assume it is a Wii Remote MP, but - // for the sake of finishing the "read extension" (might be useful in the future), - // we continue with it. - logi("Probably a Remote MP device. Switching to 0xa60000 address for registers.\n"); - ins->state = WII_FSM_DEV_UNK; - ins->register_address = 0xa6; // Register address used for Wii Remote MP. - } - } else { - // Status Ok. Good - } - wii_process_fsm(d); + if (len < 5) + return; + wii_instance_t* ins = get_wii_instance(d); +#if SWITCH_PICO_WII_IR + if (ins->state == WII_FSM_IR_SETUP) { + wii_ir_ack(d, report[3], report[4]); + return; + } +#endif + if (report[3] == WIIPROTO_REQ_RMEM && report[4] && ins->read_size) { + ins->read_size = 0; + wii_setup_read_error(d); + return; + } + if (report[3] != WIIPROTO_REQ_WMEM) + return; + // Acks contain no register address. Only a pending write may consume one. + switch (ins->state) { + case WII_FSM_EXT_DID_INIT: + case WII_FSM_EXT_DID_NO_ENCRYPTION: + if (report[4] && wii_retry_extension(d)) + break; + // FB can fail on an empty MP passthrough port; still identify A4 + // and probe A6 instead of treating an ACK as an extension identity. + wii_process_fsm(d); + break; + case WII_FSM_MP_INIT: + if (report[4]) + wii_finish_motion_setup(d); + else + wii_setup_read(d, WII_FSM_MP_CALIBRATION_FAST, WII_READ_FROM_REGISTERS, 0xa60020, 16); + break; + case WII_FSM_MP_ACTIVATE: + ins->mp_active = report[4] == 0; + wii_finish_motion_setup(d); + break; + default: + break; } } @@ -514,15 +1013,26 @@ const uint8_t* data = &report[1]; wii_instance_t* ins = get_wii_instance(d); - switch (ins->mode) { - case WII_MODE_HORIZONTAL: - process_drm_k_horizontal(ctl, data); - break; - case WII_MODE_VERTICAL: - process_drm_k_vertical(ctl, data); - break; - default: - break; + if (ins->ext_type == WII_EXT_NUNCHUK || ins->ext_type == WII_EXT_UDRAW_TABLET) { + // Preserve the existing extension mapping: C/Z -> X/Y, 1/2 -> L/R. + ctl->gamepad.dpad |= (data[0] & 0x01) ? DPAD_LEFT : 0; + ctl->gamepad.dpad |= (data[0] & 0x02) ? DPAD_RIGHT : 0; + ctl->gamepad.dpad |= (data[0] & 0x04) ? DPAD_DOWN : 0; + ctl->gamepad.dpad |= (data[0] & 0x08) ? DPAD_UP : 0; + ctl->gamepad.buttons |= (data[1] & 0x04) ? BUTTON_A : 0; + ctl->gamepad.buttons |= (data[1] & 0x08) ? BUTTON_B : 0; + ctl->gamepad.buttons |= (data[1] & 0x02) ? BUTTON_SHOULDER_L : 0; + ctl->gamepad.buttons |= (data[1] & 0x01) ? BUTTON_SHOULDER_R : 0; + if (ins->ext_type == WII_EXT_NUNCHUK) { + ctl->gamepad.axis_x = ins->nunchuk_x; + ctl->gamepad.axis_y = ins->nunchuk_y; + ctl->gamepad.buttons |= ins->nunchuk_buttons; + } + } else if (ins->mode == WII_MODE_VERTICAL) { + process_drm_k_vertical(ctl, data); + } else { + // WII_MODE_ACCEL was historically horizontal; motion no longer owns orientation. + process_drm_k_horizontal(ctl, data); } // Process misc buttons ctl->gamepad.misc_buttons |= (data[1] & 0x80) ? MISC_BUTTON_SYSTEM : 0; // Button "home" @@ -563,46 +1073,168 @@ // Used for WiiMote in Accelerometer Mode. Defined here: // http://wiibrew.org/wiki/Wiimote#0x31:_Core_Buttons_and_Accelerometer static void process_drm_ka(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { - // Process Wiimote in "accelerator mode". - /* DRM_KA: BB*2 AA*3*/ - // Expecting something like: - // 31 20 60 82 7F 99 - if (len < 6) { - loge("wii remote drm_ka: invalid report len %d\n", len); - return; - } - - uint16_t x = (report[3] << 2) | ((report[1] >> 5) & 0x3); - uint16_t y = (report[4] << 2) | ((report[2] >> 4) & 0x2); - uint16_t z = (report[5] << 2) | ((report[2] >> 5) & 0x2); - - int16_t sx = x - 0x200; - int16_t sy = y - 0x200; - int16_t sz = z - 0x200; - - // printf_hexdump(report, len); - // logi("Wii: x=%d, y=%d, z=%d\n", sx, sy, sz); - - uni_controller_t* ctl = &d->controller; - - ctl->gamepad.accel[0] = sx; - ctl->gamepad.accel[1] = sy; - ctl->gamepad.accel[2] = sz; - - // Dpad works as dpad, useful to navigate menus. - ctl->gamepad.dpad |= (report[1] & 0x01) ? DPAD_DOWN : 0; - ctl->gamepad.dpad |= (report[1] & 0x02) ? DPAD_UP : 0; - ctl->gamepad.dpad |= (report[1] & 0x04) ? DPAD_RIGHT : 0; - ctl->gamepad.dpad |= (report[1] & 0x08) ? DPAD_LEFT : 0; - - ctl->gamepad.buttons |= (report[2] & 0x02) ? BUTTON_A : 0; // Button "1" - ctl->gamepad.buttons |= (report[2] & 0x01) ? BUTTON_B : 0; // Button "2" - ctl->gamepad.buttons |= (report[2] & 0x08) ? BUTTON_X : 0; // Big button "A" - ctl->gamepad.buttons |= (report[2] & 0x04) ? BUTTON_Y : 0; // Button Shoulder - - ctl->gamepad.misc_buttons |= (report[2] & 0x80) ? MISC_BUTTON_SYSTEM : 0; // Button "home" - ctl->gamepad.misc_buttons |= (report[2] & 0x10) ? MISC_BUTTON_SELECT : 0; // Button "-" - ctl->gamepad.misc_buttons |= (report[1] & 0x10) ? MISC_BUTTON_START : 0; // Button "+" + if (len < 6) + return; + process_drm_k(d, report, len); + wii_instance_t* ins = get_wii_instance(d); + if (!ins->accel_calibrated) + return; + int32_t x = (report[3] << 2) | ((report[1] >> 5) & 3); + int32_t y = (report[4] << 2) | ((report[2] >> 4) & 2); + int32_t z = (report[5] << 2) | ((report[2] >> 5) & 2); + // SDL Wii axes: (-X, Z, Y), independent of the button orientation. + ins->accel[0] = uni_imu_scale(ins->accel_zero[0] - x, ins->accel_span[0], UNI_IMU_ACCEL_RES_PER_G); + ins->accel[1] = uni_imu_scale(z - ins->accel_zero[2], ins->accel_span[2], UNI_IMU_ACCEL_RES_PER_G); + ins->accel[2] = uni_imu_scale(y - ins->accel_zero[1], ins->accel_span[1], UNI_IMU_ACCEL_RES_PER_G); +#if SWITCH2_BRIDGE_FULL_INPUT + ins->accel_sequence = uni_hid_parser_native_motion_next_sequence(); +#else + if (++ins->accel_sequence == 0) + ++ins->accel_sequence; +#endif + for (int i = 0; i < 3; i++) + d->controller.gamepad.accel[i] = ins->accel[i]; +} + +bool uni_hid_parser_wii_accel_snapshot(uni_hid_device_t* d, + int32_t accel[3], uint32_t* sequence) { + if (!d || !accel || !sequence || d->report_parser.setup != uni_hid_parser_wii_setup) + return false; + wii_instance_t* ins = get_wii_instance(d); + if (!ins->accel_calibrated || ins->accel_sequence == 0) + return false; + for (int i = 0; i < 3; ++i) + accel[i] = ins->accel[i]; + *sequence = ins->accel_sequence; + return true; +} + +bool uni_hid_parser_wii_gyro_snapshot(uni_hid_device_t* d, + int32_t gyro[3], uint32_t* sequence) { + if (!d || !gyro || !sequence || d->report_parser.setup != uni_hid_parser_wii_setup) + return false; + wii_instance_t* ins = get_wii_instance(d); + if (!ins->mp_active || !ins->mp_calibrated || !ins->gyro_valid) + return false; + for (int i = 0; i < 3; ++i) + gyro[i] = ins->gyro[i]; + *sequence = ins->gyro_sequence; + return true; +} + +bool uni_hid_parser_wii_nunchuk_accel_snapshot(uni_hid_device_t* d, + int32_t accel[3], uint32_t* sequence) { + if (!d || !accel || !sequence || d->report_parser.setup != uni_hid_parser_wii_setup) + return false; + wii_instance_t* ins = get_wii_instance(d); + if (ins->ext_type != WII_EXT_NUNCHUK || + !ins->nunchuk_accel_calibrated || !ins->nunchuk_accel_valid) + return false; + for (int i = 0; i < 3; ++i) + accel[i] = ins->nunchuk_accel[i]; + *sequence = ins->nunchuk_accel_sequence; + return true; +} + +static int16_t wii_nunchuk_axis(uint8_t raw, const uint8_t* calibration, bool invert) { + int center = calibration[2]; + int delta = (int)raw - center; + int span = delta < 0 ? center - calibration[1] : calibration[0] - center; + int value = (invert ? -delta : delta) * (AXIS_NORMALIZE_RANGE / 2) / span; + if (value < -AXIS_NORMALIZE_RANGE / 2) + return -AXIS_NORMALIZE_RANGE / 2; + if (value >= AXIS_NORMALIZE_RANGE / 2) + return AXIS_NORMALIZE_RANGE / 2 - 1; + return value; +} + +static void wii_update_nunchuk(wii_instance_t* ins, const uint8_t* e, bool passthrough) { + ins->nunchuk_x = wii_nunchuk_axis(e[0], ins->nunchuk_stick_calibration, false); + ins->nunchuk_y = wii_nunchuk_axis(e[1], ins->nunchuk_stick_calibration + 3, true); + // MP moves C/Z from bits 1/0 to bits 3/2. + uint8_t buttons = e[5] >> (passthrough ? 2 : 0); + ins->nunchuk_buttons = (!(buttons & 2) ? BUTTON_X : 0) | (!(buttons & 1) ? BUTTON_Y : 0); + if (!ins->nunchuk_accel_calibrated) + return; + // WiiBrew Nunchuk / MotionPlus data formats. Passthrough loses bit 0 of + // each axis and moves AZ bit 2 out of the extension-connected flag. + int32_t x = (e[2] << 2) | (passthrough ? ((e[5] >> 4) & 1) << 1 : (e[5] >> 2) & 3); + int32_t y = (e[3] << 2) | (passthrough ? ((e[5] >> 5) & 1) << 1 : (e[5] >> 4) & 3); + int32_t z = passthrough ? ((e[4] & 0xfe) << 2) | (((e[5] >> 6) & 3) << 1) : + (e[4] << 2) | ((e[5] >> 6) & 3); + // Same SDL axes and Q13 units as Remote, but never write console IMU. + ins->nunchuk_accel[0] = uni_imu_scale(ins->nunchuk_accel_zero[0] - x, ins->nunchuk_accel_span[0], UNI_IMU_ACCEL_RES_PER_G); + ins->nunchuk_accel[1] = uni_imu_scale(z - ins->nunchuk_accel_zero[2], ins->nunchuk_accel_span[2], UNI_IMU_ACCEL_RES_PER_G); + ins->nunchuk_accel[2] = uni_imu_scale(y - ins->nunchuk_accel_zero[1], ins->nunchuk_accel_span[1], UNI_IMU_ACCEL_RES_PER_G); + if (++ins->nunchuk_accel_sequence == 0) + ++ins->nunchuk_accel_sequence; + ins->nunchuk_accel_valid = true; +} + +static int32_t wii_calibrate_gyro(const wii_instance_t* ins, int axis, int32_t raw, bool slow) { + const uint8_t* c = ins->mp_calibration + (slow ? 16 : 0); + int32_t zero = wii_be16(c + 2 * axis); + int32_t span = wii_be16(c + 6 + 2 * axis) - zero; + int32_t delta = raw * 4 - zero; // 14-bit sample versus 16-bit factory points. + if (span < 0) { + span = -span; + delta = -delta; + } + return uni_imu_scale(delta, span, c[12] * 6 * UNI_IMU_GYRO_RES_PER_DEG_S); +} + +static void wii_process_motion_extension(uni_hid_device_t* d, const uint8_t* e) { + wii_instance_t* ins = get_wii_instance(d); + bool accept_extension = ins->state == WII_FSM_LED_UPDATED || ins->state == WII_FSM_MP_VERIFY; +#if SWITCH_PICO_WII_IR + accept_extension = accept_extension || ins->state == WII_FSM_IR_SETUP; +#endif + if (!accept_extension) + return; + if (!ins->mp_active) { + if (ins->ext_type == WII_EXT_NUNCHUK) + wii_update_nunchuk(ins, e, false); + return; + } + // Bit zero is reserved in both MP and passthrough frames. In particular, + // ignore the FF-filled extension buffer while activation is settling. + if (e[5] & 1) + return; + bool connected = (e[4] & 1) != 0; + if (connected != ins->ext_connected) { + ins->ext_connected = connected; + ins->topology_pending = true; + wii_clear_extension(d); + } + if (!(e[5] & 2)) { + if (ins->ext_type == WII_EXT_NUNCHUK && (e[4] & 1)) + wii_update_nunchuk(ins, e, true); + if (ins->topology_pending && ins->state == WII_FSM_LED_UPDATED) + wii_discover_extension(d); + return; + } + if (!ins->mp_calibrated) + return; + int32_t yaw = e[0] | ((e[3] & 0xfc) << 6); + int32_t roll = e[1] | ((e[4] & 0xfc) << 6); + int32_t pitch = e[2] | ((e[5] & 0xfc) << 6); + // WiiBrew factory points have signed spans; Dolphin applies native + // right-hand-rule signs (-pitch,+roll,-yaw). SDL's (-X,Z,Y) then gives + // (+pitch,-yaw,+roll). Each axis has its OWN slow/fast selector. + ins->gyro[0] = wii_calibrate_gyro(ins, 2, pitch, e[3] & 1); + ins->gyro[1] = -wii_calibrate_gyro(ins, 0, yaw, e[3] & 2); + ins->gyro[2] = wii_calibrate_gyro(ins, 1, roll, e[4] & 2); +#if SWITCH2_BRIDGE_FULL_INPUT + ins->gyro_sequence = uni_hid_parser_native_motion_next_sequence(); +#else + if (++ins->gyro_sequence == 0) + ++ins->gyro_sequence; +#endif + ins->gyro_valid = true; + for (int i = 0; i < 3; i++) + d->controller.gamepad.gyro[i] = ins->gyro[i]; + if (ins->topology_pending && ins->state == WII_FSM_LED_UPDATED) + wii_discover_extension(d); } // Used in WiiMote + Nunchuk Mode @@ -617,31 +1249,17 @@ } wii_instance_t* ins = get_wii_instance(d); - if (ins->ext_type != WII_EXT_NUNCHUK && ins->ext_type != WII_EXT_UDRAW_TABLET) { - loge("Wii: unexpected Wii extension: got %d, want: %d or %d", ins->ext_type, WII_EXT_NUNCHUK, - WII_EXT_UDRAW_TABLET); - return; - } + if (ins->ext_type == WII_EXT_NUNCHUK || ins->mp_active) { + wii_process_motion_extension(d, report + 3); + process_drm_k(d, report, len); + return; + } + if (ins->ext_type != WII_EXT_UDRAW_TABLET) + return; uni_controller_t* ctl = &d->controller; - if (ins->mode != WII_MODE_VERTICAL) { - loge("Wii: When Nunchuk or uDraw Tablet is attached, only vertical mode is supported. Found: %d\n", ins->mode); - return; - } - - if (ins->ext_type == WII_EXT_NUNCHUK) { - // - // Process Nunchuk: Right axis, buttons X and Y - // - nunchuk_t n = process_nunchuk(&report[3], len - 3); - const int factor = (AXIS_NORMALIZE_RANGE / 2) / 128; - - ctl->gamepad.axis_rx = n.sx * factor; - ctl->gamepad.axis_ry = n.sy * factor; - ctl->gamepad.buttons |= n.bc ? BUTTON_X : 0; - ctl->gamepad.buttons |= n.bz ? BUTTON_Y : 0; - } else if (ins->ext_type == WII_EXT_UDRAW_TABLET) { + { // Better if uDraw reports its own "type", but for the moment // it gets reported and the 2nd half of a gamepad. @@ -668,59 +1286,59 @@ ctl->gamepad.buttons |= n.button_lower ? BUTTON_Y : 0; } - // - // Process Wii remote: DPAD, buttons A, B, Shoulder L & R, and misc. - // - - // dpad - ctl->gamepad.dpad |= (report[1] & 0x01) ? DPAD_LEFT : 0; - ctl->gamepad.dpad |= (report[1] & 0x02) ? DPAD_RIGHT : 0; - ctl->gamepad.dpad |= (report[1] & 0x04) ? DPAD_DOWN : 0; - ctl->gamepad.dpad |= (report[1] & 0x08) ? DPAD_UP : 0; - - ctl->gamepad.buttons |= (report[2] & 0x04) ? BUTTON_A : 0; // Shoulder button - ctl->gamepad.buttons |= (report[2] & 0x08) ? BUTTON_B : 0; // Big button "A" - - ctl->gamepad.buttons |= (report[2] & 0x02) ? BUTTON_SHOULDER_L : 0; // Button "1" - ctl->gamepad.buttons |= (report[2] & 0x01) ? BUTTON_SHOULDER_R : 0; // Button "2" - - ctl->gamepad.misc_buttons |= (report[2] & 0x80) ? MISC_BUTTON_SYSTEM : 0; // Button "home" - ctl->gamepad.misc_buttons |= (report[2] & 0x10) ? MISC_BUTTON_SELECT : 0; // Button "-" - ctl->gamepad.misc_buttons |= (report[1] & 0x10) ? MISC_BUTTON_START : 0; // Button "+" + process_drm_k(d, report, len); } // Defined here: // http://wiibrew.org/wiki/Wiimote#0x35:_Core_Buttons_and_Accelerometer_with_16_Extension_Bytes static void process_drm_kae(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { - // Expecting something like: - // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE - ARG_UNUSED(d); - ARG_UNUSED(report); - ARG_UNUSED(len); - loge("Wii: drm_kae not supported yet\n"); -} - -static nunchuk_t process_nunchuk(const uint8_t* e, uint16_t len) { - // Nunchuk format here: - // http://wiibrew.org/wiki/Wiimote/Extension_Controllers/Nunchuck - nunchuk_t n = {0}; - if (len < 6) { - loge("Wii: unexpected len; got %d, want >= 6\n", len); - return n; - } - n.sx = e[0] - 0x80; - // Invert polarity to match virtual gamepad. - n.sy = -(e[1] - 0x80); - n.ax = (e[2] << 2) | ((e[5] & 0b00001100) >> 2); - n.ay = (e[3] << 2) | ((e[5] & 0b00110000) >> 4); - n.az = (e[4] << 2) | ((e[5] & 0b11000000) >> 6); - n.ax -= AXIS_NORMALIZE_RANGE / 2; - n.ay -= AXIS_NORMALIZE_RANGE / 2; - n.az -= AXIS_NORMALIZE_RANGE / 2; - n.bc = !(e[5] & 0b00000010); - n.bz = !(e[5] & 0b00000001); - return n; -} + // 35 BB BB AA AA AA EE*16 + if (len < 22) + return; + // Update an interleaved extension before applying its cached controls. + wii_process_motion_extension(d, report + 6); + process_drm_ka(d, report, len); +} + +#if SWITCH_PICO_WII_IR +static void process_drm_ir(uni_hid_device_t* d, const uint8_t* report, uint16_t len, bool basic) { + if (len < (basic ? 22 : 18)) + return; + if (basic) + wii_process_motion_extension(d, report + 16); + process_drm_ka(d, report, len); + wii_ir_t* ir = wii_ir_for_device(d); + if (!ir || !ir->initialized || ir->mode != (basic ? 1 : 3)) + return; + uni_wii_ir_snapshot_t* snapshot = &ir->snapshot; + snapshot->buttons = wii_be16(report + 1) & 0x1f9f; // Remove accelerometer LSBs. + snapshot->valid_mask = 0; + for (int i = 0; i < 4; i++) { + const uint8_t* p = report + 6 + (basic ? (i / 2) * 5 : i * 3); + uint16_t x; + uint16_t y; + if (basic && (i & 1)) { + x = p[3] | ((p[2] & 0x03) << 8); + y = p[4] | ((p[2] & 0x0c) << 6); + } else { + x = p[0] | ((p[2] & 0x30) << 4); + y = p[1] | ((p[2] & 0xc0) << 2); + } + // FF-filled empty slots decode to (1023,1023). Reject all coordinates + // outside the physical camera image; never clamp an invisible spot. + if (y < 768) { + snapshot->x[i] = x; + snapshot->y[i] = y; + snapshot->valid_mask |= BIT(i); + } else { + snapshot->x[i] = 0; + snapshot->y[i] = 0; + } + } + snapshot->sequence++; +} +#endif + static udraw_tablet_t process_udraw_tablet(const uint8_t* e, uint16_t len) { // uDraw Tablet format here: @@ -1002,52 +1620,18 @@ logi("fsm: req_status\n"); wii_instance_t* ins = get_wii_instance(d); ins->state = WII_FSM_DID_REQ_STATUS; - const uint8_t status[] = {0xa2, WIIPROTO_REQ_SREQ, 0x00 /* LEDS & rumble off */}; + const uint8_t status[] = {0xa2, WIIPROTO_REQ_SREQ, ins->rumble_state == WII_STATE_RUMBLE_IN_PROGRESS}; uni_hid_device_send_intr_report(d, status, sizeof(status)); } static void wii_fsm_ext_init(uni_hid_device_t* d) { - logi("fsm: ext_init\n"); - wii_instance_t* ins = get_wii_instance(d); - ins->state = WII_FSM_EXT_DID_INIT; - // Init Wii - uint8_t report[] = { - // clang-format off - 0xa2, WIIPROTO_REQ_WMEM, - 0x04, // Control registers - 0xa4, 0x00, 0xf0, // register init extension - 0x01, 0x55, // # bytes, byte to write - // Padding, since at least 16 bytes must be sent - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, - // clang-format on - }; - report[3] = ins->register_address; - uni_hid_device_send_intr_report(d, report, sizeof(report)); + get_wii_instance(d)->state = WII_FSM_EXT_DID_INIT; + wii_write_register(d, 0xa400f0, 0x55); } static void wii_fsm_ext_encrypt_off(uni_hid_device_t* d) { - logi("fsm: ext_encrypt_off\n"); - wii_instance_t* ins = get_wii_instance(d); - ins->state = WII_FSM_EXT_DID_NO_ENCRYPTION; - // Init Wii - uint8_t report[] = { - // clang-format off - 0xa2, WIIPROTO_REQ_WMEM, - 0x04, // Control registers - 0xa4, 0x00, 0xfb, // register disable encryption - 0x01, 0x00, // # bytes, byte to write - // Padding, since at least 16 bytes must be sent - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, - // clang-format on - }; - report[3] = ins->register_address; - uni_hid_device_send_intr_report(d, report, sizeof(report)); + get_wii_instance(d)->state = WII_FSM_EXT_DID_NO_ENCRYPTION; + wii_write_register(d, 0xa400fb, 0); } static void wii_fsm_ext_read_register(uni_hid_device_t* d) { @@ -1055,8 +1639,7 @@ wii_instance_t* ins = get_wii_instance(d); ins->state = WII_FSM_EXT_DID_READ_REGISTER; - // Addr is either 0xA400FA or 0xA600FA - uint32_t offset = 0x0000fa | (ins->register_address << 16); + uint32_t offset = 0xa400fa; uint16_t bytes_to_read = 6; wii_read_mem(d, WII_READ_FROM_REGISTERS, offset, bytes_to_read); } @@ -1066,8 +1649,7 @@ wii_instance_t* ins = get_wii_instance(d); ins->state = WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION; - // Addr is either 0xA40024 or 0xA60024 - uint32_t offset = 0x000024 | (ins->register_address << 16); + uint32_t offset = 0xa40024; uint16_t bytes_to_read = 16; wii_read_mem(d, WII_READ_FROM_REGISTERS, offset, bytes_to_read); } @@ -1077,8 +1659,7 @@ wii_instance_t* ins = get_wii_instance(d); ins->state = WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION2; - // Addr is either 0xA40024 or 0xA60024 - uint32_t offset = 0x000034 | (ins->register_address << 16); + uint32_t offset = 0xa40034; uint16_t bytes_to_read = 8; wii_read_mem(d, WII_READ_FROM_REGISTERS, offset, bytes_to_read); } @@ -1086,6 +1667,10 @@ static void wii_fsm_assign_device(uni_hid_device_t* d) { logi("fsm: assign_device\n"); wii_instance_t* ins = get_wii_instance(d); +#if SWITCH_PICO_WII_IR + if (wii_ir_start(d)) + return; +#endif uint8_t dev = ins->dev_type; switch (dev) { case WII_DEVTYPE_UNK: @@ -1100,18 +1685,8 @@ } uint8_t report_type = 0xff; if (ins->ext_type == WII_EXT_NUNCHUK) { - // Request Nunchuk data - if (ins->mode == WII_MODE_ACCEL) { - // Request Core buttons + Accel + extension (nunchuk) - report_type = WII_REPORT_TYPE_KAE; - logi("Wii: requesting Core buttons + Accelerometer + E (Nunchuk)\n"); - d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_NUNCHUK_ACCEL; - } else { - // Request Core buttons + extension (nunchuk) - report_type = WII_REPORT_TYPE_KE; - logi("Wii: requesting Core buttons + E (Nunchuk)\n"); - d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_NUNCHUK; - } + report_type = WII_REPORT_TYPE_KAE; + d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_NUNCHUK_ACCEL; } else if (ins->ext_type == WII_EXT_CLASSIC_CONTROLLER) { logi("Wii: requesting E (Classic Controller)\n"); d->controller_subtype = CONTROLLER_SUBTYPE_WII_CLASSIC; @@ -1125,21 +1700,17 @@ d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_UDRAW_TABLET; report_type = WII_REPORT_TYPE_KE; } else { - if (ins->mode == WII_MODE_ACCEL) { - // Request Core buttons + accel - report_type = WII_REPORT_TYPE_KA; - logi("Wii: requesting Core buttons + Accelerometer\n"); - d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_ACCEL; - } else { - report_type = WII_REPORT_TYPE_K; - logi("Wii: requesting Core buttons\n"); - if (ins->mode == WII_MODE_VERTICAL) { - d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_VERTICAL; - } else { - d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_HORIZONTAL; - } - } + report_type = ins->mp_active ? WII_REPORT_TYPE_KAE : WII_REPORT_TYPE_KA; + if (ins->mode == WII_MODE_VERTICAL) + d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_VERTICAL; + else + d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_HORIZONTAL; } +#if SWITCH_PICO_WII_IR + wii_ir_t* ir = wii_ir_for_device(d); + if (ir && ir->initialized) + report_type = ir->mode == 1 ? WII_REPORT_TYPE_KAIE : WII_REPORT_TYPE_KAI; +#endif uni_hid_parser_wii_request_report_type(d, report_type); break; } @@ -1164,7 +1735,10 @@ ins->state = WII_FSM_LED_UPDATED; wii_process_fsm(d); - uni_hid_device_set_ready_complete(d); + if (!ins->ready) { + ins->ready = true; + uni_hid_device_set_ready_complete(d); + } } static void wii_fsm_dump_eeprom(struct uni_hid_device_s* d) { @@ -1218,7 +1792,15 @@ // Do nothing break; case WII_FSM_DEV_GUESSED: - wii_fsm_assign_device(d); + if (!ins->motion_setup_done && ins->dev_type != WII_DEVTYPE_PRO_CONTROLLER && + (ins->ext_type == WII_EXT_NONE || ins->ext_type == WII_EXT_NUNCHUK)) { + if (ins->accel_calibrated) + wii_probe_motion_plus(d); + else + wii_setup_read(d, WII_FSM_ACCEL_CALIBRATION, WII_READ_FROM_MEM, 0x16, 10); + } else { + wii_confirm_topology(d); + } break; case WII_FSM_BALANCE_BOARD_READ_CALIBRATION: wii_fsm_balance_board_read_calibration(d); @@ -1234,6 +1816,19 @@ wii_fsm_update_led(d); break; case WII_FSM_LED_UPDATED: + case WII_FSM_NUNCHUK_CALIBRATION: + case WII_FSM_ACCEL_CALIBRATION: + case WII_FSM_ACCEL_CALIBRATION_BACKUP: + case WII_FSM_MP_PROBE: + case WII_FSM_MP_INIT: + case WII_FSM_MP_CALIBRATION_FAST: + case WII_FSM_MP_CALIBRATION_SLOW: + case WII_FSM_MP_ACTIVATE: + case WII_FSM_MP_VERIFY: + case WII_FSM_TOPOLOGY_STATUS: +#if SWITCH_PICO_WII_IR + case WII_FSM_IR_SETUP: +#endif break; default: loge("Wii: wii_process_fsm() unexpected state: %d\n", ins->state); @@ -1249,10 +1844,11 @@ ins->mode = WII_MODE_HORIZONTAL; ins->state = WII_FSM_SETUP; + d->controller.battery = UNI_CONTROLLER_BATTERY_NOT_AVAILABLE; - - // Start with 0xa40000 (all Wii devices, except for the Wii Remote Plus) - // If it fails it will use 0xa60000 - ins->register_address = 0xa4; +#if SWITCH_PICO_WII_IR + wii_ir_reset(d); +#endif + // Dump EEPROM #if ENABLE_EEPROM_DUMP @@ -1266,14 +1862,51 @@ wii_process_fsm(d); } +void uni_hid_parser_wii_teardown(uni_hid_device_t* d) { + if (!d || d->report_parser.setup != uni_hid_parser_wii_setup) + return; + wii_instance_t* ins = get_wii_instance(d); +#if SWITCH2_BRIDGE_FULL_INPUT + ins->accel_calibrated = false; + ins->nunchuk_accel_calibrated = ins->nunchuk_accel_valid = false; +#endif + ins->gyro_valid = false; + ins->mp_active = false; + ins->mp_calibrated = false; + memset(ins->gyro, 0, sizeof(ins->gyro)); + memset(d->controller.gamepad.gyro, 0, sizeof(d->controller.gamepad.gyro)); + btstack_run_loop_remove_timer(&ins->rumble_timer_delayed_start); + btstack_run_loop_remove_timer(&ins->rumble_timer_duration); + ins->rumble_state = WII_STATE_RUMBLE_DISABLED; + ins->ready = false; +#if SWITCH_PICO_WII_IR + wii_ir_invalidate(d); +#endif +} + +bool uni_hid_parser_wii_rumble_ready(uni_hid_device_t* d) { + if (!d || d->report_parser.setup != uni_hid_parser_wii_setup) + return false; + wii_instance_t* ins = get_wii_instance(d); + return d->conn.connected && d->conn.interrupt_cid != 0 && + ins->ready && ins->state >= WII_FSM_LED_UPDATED; +} + void uni_hid_parser_wii_init_report(uni_hid_device_t* d) { - // Reset old state. Each report contains a full-state. + wii_instance_t* ins = get_wii_instance(d); + const uint8_t battery = d->controller.battery; memset(&d->controller, 0, sizeof(d->controller)); d->controller.klass = UNI_CONTROLLER_CLASS_GAMEPAD; + d->controller.battery = battery; + // Status/ack and Nunchuk interleaves do not contain new motion samples. + for (int i = 0; i < 3; i++) { + d->controller.gamepad.accel[i] = ins->accel[i]; + d->controller.gamepad.gyro[i] = ins->gyro[i]; + } } void uni_hid_parser_wii_parse_input_report(uni_hid_device_t* d, const uint8_t* report, uint16_t len) { - if (len == 0) + if (!report || len == 0) return; switch (report[0]) { case WIIPROTO_REQ_STATUS: @@ -1291,6 +1924,14 @@ case WII_REPORT_TYPE_KAE: process_drm_kae(d, report, len); break; +#if SWITCH_PICO_WII_IR + case WII_REPORT_TYPE_KAI: + process_drm_ir(d, report, len, false); + break; + case WII_REPORT_TYPE_KAIE: + process_drm_ir(d, report, len, true); + break; +#endif case WII_REPORT_TYPE_KEE: process_drm_kee(d, report, len); break; @@ -1307,6 +1948,9 @@ logi("Wii parser: unknown report type: 0x%02x\n", report[0]); printf_hexdump(report, len); } + if (len >= 3 && report[0] >= WIIPROTO_REQ_STATUS && report[0] <= WIIPROTO_REQ_RETURN && + get_wii_instance(d)->dev_type != WII_DEVTYPE_PRO_CONTROLLER) + process_drm_k(d, report, len); } void uni_hid_parser_wii_set_player_leds(uni_hid_device_t* d, uint8_t leds) { @@ -1371,29 +2015,25 @@ void uni_hid_parser_wii_set_mode(uni_hid_device_t* d, wii_mode_t mode) { wii_instance_t* ins = get_wii_instance(d); - + if (mode != WII_MODE_HORIZONTAL && mode != WII_MODE_VERTICAL && mode != WII_MODE_ACCEL) + return; + // Extension mappings are vertical independently of the selected standalone + // orientation. Preserve the user's choice through attachment and removal. ins->mode = mode; - switch (ins->mode) { - case WII_MODE_HORIZONTAL: - d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_HORIZONTAL; - break; - case WII_MODE_VERTICAL: - d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_VERTICAL; - break; - case WII_MODE_ACCEL: - // TODO: request Accel report. As it is, it doesn't work. - if (ins->ext_type == WII_EXT_NONE) - d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_ACCEL; - else if (ins->ext_type == WII_EXT_NUNCHUK) - d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_NUNCHUK_ACCEL; - break; - default: - break; - } + if (ins->state == WII_FSM_LED_UPDATED) + wii_fsm_assign_device(d); } void uni_hid_parser_wii_request_report_type(struct uni_hid_device_s* d, wii_report_type_t report_type) { - uint8_t report[] = {0xa2, WIIPROTO_REQ_DRM, 0x00, report_type}; + wii_instance_t* ins = get_wii_instance(d); + ins->report_type = report_type; + uint8_t continuous = (report_type == WII_REPORT_TYPE_KA || report_type == WII_REPORT_TYPE_KAE) ? 4 : 0; +#if SWITCH_PICO_WII_IR + if (report_type == WII_REPORT_TYPE_KAI || report_type == WII_REPORT_TYPE_KAIE) + continuous = 4; +#endif + uint8_t report[] = {0xa2, WIIPROTO_REQ_DRM, + continuous | (ins->rumble_state == WII_STATE_RUMBLE_IN_PROGRESS), report_type}; uni_hid_device_send_intr_report(d, report, sizeof(report)); } @@ -1480,10 +2120,13 @@ static void wii_read_mem(uni_hid_device_t* d, wii_read_type_t t, uint32_t offset, uint16_t size) { logi("****** read_mem: offset=0x%04x, size=%d from=%d\n", offset, size, t); + wii_instance_t* ins = get_wii_instance(d); + ins->read_address = offset & 0xffff; + ins->read_size = size; uint8_t report[] = { // clang-format off 0xa2, WIIPROTO_REQ_RMEM, - t, // Read from registers or memory + t | (ins->rumble_state == WII_STATE_RUMBLE_IN_PROGRESS), (offset & 0xff0000) >> 16, (offset & 0xff00) >> 8, (offset & 0xff), // Offset (size & 0xff00) >> 8, (size & 0xff), // Size in bytes // clang-format on @@ -1491,6 +2134,15 @@ uni_hid_device_send_intr_report(d, report, sizeof(report)); } +static void wii_write_register(uni_hid_device_t* d, uint32_t address, uint8_t value) { + wii_instance_t* ins = get_wii_instance(d); + uint8_t report[23] = { + 0xa2, WIIPROTO_REQ_WMEM, 0x04 | (ins->rumble_state == WII_STATE_RUMBLE_IN_PROGRESS), + address >> 16, address >> 8, address, 1, value, + }; + uni_hid_device_send_intr_report(d, report, sizeof(report)); +} + void uni_hid_parser_wii_device_dump(uni_hid_device_t* d) { wii_instance_t* ins = get_wii_instance(d); logi("\tWii: device '%s', extension '%s'\n", wii_devtype_names[ins->dev_type], wii_exttype_names[ins->ext_type]); diff --git a/src/components/bluepad32/uni_hid_device.c b/src/components/bluepad32/uni_hid_device.c --- a/src/components/bluepad32/uni_hid_device.c +++ b/src/components/bluepad32/uni_hid_device.c @@ -32,6 +32,10 @@ #include "parser/uni_hid_parser_stadia.h" #include "parser/uni_hid_parser_steam.h" #include "parser/uni_hid_parser_switch.h" +#include "parser/uni_hid_parser_switch2.h" +#if SWITCH2_BRIDGE_FULL_INPUT +#include "parser/uni_hid_parser_native_motion.h" +#endif #include "parser/uni_hid_parser_wii.h" #include "parser/uni_hid_parser_xboxone.h" #include "platform/uni_platform.h" @@ -447,6 +451,18 @@ return; } + if (d->report_parser.setup == uni_hid_parser_switch_setup) + uni_hid_parser_switch_teardown(d); + if (uni_hid_parser_switch2_is_ble_device(d)) + uni_hid_parser_switch2_teardown(d); + if (d->report_parser.setup == uni_hid_parser_wii_setup) + uni_hid_parser_wii_teardown(d); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_forget(d); + if (d->report_parser.setup == uni_hid_parser_ds5_setup) + uni_hid_parser_ds5_bridge_teardown(d); +#endif + // Disconnect child first if (d->child) uni_hid_device_disconnect(d->child); @@ -465,9 +481,9 @@ // Cleanup if (!uni_hid_device_is_virtual_device(d)) { type = gap_get_connection_type(d->conn.handle); - if (IS_ENABLED(UNI_ENABLE_BLE) && type == GAP_CONNECTION_LE) + if (IS_ENABLED(ENABLE_BLE) && type == GAP_CONNECTION_LE) uni_bt_le_disconnect(d); - else if (IS_ENABLED(UNI_ENABLE_BREDR) && type == GAP_CONNECTION_ACL) + else if (IS_ENABLED(ENABLE_CLASSIC) && type == GAP_CONNECTION_ACL) uni_bt_bredr_disconnect(d); else loge("uni_hid_device_disconnect: Unknown GAP connection type: %d\n", type); @@ -490,6 +506,18 @@ loge("uni_hid_device_delete: invalid hid device: NULL\n"); return; } + if (d->report_parser.setup == uni_hid_parser_switch_setup) + uni_hid_parser_switch_teardown(d); + if (uni_hid_parser_switch2_is_ble_device(d)) + uni_hid_parser_switch2_teardown(d); + if (d->report_parser.setup == uni_hid_parser_wii_setup) + uni_hid_parser_wii_teardown(d); +#if SWITCH2_BRIDGE_FULL_INPUT + uni_hid_parser_native_motion_forget(d); + if (d->report_parser.setup == uni_hid_parser_ds5_setup) + uni_hid_parser_ds5_bridge_teardown(d); +#endif + // Delete child first if (d->child) @@ -655,6 +683,7 @@ d->report_parser.setup = uni_hid_parser_psmove_setup; d->report_parser.init_report = uni_hid_parser_psmove_init_report; d->report_parser.parse_input_report = uni_hid_parser_psmove_parse_input_report; + d->report_parser.parse_feature_report = uni_hid_parser_psmove_parse_feature_report; d->report_parser.set_lightbar_color = uni_hid_parser_psmove_set_lightbar_color; d->report_parser.play_dual_rumble = uni_hid_parser_psmove_play_dual_rumble; logi("Device detected as PS Move: 0x%02x\n", type); @@ -718,6 +747,15 @@ d->report_parser.device_dump = uni_hid_parser_switch_device_dump; logi("Device detected as Nintendo Switch Pro controller: 0x%02x\n", type); break; + case CONTROLLER_TYPE_Switch2ProController: + case CONTROLLER_TYPE_Switch2JoyConLeft: + case CONTROLLER_TYPE_Switch2JoyConRight: + d->report_parser.setup = uni_hid_parser_switch2_setup; + d->report_parser.init_report = uni_hid_parser_switch2_init_report; + d->report_parser.parse_input_report = uni_hid_parser_switch2_parse_input_report; + d->report_parser.set_player_leds = uni_hid_parser_switch2_set_player_leds; + d->report_parser.play_dual_rumble = uni_hid_parser_switch2_play_dual_rumble; + break; case CONTROLLER_TYPE_SteamController: d->report_parser.setup = uni_hid_parser_steam_setup; d->report_parser.init_report = uni_hid_parser_steam_init_report; @@ -809,7 +847,11 @@ return; } - int err = l2cap_send(cid, (uint8_t*)report, len); + int err; + if (d->report_parser.setup == uni_hid_parser_switch_setup) + err = uni_hid_parser_switch_send_report(d, cid, report, len); + else + err = l2cap_send(cid, (uint8_t*)report, len); if (err != 0) { logd("Could not send report (error=0x%04x). Adding it to queue\n", err); if (uni_circular_buffer_put(&d->outgoing_buffer, cid, report, len) != 0) {