jyapayne/switch-pico
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add selectable Wii IR aiming and DolphinBar keepalive

Browse source
This commit is contained in:
Joey Yakimowich-Payne 2026-09-09 17:13:27 -06:00
commit 5ecfca105a
27 changed files with 4079 additions and 53 deletions
Download patch file Download diff file Expand all files Collapse all files

31
CMakeLists.txt
View file

@ -24,6 +24,10 @@ if (EXISTS ${picoVscode})
endif()
# ====================================================================================
option(SWITCH_PICO_LOG "Enable UART debug logging" OFF)
option(SWITCH_PICO_WII_IR_MOUSE
"Experiment: Wii IR as a composite USB mouse with Switch controllers" OFF)
option(SWITCH_PICO_WII_IR_GYRO
"Experiment: selectable Wii IR or MotionPlus gyro aiming" OFF)
if(SWITCH_PICO_INPUT_BACKEND STREQUAL "BLUEPAD32")
set(SWITCH_PICO_NATIVE_DEFAULT ON)
set(SWITCH_PICO_CLOCK_DEFAULT 300)
@ -111,6 +115,22 @@ if(SWITCH_PICO_INPUT_BACKEND STREQUAL "BLUEPAD32")
elseif(NOT SWITCH_PICO_BLUETOOTH_MODE STREQUAL "MIXED")
message(FATAL_ERROR "Single-transport Bluetooth modes require the BLUEPAD32 backend")
endif()
if(SWITCH_PICO_WII_IR_MOUSE AND SWITCH_PICO_WII_IR_GYRO)
message(FATAL_ERROR "Select either Wii IR mouse or selectable gyro output")
endif()
if(SWITCH_PICO_WII_IR_GYRO)
add_compile_definitions(SWITCH_PICO_WII_IR_GYRO=1)
endif()
if(SWITCH_PICO_WII_IR_MOUSE OR SWITCH_PICO_WII_IR_GYRO)
if(NOT SWITCH_PICO_INPUT_BACKEND STREQUAL "BLUEPAD32"
OR NOT SWITCH_PICO_ENABLE_CLASSIC)
message(FATAL_ERROR "Wii IR requires BLUEPAD32 with Classic Bluetooth")
endif()
add_compile_definitions(SWITCH_PICO_WII_IR=1)
endif()
if(SWITCH_PICO_WII_IR_MOUSE)
add_compile_definitions(SWITCH_PICO_WII_IR_MOUSE=1)
endif()
set(PICO_BOARD pico CACHE STRING "Board type")
if(SWITCH_PICO_INPUT_BACKEND STREQUAL "BLUEPAD32"
AND NOT PICO_BOARD STREQUAL "pico2_w")
@ -266,6 +286,15 @@ add_executable(switch-pico
${SWITCH_PICO_SOURCE_DIR}/usb/switch/switch_pro_driver.cpp
${SWITCH_PICO_SOURCE_DIR}/usb/switch/switch_haptics.cpp
)
if(SWITCH_PICO_WII_IR_MOUSE OR SWITCH_PICO_WII_IR_GYRO)
target_sources(switch-pico PRIVATE
${SWITCH_PICO_SOURCE_DIR}/input/wii_ir_pointer.cpp
${SWITCH_PICO_SOURCE_DIR}/input/wii_ir_tracker.cpp)
endif()
if(SWITCH_PICO_WII_IR_MOUSE)
target_sources(switch-pico PRIVATE
${SWITCH_PICO_SOURCE_DIR}/usb/wii_ir_mouse_usb.cpp)
endif()
if(SWITCH_PICO_INPUT_BACKEND STREQUAL "BLUEPAD32")
target_sources(switch-pico PRIVATE
${SWITCH_PICO_SOURCE_DIR}/adapter/adapter_host_probe.cpp
@ -311,7 +340,7 @@ if(SWITCH_PICO_INPUT_BACKEND STREQUAL "BLUEPAD32")
SWITCH_PICO_BLUEPAD32=1
SWITCH_PICO_NATIVE_SWITCH_RUMBLE=1
SWITCH_PICO_HID_INSTANCE_COUNT=4
SWITCH_PICO_USB_OUTPUT_MODES=1
$<$<NOT:$<OR:$<BOOL:${SWITCH_PICO_WII_IR_MOUSE}>,$<BOOL:${SWITCH_PICO_WII_IR_GYRO}>>>:SWITCH_PICO_USB_OUTPUT_MODES=1>
PICO_FLASH_ASSUME_CORE1_SAFE=0
PICO_STACK_SIZE=4096
PICO_BTSTACK_CYW43_MAX_HCI_PROCESS_LOOP_COUNT=$<IF:$<BOOL:${SWITCH_PICO_HAPTICS_EXPERIMENT}>,1,16>

2
README.md
View file

@ -467,6 +467,8 @@ The AIO firmware enables motion automatically for original Wii Remotes with an e
- **Nunchuk stick travel:** the parser reads the 16-byte extension calibration at `A40020` before activating MotionPlus. It validates both checksum bytes and the X/Y min/center/max ordering, then scales each side of each axis independently into the full normalized range, clamps overshoot, and inverts Y. It preserves the notched gate rather than expanding diagonals into square corners. Unreadable/invalid calibration uses documented nominal travel (center 128, ±96), not the full 0–255 byte range. Replacement Nunchuks reload their own calibration. Existing left-stick profile tuning applies; saved right-stick tuning is not copied or overwritten.
- **Motion:** ordinary remotes use continuous `0x31`; MotionPlus and Nunchuk use combined `0x35` reports. The parser checks primary/backup accelerometer factory calibration and both fast/slow gyro calibration blocks, including checksums. Invalid calibration disables only the affected sensor rather than inventing readings. MotionPlus selects sensitivity independently for each axis; factory-only zero correction can retain temperature-dependent gyro drift.
- **Nunchuk passthrough:** MotionPlus and Nunchuk samples alternate. Held Nunchuk controls persist across gyro packets, moved C/Z bits are decoded, and the remote's accelerometer remains the motion source. Attaching or removing a Nunchuk triggers serialized extension discovery, updates Studio's layout, and clears detached stick/C/Z state. Extension identifiers accept the upstream-compatible type suffix rather than requiring a vendor-specific prefix; initializing extensions get bounded reply-paced retries. Periodic discovery of an inactive external MotionPlus attached alone and Classic Controller MotionPlus passthrough remain unimplemented. Existing ordinary Classic Controller, Wii U Pro, Balance Board and uDraw paths remain separate.
- **Experimental IR aiming:** `SWITCH_PICO_WII_IR_GYRO=ON` enables camera-derived aiming instead of the mutually exclusive IR mouse experiment. Hold **1 + 2 for two seconds** to switch sources: one rumble pulse selects physical gyro, two select IR. Physical gyro is the connection default. With a Nunchuk, hold **C + 1** to reposition without sending aim motion; 1 alone remains usable. IR horizontal motion uses Nintendo **gyro Z**, matching a captured physical MotionPlus yaw reference; gyro X remains neutral. Vertical output uses gyro Y with **1.5x pitch gain**, conservatively tuned against a physical-gyro capture without changing horizontal gain, camera geometry or filtering. The real accelerometer is retained. Tracking loss stops IR aiming rather than silently switching to physical gyro. Horizontal aiming is user-confirmed in-game; the increased vertical gain still requires in-game qualification.
- **IR tracking:** gyro mode follows relative camera-frame movement of one to four persistent spots, without requiring both ends of the bar or expiring a continuously visible single spot after 80 ms. Mutual nearest matches tolerate camera-slot changes; conflicting motion, large jumps, stale reports and complete visibility loss rebaseline rather than replay missing movement. The IR mouse experiment retains its two-end bar model. Relative single-spot tracking cannot distinguish remote roll from pointing motion, so keep the remote upright for aiming.
- **Hardware verification:** Remote Plus `B8:AE:6E:21:8F:C2` reconnected after flashing and delivered 100 real USB `0x30` reports with changing accelerometer and gyro values on all axes. Identify dispatched rumble without losing the link. All 72 profile records, names/aliases, active selections, nine pairings and adapter configuration generation 21 / CRC `b58672ac` were unchanged. The subsequent Studio fix detects the attached Nunchuk with live stick and gyro data and renders the matching layout and vertical lock. Native tests cover integrated/external detection, calibration/error boundaries, hotplug ordering, interleaved controls, orientation changes and stale-request rejection. External MotionPlus combinations still need physical qualification.
Protocol references: [WiiBrew Wiimote](https://wiibrew.org/wiki/Wiimote), [MotionPlus registers/calibration](https://wiibrew.org/wiki/Wiimote/Extension_Controllers/Wii_Motion_Plus), [Dolphin MotionPlus calibration](https://github.com/dolphin-emu/dolphin/blob/master/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp), and [SDL Wii sensor axes](https://github.com/libsdl-org/SDL/blob/main/src/joystick/hidapi/SDL_hidapi_wii.c).

37
bluepad32_config/parser/uni_hid_parser_wii_ir.h Normal file
View file

@ -0,0 +1,37 @@
// SPDX-License-Identifier: Apache-2.0
#ifndef UNI_HID_PARSER_WII_IR_H
#define UNI_HID_PARSER_WII_IR_H
#include <stdbool.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
#include "uni_hid_device.h"
#if SWITCH_PICO_WII_IR
// Bluetooth-thread snapshot, before orientation or controller button mapping.
// buttons uses the big-endian Wiimote mask: A=0x0008, B=0x0004.
// valid_mask bit i selects x[i]/y[i] (0..1023 / 0..767); invalid slots are zero.
// sequence advances only for a fresh IR report, even with no visible spots.
// Hotplug retains the sequence; reconnect/setup resets it to zero. The getter
// returns false while IR is unavailable or initializing, and for other devices.
typedef struct {
uint32_t sequence;
uint16_t buttons;
uint16_t x[4];
uint16_t y[4];
uint8_t valid_mask;
} uni_wii_ir_snapshot_t;
bool uni_hid_parser_wii_ir_snapshot(uni_hid_device_t* d, uni_wii_ir_snapshot_t* out);
#endif
#ifdef __cplusplus
}
#endif
#endif // UNI_HID_PARSER_WII_IR_H

380
patches/bluepad32-sdl3-imu.patch
View file

@ -1268,15 +1268,18 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
#include <assert.h>
#include <stdbool.h>
@@ -19,6 +21,7 @@
@@ -19,6 +21,10 @@
#endif // ENABLE_EEPROM_DUMP
#include "parser/uni_hid_parser_wii.h"
+#include "parser/uni_hid_parser_imu.h"
+#if SWITCH_PICO_WII_IR
+#include "parser/uni_hid_parser_wii_ir.h"
+#endif
#include "controller/uni_controller.h"
#include "hid_usage.h"
@@ -82,6 +85,16 @@
@@ -82,10 +88,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
@ -1293,7 +1296,14 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
WII_FSM_BALANCE_BOARD_READ_CALIBRATION,
WII_FSM_BALANCE_BOARD_READ_CALIBRATION2,
WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION,
@@ -104,16 +117,6 @@
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 +123,6 @@
WII_READ_FROM_REGISTERS = 0x04,
} wii_read_type_t;
@ -1310,7 +1320,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
// balance_board_t represents the data provided by the Balance Board.
typedef struct balance_board_s {
@@ -144,7 +147,6 @@
@@ -144,7 +153,6 @@
// wii_instance_t represents data used by the Wii driver instance.
typedef struct wii_instance_s {
uint8_t state;
@ -1318,7 +1328,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
wii_mode_t mode; /* horizontal, accel, vertical, rumble, etc.. */
enum wii_devtype dev_type;
enum wii_exttype ext_type;
@@ -159,6 +161,29 @@
@@ -159,6 +167,29 @@
uint16_t rumble_duration_ms;
balance_board_calibration_t balance_board_calibration;
@ -1348,7 +1358,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
// Debug only
int debug_fd; // File descriptor where dump is saved
@@ -177,7 +202,6 @@
@@ -177,7 +208,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);
@ -1356,16 +1366,51 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
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 +220,8 @@
@@ -196,6 +226,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 +243,258 @@
@@ -217,124 +284,436 @@
// process_ functions
@ -1402,6 +1447,9 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
+
+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;
@ -1439,6 +1487,175 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
+ 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\n", ir->mode);
+ 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 {
+ // WiiBrew sensitivity level 3. 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, 0xaa, 0x00, 0x64};
+ static const uint8_t sensitivity2[] = {0x63, 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;
@ -1621,6 +1838,12 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
- 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.
}
@ -1734,7 +1957,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
}
static void process_req_data_read_calibration_data(uni_hid_device_t* d, const uint8_t* report, uint16_t len) {
@@ -444,17 +604,83 @@
@@ -444,17 +823,83 @@
// 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) {
@ -1825,7 +2048,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
case WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION:
process_req_data_read_calibration_data(d, report, len);
break;
@@ -465,7 +691,6 @@
@@ -465,7 +910,6 @@
process_req_data_dump_eeprom(d, report, len);
break;
default:
@ -1833,7 +2056,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
break;
}
}
@@ -473,29 +698,38 @@
@@ -473,29 +917,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) {
@ -1863,6 +2086,12 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
+ 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);
@ -1895,7 +2124,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
}
}
@@ -514,15 +748,26 @@
@@ -514,15 +973,26 @@
const uint8_t* data = &report[1];
wii_instance_t* ins = get_wii_instance(d);
@ -1931,7 +2160,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
}
// Process misc buttons
ctl->gamepad.misc_buttons |= (data[1] & 0x80) ? MISC_BUTTON_SYSTEM : 0; // Button "home"
@@ -563,46 +808,97 @@
@@ -563,46 +1033,101 @@
// 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) {
@ -2027,7 +2256,11 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
+
+static void wii_process_motion_extension(uni_hid_device_t* d, const uint8_t* e) {
+ wii_instance_t* ins = get_wii_instance(d);
+ if (ins->state != WII_FSM_LED_UPDATED && ins->state != WII_FSM_MP_VERIFY)
+ 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)
@ -2069,7 +2302,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
}
// Used in WiiMote + Nunchuk Mode
@@ -617,31 +913,17 @@
@@ -617,31 +1142,17 @@
}
wii_instance_t* ins = get_wii_instance(d);
@ -2109,7 +2342,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
// Better if uDraw reports its own "type", but for the moment
// it gets reported and the 2nd half of a gamepad.
@@ -668,59 +950,20 @@
@@ -668,59 +1179,59 @@
ctl->gamepad.buttons |= n.button_lower ? BUTTON_Y : 0;
}
@ -2174,11 +2407,50 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
+ 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 +1245,18 @@
@@ -1002,52 +1513,18 @@
logi("fsm: req_status\n");
wii_instance_t* ins = get_wii_instance(d);
ins->state = WII_FSM_DID_REQ_STATUS;
@ -2236,7 +2508,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
}
static void wii_fsm_ext_read_register(uni_hid_device_t* d) {
@@ -1055,8 +1264,7 @@
@@ -1055,8 +1532,7 @@
wii_instance_t* ins = get_wii_instance(d);
ins->state = WII_FSM_EXT_DID_READ_REGISTER;
@ -2246,7 +2518,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
uint16_t bytes_to_read = 6;
wii_read_mem(d, WII_READ_FROM_REGISTERS, offset, bytes_to_read);
}
@@ -1066,8 +1274,7 @@
@@ -1066,8 +1542,7 @@
wii_instance_t* ins = get_wii_instance(d);
ins->state = WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION;
@ -2256,7 +2528,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
uint16_t bytes_to_read = 16;
wii_read_mem(d, WII_READ_FROM_REGISTERS, offset, bytes_to_read);
}
@@ -1077,8 +1284,7 @@
@@ -1077,8 +1552,7 @@
wii_instance_t* ins = get_wii_instance(d);
ins->state = WII_FSM_BALANCE_BOARD_DID_READ_CALIBRATION2;
@ -2266,7 +2538,18 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
uint16_t bytes_to_read = 8;
wii_read_mem(d, WII_READ_FROM_REGISTERS, offset, bytes_to_read);
}
@@ -1100,18 +1306,8 @@
@@ -1086,6 +1560,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 +1578,8 @@
}
uint8_t report_type = 0xff;
if (ins->ext_type == WII_EXT_NUNCHUK) {
@ -2287,7 +2570,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
} else if (ins->ext_type == WII_EXT_CLASSIC_CONTROLLER) {
logi("Wii: requesting E (Classic Controller)\n");
d->controller_subtype = CONTROLLER_SUBTYPE_WII_CLASSIC;
@@ -1125,20 +1321,11 @@
@@ -1125,21 +1593,17 @@
d->controller_subtype = CONTROLLER_SUBTYPE_WIIMOTE_UDRAW_TABLET;
report_type = WII_REPORT_TYPE_KE;
} else {
@ -2311,9 +2594,15 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
+ 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 +1351,10 @@
}
@@ -1164,7 +1628,10 @@
ins->state = WII_FSM_LED_UPDATED;
wii_process_fsm(d);
@ -2325,7 +2614,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
}
static void wii_fsm_dump_eeprom(struct uni_hid_device_s* d) {
@@ -1218,7 +1408,15 @@
@@ -1218,7 +1685,15 @@
// Do nothing
break;
case WII_FSM_DEV_GUESSED:
@ -2342,7 +2631,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
break;
case WII_FSM_BALANCE_BOARD_READ_CALIBRATION:
wii_fsm_balance_board_read_calibration(d);
@@ -1234,6 +1432,16 @@
@@ -1234,6 +1709,19 @@
wii_fsm_update_led(d);
break;
case WII_FSM_LED_UPDATED:
@ -2356,20 +2645,28 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
+ 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);
@@ -1250,9 +1458,6 @@
@@ -1249,10 +1737,10 @@
ins->mode = WII_MODE_HORIZONTAL;
ins->state = WII_FSM_SETUP;
-
- // 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
@@ -1267,13 +1472,18 @@
@@ -1267,13 +1755,18 @@
}
void uni_hid_parser_wii_init_report(uni_hid_device_t* d) {
@ -2390,7 +2687,22 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
return;
switch (report[0]) {
case WIIPROTO_REQ_STATUS:
@@ -1307,6 +1517,9 @@
@@ -1291,6 +1784,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 +1808,9 @@
logi("Wii parser: unknown report type: 0x%02x\n", report[0]);
printf_hexdump(report, len);
}
@ -2400,7 +2712,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
}
void uni_hid_parser_wii_set_player_leds(uni_hid_device_t* d, uint8_t leds) {
@@ -1371,29 +1584,21 @@
@@ -1371,29 +1875,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);
@ -2436,12 +2748,16 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
+ 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 +1685,13 @@
@@ -1480,10 +1980,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);
@ -2456,7 +2772,7 @@ diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/componen
(offset & 0xff0000) >> 16, (offset & 0xff00) >> 8, (offset & 0xff), // Offset
(size & 0xff00) >> 8, (size & 0xff), // Size in bytes
// clang-format on
@@ -1491,6 +1699,15 @@
@@ -1491,6 +1994,15 @@
uni_hid_device_send_intr_report(d, report, sizeof(report));
}

222
src/firmware/input/bluepad32_input_backend.cpp
View file

@ -2,6 +2,10 @@
#include "bluetooth_transport_config.h"
#include "input/controller_hotkey_config.h"
#include "input/switch2_wake.h"
#ifdef SWITCH_PICO_WII_IR
#include "input/wii_ir_pointer.h"
#include "parser/uni_hid_parser_wii_ir.h"
#endif
#ifdef SWITCH_PICO_NATIVE_SWITCH_RUMBLE
#include "input/switch_native_output.h"
#endif
@ -66,6 +70,11 @@ constexpr uint8_t kProfileFeedbackStrongMagnitude = UINT8_MAX;
constexpr uint32_t kJoyConGestureHoldMs = 2000;
constexpr uint32_t kJoyConGestureFreshMs = 250;
constexpr uint16_t kJoyConGestureFeedbackMs = 75;
#ifdef SWITCH_PICO_WII_IR_GYRO
constexpr uint32_t kWiiAimChordHoldUs = 2000000;
constexpr uint32_t kWiiAimChordFreshUs = 150000;
constexpr uint16_t kWiiAimChordButtons = 0x0002 | 0x0001;
#endif
// One initial indication can be followed by one committed switch before the
// Core 1 timer drains the queue. Profile commits are rate-limited well beyond
// the longest feedback sequence.
@ -170,6 +179,19 @@ struct WiiOrientationRequest {
uint32_t connection_generation;
bool vertical;
};
#ifdef SWITCH_PICO_WII_IR_GYRO
struct WiiAimSource {
uint32_t sequence;
uint32_t last_report_us;
uint32_t started_us;
bool have_sequence;
bool infrared;
bool holding;
bool masked;
bool latched;
bool reposition_masked;
};
#endif
@ -203,6 +225,9 @@ struct BackendSlot {
bool active;
bool wii_orientation_pending;
WiiOrientationRequest pending_wii_orientation;
#ifdef SWITCH_PICO_WII_IR_GYRO
WiiAimSource wii_aim;
#endif
bool rumble_pending;
bool motion_enabled;
bool feedback_pending;
@ -1087,15 +1112,30 @@ void publish_device_state(uint8_t slot, uni_hid_device_t* device,
if (target.active && target.device == device) {
target.state = state;
target.pre_hotkey_button_mask = pre_hotkey_button_mask;
#ifdef SWITCH_PICO_WII_IR_GYRO
if (device->controller_type == CONTROLLER_TYPE_WiiController) {
const ControllerMotionSample sample =
state.motion_sample_count != 0
? state.motion_samples[0] : ControllerMotionSample{};
wii_ir_gyro_update_motion(
slot, target.connection_generation, target.motion_enabled, sample);
if (!target.motion_enabled) {
target.state.motion_sample_count = 0;
}
}
#endif
++target.state_generation;
g_macro_capture.observe(slot, target.connection_generation,
time_us_32(), state);
time_us_32(), target.state);
}
critical_section_exit(&g_state_lock);
}
void publish_all_neutral() {
critical_section_enter_blocking(&g_state_lock);
#ifdef SWITCH_PICO_WII_IR
wii_ir_pointer_reset();
#endif
for (BackendSlot& slot : g_slots) {
clear_switch2_ingress(slot);
reset_switch2_outputs(slot);
@ -1111,6 +1151,9 @@ void publish_all_neutral() {
slot.active = false;
slot.wii_orientation_pending = false;
slot.pending_wii_orientation = {};
#ifdef SWITCH_PICO_WII_IR_GYRO
slot.wii_aim = {};
#endif
slot.rumble_pending = false;
slot.retained_host_rumble_valid = false;
slot.retained_host_rumble = {};
@ -1452,9 +1495,38 @@ void rotate_solo_joycon(uni_gamepad_t& gamepad, int side,
axes[1] = side < 0 ? x : negate_motion_axis(x);
}
}
#ifdef SWITCH_PICO_WII_IR_GYRO
uint32_t wii_aim_chord_button_mask(const uni_hid_device_t* device) {
if (device == nullptr ||
device->controller_type != CONTROLLER_TYPE_WiiController) {
return 0;
}
switch (device->controller_subtype) {
case CONTROLLER_SUBTYPE_WIIMOTE_HORIZONTAL:
case CONTROLLER_SUBTYPE_WIIMOTE_ACCEL:
return BUTTON_A | BUTTON_B;
case CONTROLLER_SUBTYPE_WIIMOTE_VERTICAL:
return BUTTON_X | BUTTON_Y;
case CONTROLLER_SUBTYPE_WIIMOTE_NUNCHUK:
case CONTROLLER_SUBTYPE_WIIMOTE_NUNCHUK_ACCEL:
return BUTTON_SHOULDER_L | BUTTON_SHOULDER_R;
default:
return 0;
}
}
#endif
uni_gamepad_t logical_gamepad(const BackendSlot& slot) {
uni_gamepad_t gamepad = slot.gamepad;
mask_joycon_gesture(gamepad, slot.device);
#ifdef SWITCH_PICO_WII_IR_GYRO
if (slot.wii_aim.masked) {
gamepad.buttons &= ~wii_aim_chord_button_mask(slot.device);
}
if (slot.wii_aim.reposition_masked) {
gamepad.buttons &= ~(BUTTON_X | BUTTON_SHOULDER_L);
}
#endif
if (slot.companion != nullptr) {
const uni_gamepad_t& right = slot.companion_gamepad;
gamepad.dpad |= right.dpad;
@ -1501,11 +1573,103 @@ void queue_local_feedback(BackendSlot& slot, uint16_t duration_ms,
strong_magnitude};
__atomic_add_fetch(&g_local_feedback_requests, 1, __ATOMIC_RELAXED);
}
void queue_profile_feedback(BackendSlot& slot,
const ProfileFeedbackEnvelope& feedback) {
if (slot.pending_profile_feedback_count < kProfileFeedbackQueueCapacity) {
slot.pending_profile_feedback[
slot.pending_profile_feedback_count++] = feedback;
} else {
slot.pending_profile_feedback[kProfileFeedbackQueueCapacity - 1u] =
feedback;
}
}
#ifdef SWITCH_PICO_WII_IR_GYRO
void observe_wii_aim_chord(BackendSlot& slot, uni_hid_device_t* device,
const uni_gamepad_t& gamepad,
const uni_wii_ir_snapshot_t* infrared,
uint32_t now_us) {
const uint32_t mapped_buttons = wii_aim_chord_button_mask(device);
if (mapped_buttons == 0) return;
WiiAimSource& aim = slot.wii_aim;
const uint8_t slot_index = static_cast<uint8_t>(&slot - g_slots);
if (mapped_buttons == (BUTTON_SHOULDER_L | BUTTON_SHOULDER_R)) {
const uint32_t controls = BUTTON_X | BUTTON_SHOULDER_L; // Nunchuk C + 1.
const uint32_t pressed = gamepad.buttons & controls;
if (aim.infrared && pressed == controls) {
aim.reposition_masked = true;
} else if (pressed == 0) {
aim.reposition_masked = false;
}
} else {
aim.reposition_masked = false;
}
// Expire before accepting a returning packet. Cached snapshots cannot
// extend or complete a hold, and a gap requires a full release to retry.
// A transport gap cancels the gesture, not the user's selected source.
// The pointer's freshness guard stops IR output without falling back.
if (aim.have_sequence &&
now_us - aim.last_report_us >= kWiiAimChordFreshUs) {
aim.holding = false;
aim.latched = aim.latched || aim.masked;
}
const uint32_t pressed_buttons = gamepad.buttons & mapped_buttons;
if (pressed_buttons == mapped_buttons) {
aim.masked = true;
} else {
aim.holding = false;
if (pressed_buttons == 0) {
aim.masked = false;
aim.latched = false;
} else {
aim.latched = aim.latched || aim.masked;
}
}
if (infrared == nullptr ||
(aim.have_sequence && infrared->sequence == aim.sequence) ||
(!aim.have_sequence && infrared->sequence == 0)) {
return;
}
aim.have_sequence = true;
aim.sequence = infrared->sequence;
aim.last_report_us = now_us;
const uint16_t buttons = infrared->buttons & kWiiAimChordButtons;
if (buttons != kWiiAimChordButtons || pressed_buttons != mapped_buttons) {
aim.holding = false;
aim.latched = aim.latched || aim.masked;
return;
}
if (aim.latched) return;
if (!aim.holding) {
aim.started_us = now_us;
aim.holding = true;
return;
}
if (now_us - aim.started_us < kWiiAimChordHoldUs) return;
aim.latched = true;
if (!wii_ir_gyro_select(
slot_index, slot.connection_generation, !aim.infrared)) {
return;
}
aim.infrared = !aim.infrared;
queue_profile_feedback(
slot, {slot.connection_generation,
static_cast<uint8_t>(aim.infrared ? 2 : 1),
ControllerProfileConfirmationPolicy::kRumble});
__atomic_add_fetch(&g_local_feedback_requests, 1, __ATOMIC_RELAXED);
}
#endif
void reset_slot_hotkeys(BackendSlot& slot) {
g_macro_capture.disconnect(static_cast<uint8_t>(&slot - g_slots),
slot.connection_generation, time_us_32());
slot.wii_orientation_pending = false;
slot.pending_wii_orientation = {};
#ifdef SWITCH_PICO_WII_IR_GYRO
slot.wii_aim = {};
#endif
slot.motion_enabled = kDefaultMotionEnabled;
slot.pre_hotkey_button_mask = 0;
slot.feedback_pending = false;
@ -1522,6 +1686,9 @@ void reset_slot_hotkeys(BackendSlot& slot) {
}
void invalidate_slot(BackendSlot& slot) {
#ifdef SWITCH_PICO_WII_IR
wii_ir_pointer_disconnect(static_cast<uint8_t>(&slot - g_slots));
#endif
clear_switch2_ingress(slot);
reset_switch2_outputs(slot);
reset_slot_hotkeys(slot);
@ -3102,6 +3269,25 @@ void platform_on_controller_data(uni_hid_device_t* device,
critical_section_exit(&g_state_lock);
return;
}
#ifdef SWITCH_PICO_WII_IR
uni_wii_ir_snapshot_t infrared{};
const bool have_infrared = uni_hid_parser_wii_ir_snapshot(device, &infrared);
#ifdef SWITCH_PICO_WII_IR_GYRO
observe_wii_aim_chord(
slot, device, controller->gamepad,
have_infrared ? &infrared : nullptr, time_us_32());
#endif
if (have_infrared) {
const bool nunchuk_c =
(device->controller_subtype == CONTROLLER_SUBTYPE_WIIMOTE_NUNCHUK ||
device->controller_subtype == CONTROLLER_SUBTYPE_WIIMOTE_NUNCHUK_ACCEL) &&
(controller->gamepad.buttons & BUTTON_X) != 0;
wii_ir_pointer_observe(static_cast<uint8_t>(slot_index),
slot.connection_generation, infrared.sequence,
infrared.buttons, infrared.x, infrared.y,
infrared.valid_mask, nunchuk_c);
}
#endif
const uint8_t extras = uni_hid_parser_switch2_extra_buttons(device);
if (slot.companion == device) {
slot.companion_gamepad = controller->gamepad;
@ -3297,6 +3483,9 @@ void bluepad32_input_backend_init() {
}
critical_section_init(&g_state_lock);
#ifdef SWITCH_PICO_WII_IR
wii_ir_pointer_init();
#endif
configuration_service_prepare();
profile_service_prepare();
#if defined(SWITCH_PICO_HAPTICS_EXPERIMENT) || defined(SWITCH_PICO_NATIVE_SWITCH_RUMBLE)
@ -3569,6 +3758,19 @@ bool bluepad32_input_backend_toggle_motion(
if (slot.active &&
slot.connection_generation == connection_generation) {
slot.motion_enabled = !slot.motion_enabled;
#ifdef SWITCH_PICO_WII_IR_GYRO
if (slot.device != nullptr &&
slot.device->controller_type == CONTROLLER_TYPE_WiiController) {
const ControllerMotionSample sample =
slot.state.motion_sample_count != 0
? slot.state.motion_samples[0] : ControllerMotionSample{};
wii_ir_gyro_update_motion(
slot_index, connection_generation, slot.motion_enabled, sample);
if (!slot.motion_enabled) {
slot.state.motion_sample_count = 0;
}
}
#endif
if (slot.motion_enabled) {
queue_local_feedback(
slot, kMotionEnabledFeedbackDurationMs,
@ -3722,14 +3924,7 @@ bool bluepad32_input_backend_identify(
const ProfileFeedbackEnvelope feedback{
slot.connection_generation, 1,
ControllerProfileConfirmationPolicy::kRumbleAndLed};
if (slot.pending_profile_feedback_count <
kProfileFeedbackQueueCapacity) {
slot.pending_profile_feedback[
slot.pending_profile_feedback_count++] = feedback;
} else {
slot.pending_profile_feedback[
kProfileFeedbackQueueCapacity - 1u] = feedback;
}
queue_profile_feedback(slot, feedback);
queued = true;
break;
}
@ -3755,14 +3950,7 @@ void bluepad32_input_backend_queue_profile_feedback(
slot.connection_generation == connection_generation) {
const ProfileFeedbackEnvelope feedback{
connection_generation, active_profile_number, policy};
if (slot.pending_profile_feedback_count <
kProfileFeedbackQueueCapacity) {
slot.pending_profile_feedback[
slot.pending_profile_feedback_count++] = feedback;
} else {
slot.pending_profile_feedback[
kProfileFeedbackQueueCapacity - 1u] = feedback;
}
queue_profile_feedback(slot, feedback);
}
critical_section_exit(&g_state_lock);
}

508
src/firmware/input/wii_ir_pointer.cpp Normal file
View file

@ -0,0 +1,508 @@
#include "input/wii_ir_pointer.h"
#include "input/wii_ir_tracker.h"
#include <math.h>
#include <string.h>
#include "pico/critical_section.h"
#include "pico/time.h"
namespace {
constexpr uint32_t kStaleUs = 150000;
#ifndef SWITCH_PICO_WII_IR_GYRO
constexpr int32_t kMaximumPendingQ8 = 4096 * 256;
#endif
constexpr uint16_t kButtonA = 0x0008;
constexpr uint16_t kButtonB = 0x0004;
constexpr uint16_t kButtonOne = 0x0002;
constexpr WiiIrCameraModel kCamera{};
#ifdef SWITCH_PICO_WII_IR_GYRO
constexpr float kDegreesPerRadian = 57.295779513f;
WiiIrTracker g_tracker{kCamera, true};
#else
WiiIrTracker g_tracker{kCamera};
#endif
critical_section_t g_lock;
bool g_initialized;
uint8_t g_owner = 0xff;
uint32_t g_connection_generation;
uint32_t g_generation;
uint32_t g_sequence;
uint32_t g_received_us;
uint32_t g_samples;
uint32_t g_sent;
uint32_t g_rebaselines;
uint16_t g_x[4], g_y[4];
uint16_t g_raw_buttons;
uint8_t g_valid_mask;
uint8_t g_buttons;
uint8_t g_sent_buttons;
bool g_have_sample;
bool g_tracking;
bool g_clutch;
int32_t g_filtered_x_q8, g_filtered_y_q8;
int32_t g_pending_x_q8, g_pending_y_q8;
#ifdef SWITCH_PICO_WII_IR_GYRO
constexpr uint32_t kOutputStallUs = 100000;
// Angular position is Q8 at 50 units/degree (12800 units/degree). Spread
// displacement over three 5 ms samples: raw = angular_units * 4457/59904.
constexpr int32_t kGyroNumerator = 4457;
constexpr int32_t kGyroDenominator = 59904;
constexpr int32_t kMaximumGyro = 720 * 13371 / 936;
bool g_infrared;
bool g_motion_enabled;
bool g_output_armed;
bool g_have_gyro_send;
uint32_t g_output_progress_us;
uint32_t g_gyro_sent_us;
uint32_t g_gyro_sent;
int16_t g_sent_gyro_z, g_sent_gyro_y;
ControllerMotionSample g_real_motion{};
// Fractional Q8 units, denominator kGyroNumerator. These retain exactly the
// displacement represented by integral raw gyro values across USB reports.
int32_t g_fraction_x, g_fraction_y;
#endif
int32_t clamp(int32_t value, int32_t limit) {
return value < -limit ? -limit : (value > limit ? limit : value);
}
void stop_tracking(bool reset_model = true) {
if (reset_model) g_tracker.reset();
bool pending = g_pending_x_q8 || g_pending_y_q8;
#ifdef SWITCH_PICO_WII_IR_GYRO
pending = pending || g_fraction_x || g_fraction_y;
g_fraction_x = 0;
g_fraction_y = 0;
#endif
if (g_tracking || pending) {
++g_generation;
++g_rebaselines;
}
g_tracking = false;
g_pending_x_q8 = 0;
g_pending_y_q8 = 0;
}
#ifdef SWITCH_PICO_WII_IR_GYRO
void reset_gyro_source() {
g_infrared = false;
g_output_armed = false;
}
void expire_output(uint32_t now) {
if (g_infrared && g_output_armed &&
static_cast<int32_t>(now - g_output_progress_us) >=
static_cast<int32_t>(kOutputStallUs)) {
stop_tracking();
++g_generation;
g_output_armed = false;
}
}
void set_pending(int32_t& pending, int32_t& fraction, int64_t value) {
// Bound backlog to one report, without clipping ordinary fast sweeps at
// the old 180 deg/s limit. Association rejects outliers before this stage.
constexpr int64_t maximum = static_cast<int64_t>(kMaximumGyro) * kGyroDenominator;
if (value > maximum) value = maximum;
if (value < -maximum) value = -maximum;
pending = static_cast<int32_t>(value / kGyroNumerator);
fraction = static_cast<int32_t>(value % kGyroNumerator);
}
int32_t pending_gyro(int32_t pending, int32_t fraction) {
return (pending * kGyroNumerator + fraction) / kGyroDenominator;
}
int32_t consumed_gyro(int32_t consumed_q8) {
// The integer Q8 displacement uniquely identifies its raw gyro count:
// one raw count spans >13 Q8 units, so recover it by nearest rounding.
int32_t scaled = consumed_q8 * kGyroNumerator;
scaled += scaled < 0 ? -kGyroDenominator / 2 : kGyroDenominator / 2;
return scaled / kGyroDenominator;
}
#endif
void expire(uint32_t now) {
// A USB scheduling timestamp can precede a newer Core 1 sample observed
// before this lock was acquired. A small negative age is not a timeout.
if (g_have_sample && static_cast<int32_t>(now - g_received_us) >=
static_cast<int32_t>(kStaleUs)) {
stop_tracking();
g_buttons = 0;
// Losing IR must not silently select physical gyro. Keep the chosen
// source and emit zero until a fresh tracking baseline is available.
}
}
void put16(uint8_t* p, uint16_t value) {
p[0] = static_cast<uint8_t>(value);
p[1] = static_cast<uint8_t>(value >> 8);
}
void put32(uint8_t* p, uint32_t value) {
put16(p, static_cast<uint16_t>(value));
put16(p + 2, static_cast<uint16_t>(value >> 16));
}
bool claim_owner(uint8_t slot, uint32_t connection_generation) {
if (g_owner != 0xff && g_owner != slot) return false;
if (g_owner != slot || g_connection_generation != connection_generation) {
stop_tracking();
++g_generation;
g_owner = slot;
g_connection_generation = connection_generation;
g_have_sample = false;
#ifdef SWITCH_PICO_WII_IR_GYRO
reset_gyro_source();
g_motion_enabled = false;
g_real_motion = {};
#endif
}
return true;
}
// Caller holds g_lock. Both public diagnostics use this directly rather than
// nesting public functions on the same non-recursive striped spinlock.
void pointer_diagnostics(uint8_t* data, uint32_t now) {
data[0] = 1;
data[1] = g_owner;
data[2] = g_valid_mask;
data[3] = (g_tracking ? 1 : 0) | (g_clutch ? 2 : 0) |
(g_have_sample && now - g_received_us < kStaleUs ? 4 : 0);
put32(data + 4, g_sequence);
put32(data + 8, g_samples);
put32(data + 12, g_sent);
for (unsigned i = 0; i < 4; ++i) {
put16(data + 16 + 4 * i, g_x[i]);
put16(data + 18 + 4 * i, g_y[i]);
}
put16(data + 32, g_raw_buttons);
data[34] = g_buttons;
data[35] = g_sent_buttons;
put32(data + 36, g_have_sample ? now - g_received_us : UINT32_MAX);
put32(data + 40, g_rebaselines);
put16(data + 44, static_cast<uint16_t>(g_pending_x_q8 / 256));
put16(data + 46, static_cast<uint16_t>(g_pending_y_q8 / 256));
}
} // namespace
void wii_ir_pointer_init() {
if (g_initialized) return;
// The backend holds an exclusive lock while calling us. Do not consume
// another scarce exclusive lock or nest any striped lock inside this one.
critical_section_init_with_lock_num(&g_lock, next_striped_spin_lock_num());
g_initialized = true;
}
void wii_ir_pointer_reset() {
if (!g_initialized) return;
critical_section_enter_blocking(&g_lock);
stop_tracking();
++g_generation;
g_buttons = 0;
g_owner = 0xff;
// Force an explicit release after reset if the host saw a held button.
g_have_sample = false;
g_valid_mask = 0;
#ifdef SWITCH_PICO_WII_IR_GYRO
reset_gyro_source();
g_motion_enabled = false;
g_real_motion = {};
#endif
critical_section_exit(&g_lock);
}
void wii_ir_pointer_disconnect(uint8_t slot) {
if (!g_initialized) return;
critical_section_enter_blocking(&g_lock);
if (g_owner == slot) {
stop_tracking();
++g_generation;
g_owner = 0xff;
g_buttons = 0;
g_have_sample = false;
g_valid_mask = 0;
#ifdef SWITCH_PICO_WII_IR_GYRO
reset_gyro_source();
g_motion_enabled = false;
g_real_motion = {};
#endif
}
critical_section_exit(&g_lock);
}
void wii_ir_pointer_observe(uint8_t slot, uint32_t connection_generation,
uint32_t sequence, uint16_t buttons,
const uint16_t x[4], const uint16_t y[4],
uint8_t valid_mask, bool nunchuk_c) {
if (!g_initialized) return;
const uint32_t now = time_us_32();
critical_section_enter_blocking(&g_lock);
if (!claim_owner(slot, connection_generation)) {
critical_section_exit(&g_lock);
return;
}
if (g_have_sample && sequence == g_sequence) {
critical_section_exit(&g_lock);
return;
}
expire(now);
#ifdef SWITCH_PICO_WII_IR_GYRO
expire_output(now);
#endif
g_have_sample = true;
g_sequence = sequence;
g_received_us = now;
++g_samples;
memcpy(g_x, x, sizeof(g_x));
memcpy(g_y, y, sizeof(g_y));
g_raw_buttons = buttons;
g_valid_mask = valid_mask & 0x0f;
g_buttons = ((buttons & kButtonA) ? 1 : 0) | ((buttons & kButtonB) ? 2 : 0);
g_clutch = (buttons & kButtonOne) != 0;
#ifdef SWITCH_PICO_WII_IR_GYRO
// C + 1 is the reposition chord. Keep 1 + 2 motionless while the
// separate source-switch gesture is held; 1 alone remains a game button.
g_clutch = g_clutch && (nunchuk_c || (buttons & 0x0001) != 0);
#else
(void)nunchuk_c;
#endif
bool usable = !g_clutch;
#ifdef SWITCH_PICO_WII_IR_GYRO
usable = usable && (!g_infrared || g_motion_enabled);
#endif
if (!usable) {
stop_tracking();
critical_section_exit(&g_lock);
return;
}
const WiiIrTrackingResult tracked = g_tracker.update(x, y, g_valid_mask, now);
if (!tracked.tracked) {
// Discard output, but retain bounded association history so a brief
// occlusion does not make another reflection become the reference.
stop_tracking(false);
critical_section_exit(&g_lock);
return;
}
#ifdef SWITCH_PICO_WII_IR_GYRO
constexpr float kUnitsPerRadian = 12800.0f * kDegreesPerRadian;
// Conservative pitch tuning against the physical MotionPlus reference.
// Keep camera geometry and the working horizontal response unchanged.
constexpr float kPitchAimGain = 1.5f;
const int32_t px = static_cast<int32_t>(lroundf(tracked.yaw_radians * kUnitsPerRadian));
const int32_t py = static_cast<int32_t>(
lroundf(tracked.pitch_radians * (kUnitsPerRadian * kPitchAimGain)));
#else
// Mouse output keeps roughly two counts/pixel at the image center, but
// uses the same angular tracking and filtering as gyro output.
const int32_t px = static_cast<int32_t>(lroundf(tracked.yaw_radians * kCamera.fx * 512.0f));
const int32_t py = static_cast<int32_t>(lroundf(tracked.pitch_radians * kCamera.fy * 512.0f));
#endif
if (!g_tracking || tracked.rebased) {
stop_tracking(false);
g_tracking = true;
g_filtered_x_q8 = px;
g_filtered_y_q8 = py;
++g_rebaselines;
} else {
const int32_t dx = px - g_filtered_x_q8;
const int32_t dy = py - g_filtered_y_q8;
g_filtered_x_q8 = px;
g_filtered_y_q8 = py;
#ifdef SWITCH_PICO_WII_IR_GYRO
if (g_infrared && g_motion_enabled && g_output_armed) {
set_pending(g_pending_x_q8, g_fraction_x,
(static_cast<int64_t>(g_pending_x_q8) + dx) *
kGyroNumerator + g_fraction_x);
set_pending(g_pending_y_q8, g_fraction_y,
(static_cast<int64_t>(g_pending_y_q8) + dy) *
kGyroNumerator + g_fraction_y);
}
#else
g_pending_x_q8 = clamp(g_pending_x_q8 + dx, kMaximumPendingQ8);
g_pending_y_q8 = clamp(g_pending_y_q8 + dy, kMaximumPendingQ8);
#endif
}
critical_section_exit(&g_lock);
}
bool wii_ir_mouse_peek(WiiIrMouseReport* report) {
if (!g_initialized || report == nullptr) return false;
critical_section_enter_blocking(&g_lock);
expire(time_us_32());
report->dx = static_cast<int8_t>(clamp(g_pending_x_q8 / 256, 127));
report->dy = static_cast<int8_t>(clamp(g_pending_y_q8 / 256, 127));
report->buttons = g_buttons;
report->generation = g_generation;
const bool pending = report->dx || report->dy || g_buttons != g_sent_buttons;
critical_section_exit(&g_lock);
return pending;
}
void wii_ir_mouse_commit(const WiiIrMouseReport& report) {
if (!g_initialized) return;
critical_section_enter_blocking(&g_lock);
if (report.generation == g_generation) {
g_pending_x_q8 -= static_cast<int32_t>(report.dx) * 256;
g_pending_y_q8 -= static_cast<int32_t>(report.dy) * 256;
}
// Even a raced reset must release buttons that USB actually accepted.
g_sent_buttons = report.buttons;
++g_sent;
critical_section_exit(&g_lock);
}
size_t wii_ir_pointer_diagnostics(uint8_t* buffer, size_t capacity) {
if (!g_initialized || buffer == nullptr) return 0;
uint8_t data[WII_IR_MOUSE_DIAGNOSTIC_SIZE]{};
critical_section_enter_blocking(&g_lock);
const uint32_t now = time_us_32();
expire(now);
pointer_diagnostics(data, now);
critical_section_exit(&g_lock);
const size_t size = capacity < sizeof(data) ? capacity : sizeof(data);
memcpy(buffer, data, size);
return size;
}
#ifdef SWITCH_PICO_WII_IR_GYRO
bool wii_ir_gyro_select(uint8_t slot, uint32_t connection_generation,
bool infrared) {
if (!g_initialized) return false;
critical_section_enter_blocking(&g_lock);
if (!claim_owner(slot, connection_generation)) {
critical_section_exit(&g_lock);
return false;
}
expire(time_us_32());
if (g_infrared != infrared) {
stop_tracking();
++g_generation;
g_infrared = infrared;
g_output_armed = false;
}
critical_section_exit(&g_lock);
return true;
}
void wii_ir_gyro_update_motion(uint8_t slot, uint32_t connection_generation,
bool enabled, const ControllerMotionSample& sample) {
if (!g_initialized) return;
critical_section_enter_blocking(&g_lock);
if (claim_owner(slot, connection_generation)) {
const uint32_t now = time_us_32();
expire(now);
expire_output(now);
if (g_motion_enabled != enabled) {
stop_tracking();
++g_generation;
g_motion_enabled = enabled;
}
// Physical gyro stays in the normal controller state. IR prepare
// writes all gyro axes itself and uses only these real accel axes.
g_real_motion.accel_x = sample.accel_x;
g_real_motion.accel_y = sample.accel_y;
g_real_motion.accel_z = sample.accel_z;
}
critical_section_exit(&g_lock);
}
void wii_ir_gyro_reset_output() {
if (!g_initialized) return;
critical_section_enter_blocking(&g_lock);
stop_tracking();
++g_generation;
g_output_armed = false;
g_output_progress_us = 0;
critical_section_exit(&g_lock);
}
bool wii_ir_gyro_prepare(uint8_t slot, uint32_t now_us, ControllerState* state,
WiiIrGyroReport* report) {
if (!g_initialized || state == nullptr || report == nullptr) return false;
critical_section_enter_blocking(&g_lock);
expire(now_us);
expire_output(now_us);
if (g_owner != slot || !g_infrared) {
critical_section_exit(&g_lock);
return false;
}
int32_t raw_x = 0, raw_y = 0;
if (g_motion_enabled && g_tracking && !g_clutch && g_output_armed) {
raw_x = pending_gyro(g_pending_x_q8, g_fraction_x);
raw_y = pending_gyro(g_pending_y_q8, g_fraction_y);
}
ControllerMotionSample sample{};
if (g_motion_enabled) {
sample.accel_x = g_real_motion.accel_x;
sample.accel_y = g_real_motion.accel_y;
sample.accel_z = g_real_motion.accel_z;
// Match the physical Wii yaw axis after Bluepad32 normalization:
// horizontal camera travel rotates Z, not the longitudinal roll X.
sample.gyro_z = static_cast<int16_t>(-raw_x);
sample.gyro_y = static_cast<int16_t>(raw_y);
}
state->motion_sample_count =
g_motion_enabled ? CONTROLLER_MOTION_SAMPLE_CAPACITY : 0;
for (ControllerMotionSample& destination : state->motion_samples) {
destination = sample;
}
report->generation = g_generation;
report->consumed_x_q8 = raw_x * kGyroDenominator / kGyroNumerator;
report->consumed_y_q8 = raw_y * kGyroDenominator / kGyroNumerator;
report->submitted_us = now_us;
critical_section_exit(&g_lock);
return true;
}
void wii_ir_gyro_commit(const WiiIrGyroReport& report) {
if (!g_initialized) return;
const int32_t raw_x = consumed_gyro(report.consumed_x_q8);
const int32_t raw_y = consumed_gyro(report.consumed_y_q8);
critical_section_enter_blocking(&g_lock);
if (report.generation == g_generation && g_infrared) {
set_pending(g_pending_x_q8, g_fraction_x,
g_pending_x_q8 * kGyroNumerator + g_fraction_x -
raw_x * kGyroDenominator);
set_pending(g_pending_y_q8, g_fraction_y,
g_pending_y_q8 * kGyroNumerator + g_fraction_y -
raw_y * kGyroDenominator);
if (!g_output_armed) {
// Start from the next fresh camera baseline, not motion collected
// while the host was negotiating or unavailable.
stop_tracking();
g_output_armed = true;
}
g_output_progress_us = report.submitted_us;
}
// These fields describe what USB actually accepted, including a report
// whose generation raced a source switch or tracking rebaseline.
g_sent_gyro_y = static_cast<int16_t>(raw_y);
g_sent_gyro_z = static_cast<int16_t>(-raw_x);
g_gyro_sent_us = report.submitted_us;
g_have_gyro_send = true;
++g_gyro_sent;
critical_section_exit(&g_lock);
}
size_t wii_ir_gyro_diagnostics(uint8_t* output, size_t capacity) {
if (!g_initialized || output == nullptr) return 0;
uint8_t data[WII_IR_GYRO_DIAGNOSTIC_SIZE]{};
critical_section_enter_blocking(&g_lock);
const uint32_t now = time_us_32();
expire(now);
expire_output(now);
pointer_diagnostics(data, now);
data[48] = g_infrared ? 1 : 0;
data[49] = g_motion_enabled ? 1 : 0;
put16(data + 52, static_cast<uint16_t>(g_sent_gyro_y));
put16(data + 54, static_cast<uint16_t>(g_sent_gyro_z));
put32(data + 56, g_gyro_sent);
put32(data + 60, g_have_gyro_send ? now - g_gyro_sent_us : UINT32_MAX);
critical_section_exit(&g_lock);
const size_t size = capacity < sizeof(data) ? capacity : sizeof(data);
memcpy(output, data, size);
return size;
}
#endif

66
src/firmware/input/wii_ir_pointer.h Normal file
View file

@ -0,0 +1,66 @@
#pragma once
#include <stddef.h>
#include <stdint.h>
#include "core/controller_state.h"
struct WiiIrMouseReport {
int8_t dx;
int8_t dy;
uint8_t buttons;
uint32_t generation;
};
// Core 0 initializes before starting Bluetooth. The producer runs on Core 1;
// USB peeks/commits on Core 0, preserving movement until a successful send.
void wii_ir_pointer_init();
void wii_ir_pointer_reset();
bool wii_ir_mouse_peek(WiiIrMouseReport* report);
void wii_ir_mouse_commit(const WiiIrMouseReport& report);
constexpr size_t WII_IR_MOUSE_DIAGNOSTIC_SIZE = 48;
size_t wii_ir_pointer_diagnostics(uint8_t* buffer, size_t capacity);
// The first Wii source owns the experimental pointer until disconnected.
void wii_ir_pointer_disconnect(uint8_t slot);
// nunchuk_c is the unmapped physical C state, false without a Nunchuk.
void wii_ir_pointer_observe(uint8_t slot, uint32_t connection_generation,
uint32_t sequence, uint16_t buttons,
const uint16_t x[4], const uint16_t y[4],
uint8_t valid_mask, bool nunchuk_c);
#ifdef SWITCH_PICO_WII_IR_GYRO
struct WiiIrGyroReport {
uint32_t generation;
int32_t consumed_x_q8;
int32_t consumed_y_q8;
uint32_t submitted_us;
};
// Real gyro is the default. Selection is connection-local, never persisted.
// A different live Wii owner rejects selection; changing sources rebaselines.
bool wii_ir_gyro_select(uint8_t slot, uint32_t connection_generation,
bool infrared);
void wii_ir_gyro_update_motion(uint8_t slot, uint32_t connection_generation,
bool enabled, const ControllerMotionSample& sample);
// USB reconnect/reenumeration discards pending motion without changing source.
void wii_ir_gyro_reset_output();
// Call only for a due regular 0x30 report. False leaves real motion untouched.
// True writes the motion count and all three 5 ms samples without reading state,
// retaining the latest real accelerometer (or no motion when disabled).
// Uses a nominal 33 x 23 degree pinhole model, not a measured unit calibration.
// Gyro pending positions are Q8 at 50 units/degree (12800 units/degree).
// Consumed fields carry represented whole units; commit retains the fraction.
// Only a successful USB queue may commit. No successful send for 100 ms drops
// pending motion; backlog above one 720 degree/second report is discarded.
bool wii_ir_gyro_prepare(uint8_t slot, uint32_t now_us, ControllerState* state,
WiiIrGyroReport* report);
void wii_ir_gyro_commit(const WiiIrGyroReport& report);
constexpr size_t WII_IR_GYRO_DIAGNOSTIC_SIZE = 64;
// Bytes 0..47 retain the pointer diagnostic format. Bytes 48/49 are IR source
// and motion-enabled flags. Schema 2 LE fields: 50 gyro_x i16, 52 gyro_y i16,
// 54 gyro_z i16, 56 successful IR reports u32, 60 last send age_us u32.
size_t wii_ir_gyro_diagnostics(uint8_t* output, size_t capacity);
#endif

421
src/firmware/input/wii_ir_tracker.cpp Normal file
View file

@ -0,0 +1,421 @@
#include "input/wii_ir_tracker.h"
#include <math.h>
namespace {
constexpr float kPi = 3.14159265358979323846f;
constexpr float kRadiansToDegrees = 180.0f / kPi;
constexpr float kColdMinimumSpan = 24.0f;
constexpr float kTrackingMinimumSpan = 12.0f;
constexpr uint32_t kMaximumGapUs = 150000;
constexpr uint32_t kSingleMarkerUs = 80000;
constexpr float kAmbiguityMarginSquared = 64.0f;
float distance_squared(float ax, float ay, float bx, float by) {
const float dx = ax - bx;
const float dy = ay - by;
return dx * dx + dy * dy;
}
float endpoint_gate(uint32_t elapsed_us) {
return 12.0f + 0.006f * static_cast<float>(elapsed_us);
}
float lowpass_alpha(float cutoff, float dt) {
const float scaled = 2.0f * kPi * cutoff * dt;
return scaled / (1.0f + scaled);
}
bool unambiguous(float best, float runner_up) {
// A close reflection is not evidence of which physical endpoint survived.
return runner_up > best * 1.25f + kAmbiguityMarginSquared;
}
} // namespace
WiiIrTracker::WiiIrTracker(WiiIrCameraModel model, bool relative_motion)
: model_(model), relative_motion_(relative_motion) {
model_valid_ = isfinite(model_.fx) && model_.fx > 0 &&
isfinite(model_.fy) && model_.fy > 0 &&
isfinite(model_.cx) && isfinite(model_.cy);
if (model_valid_) {
inverse_fx_ = 1.0f / model_.fx;
inverse_fy_ = 1.0f / model_.fy;
model_valid_ = isfinite(inverse_fx_) && inverse_fx_ > 0 &&
isfinite(inverse_fy_) && inverse_fy_ > 0;
}
}
void WiiIrTracker::reset() {
geometry_ = {};
candidate_ = {};
yaw_ = {};
pitch_ = {};
geometry_us_ = 0;
full_pair_us_ = 0;
candidate_us_ = 0;
sample_us_ = 0;
have_geometry_ = false;
have_candidate_ = false;
have_timestamp_ = false;
output_valid_ = false;
previous_count_ = 0;
was_inferred_ = false;
}
WiiIrTrackingResult WiiIrTracker::lose() {
output_valid_ = false;
have_candidate_ = false;
// Retain only previously accepted geometry, never an outlier or prediction.
return {};
}
bool WiiIrTracker::make_pair(const Point& first, const Point& second,
uint8_t mask, float minimum_span,
bool gravity_valid, float gravity_x, float gravity_y,
Pair& pair) {
const float span_squared = distance_squared(first.x, first.y, second.x, second.y);
if (span_squared < minimum_span * minimum_span) return false;
const float dx = second.nx - first.nx;
const float dy = second.ny - first.ny;
const float normalized_span_squared = dx * dx + dy * dy;
if (!(normalized_span_squared > 0) || !isfinite(normalized_span_squared)) {
return false;
}
if (gravity_valid) {
const float dot = dx * gravity_x + dy * gravity_y;
// Gravity is only a modulo-pi geometry prior. Never use it to reorder
// an established pair, including when rolling through 90 degrees.
if (dot * dot < 0.5f * normalized_span_squared) return false;
}
pair.first = first;
pair.second = second;
pair.pixel_span = sqrtf(span_squared);
pair.mask = mask;
return true;
}
bool WiiIrTracker::match_pair(const Point points[4], const uint8_t masks[4],
unsigned count, const Pair& reference,
float gate, float minimum_span,
bool gravity_valid, float gravity_x, float gravity_y,
Pair& matched) {
const float gate_squared = gate * gate;
const float span_gate = 0.30f * reference.pixel_span + 20.0f;
float best = INFINITY;
float runner_up = INFINITY;
bool found = false;
for (unsigned i = 0; i < count; ++i) {
for (unsigned j = i + 1; j < count; ++j) {
Pair pair;
if (!make_pair(points[i], points[j], masks[i] | masks[j], minimum_span,
gravity_valid, gravity_x, gravity_y, pair) ||
fabsf(pair.pixel_span - reference.pixel_span) > span_gate) {
continue;
}
for (unsigned ordering = 0; ordering < 2; ++ordering) {
const float first_distance = distance_squared(
pair.first.x, pair.first.y, reference.first.x, reference.first.y);
const float second_distance = distance_squared(
pair.second.x, pair.second.y, reference.second.x, reference.second.y);
if (first_distance <= gate_squared && second_distance <= gate_squared) {
const float score = first_distance + second_distance;
if (score < best) {
runner_up = best;
best = score;
matched = pair;
found = true;
} else if (score < runner_up) {
runner_up = score;
}
}
const Point temporary = pair.first;
pair.first = pair.second;
pair.second = temporary;
}
}
}
return found && unambiguous(best, runner_up);
}
float WiiIrTracker::filter_axis(AxisFilter& state, float radians, float dt,
float inverse_dt, float velocity_alpha, bool rebased) {
if (rebased) {
state.raw = radians;
state.filtered = radians;
state.velocity = 0;
state.output = radians;
return radians;
}
// The caller admits only strictly increasing timestamps. Estimate speed
// from unfiltered angles so a fast intentional motion raises the cutoff
// before the position low-pass would otherwise suppress it.
const float velocity = (radians - state.raw) * inverse_dt;
state.raw = radians;
state.velocity += velocity_alpha * (velocity - state.velocity);
const float cutoff = fminf(45.0f, 3.0f + 0.2f * fabsf(state.velocity) * kRadiansToDegrees);
state.filtered += lowpass_alpha(cutoff, dt) * (radians - state.filtered);
// A play operator, not a per-frame dead zone: slow sub-threshold movement
// accumulates in the absolute filter state until it crosses the boundary.
constexpr float hysteresis = 0.01f / kRadiansToDegrees;
const float difference = state.filtered - state.output;
if (difference > hysteresis) {
state.output = state.filtered - hysteresis;
} else if (difference < -hysteresis) {
state.output = state.filtered + hysteresis;
}
return state.output;
}
WiiIrTrackingResult WiiIrTracker::accept(const Pair& pair, uint32_t now_us,
bool inferred) {
const float dx = pair.second.nx - pair.first.nx;
const float dy = pair.second.ny - pair.first.ny;
const float normalized_span = sqrtf(dx * dx + dy * dy);
if (!(normalized_span > 0) || !isfinite(normalized_span)) return lose();
const float inverse_span = 1.0f / normalized_span;
if (!isfinite(inverse_span)) return lose();
const float cosine = dx * inverse_span;
const float sine = dy * inverse_span;
const float mx = 0.5f * pair.first.nx + 0.5f * pair.second.nx;
const float my = 0.5f * pair.first.ny + 0.5f * pair.second.ny;
const float mx_rotated = mx * cosine + my * sine;
const float my_rotated = -mx * sine + my * cosine;
if (!isfinite(mx_rotated) || !isfinite(my_rotated)) return lose();
const float yaw = -atan2f(mx_rotated, 1.0f);
const float pitch = -atan2f(my_rotated, hypotf(1.0f, mx_rotated));
const bool rebased = !output_valid_ || !have_geometry_ || (was_inferred_ && !inferred);
const float dt = static_cast<float>(now_us - geometry_us_) * 0.000001f;
const float inverse_dt = rebased ? 0.0f : 1.0f / dt;
const float velocity_alpha = rebased ? 0.0f : lowpass_alpha(10.0f, dt);
WiiIrTrackingResult result;
result.tracked = true;
result.rebased = rebased;
result.inferred = inferred;
result.pair_mask = pair.mask;
result.yaw_radians = filter_axis(yaw_, yaw, dt, inverse_dt, velocity_alpha, rebased);
result.pitch_radians = filter_axis(pitch_, pitch, dt, inverse_dt, velocity_alpha, rebased);
result.range_in_bar_widths = inverse_span;
geometry_ = pair;
geometry_us_ = now_us;
if (!inferred) full_pair_us_ = now_us;
have_geometry_ = true;
have_candidate_ = false;
output_valid_ = true;
was_inferred_ = inferred;
return result;
}
WiiIrTrackingResult WiiIrTracker::follow_spots(
const Point points[4], const uint8_t masks[4], unsigned count, uint32_t now_us) {
struct Flow {
float dx, dy, yaw, pitch;
uint8_t mask;
};
Flow flows[4]{};
unsigned matches = 0;
const uint32_t elapsed = now_us - geometry_us_;
if (output_valid_ && have_geometry_) {
const float gate = endpoint_gate(elapsed);
for (unsigned i = 0; i < count; ++i) {
float best = INFINITY, runner_up = INFINITY;
unsigned previous = 0;
for (unsigned j = 0; j < previous_count_; ++j) {
const float distance = distance_squared(
points[i].x, points[i].y, previous_points_[j].x, previous_points_[j].y);
if (distance < best) {
runner_up = best;
best = distance;
previous = j;
} else if (distance < runner_up) {
runner_up = distance;
}
}
if (best > gate * gate || !unambiguous(best, runner_up)) continue;
const Point& old = previous_points_[previous];
float alternative = INFINITY;
for (unsigned j = 0; j < count; ++j) {
if (j == i) continue;
alternative = fminf(alternative, distance_squared(
points[j].x, points[j].y, old.x, old.y));
}
// Mutual, unambiguous matches prevent two current reflections
// from inheriting the same old spot, regardless of camera slot.
if (!unambiguous(best, alternative)) continue;
flows[matches++] = {
points[i].x - old.x, points[i].y - old.y,
-atan2f(points[i].nx - old.nx, 1.0f + points[i].nx * old.nx),
-atan2f(points[i].ny - old.ny, 1.0f + points[i].ny * old.ny),
masks[i]};
}
}
unsigned best_count = 0;
uint8_t best_set = 0;
for (unsigned i = 0; i < matches; ++i) {
const float tolerance = 3.0f + 0.15f * hypotf(flows[i].dx, flows[i].dy);
unsigned inliers = 0;
uint8_t set = 0;
for (unsigned j = 0; j < matches; ++j) {
if (distance_squared(flows[i].dx, flows[i].dy, flows[j].dx, flows[j].dy) <=
tolerance * tolerance) {
++inliers;
set |= static_cast<uint8_t>(1u << j);
}
}
if (inliers > best_count) {
best_count = inliers;
best_set = set;
}
}
// A lone continuous spot is sufficient. Conflicting matches need a
// strict majority: never select one of two contradictory movements.
const bool rebased = best_count == 0 || best_count <= matches / 2;
float yaw = 0, pitch = 0;
uint8_t used_mask = 0;
if (!rebased) {
for (unsigned i = 0; i < matches; ++i) {
if (!(best_set & (1u << i))) continue;
yaw += flows[i].yaw;
pitch += flows[i].pitch;
used_mask |= flows[i].mask;
}
yaw = yaw_.raw + yaw / best_count;
pitch = pitch_.raw + pitch / best_count;
}
const float dt = static_cast<float>(elapsed) * 0.000001f;
const float inverse_dt = rebased ? 0.0f : 1.0f / dt;
const float velocity_alpha = rebased ? 0.0f : lowpass_alpha(10.0f, dt);
WiiIrTrackingResult result;
result.tracked = true;
result.rebased = rebased;
result.pair_mask = used_mask;
result.yaw_radians = filter_axis(yaw_, yaw, dt, inverse_dt, velocity_alpha, rebased);
result.pitch_radians = filter_axis(pitch_, pitch, dt, inverse_dt, velocity_alpha, rebased);
for (unsigned i = 0; i < count; ++i) previous_points_[i] = points[i];
previous_count_ = count;
geometry_us_ = now_us;
have_geometry_ = true;
output_valid_ = true;
return result;
}
WiiIrTrackingResult WiiIrTracker::update(const uint16_t x[4], const uint16_t y[4],
uint8_t valid_mask, uint32_t now_us,
float gravity_roll_radians, bool gravity_valid) {
if (!model_valid_ || !x || !y) return lose();
if (have_timestamp_) {
const uint32_t elapsed = now_us - sample_us_;
// Unsigned subtraction handles timer wrap. Duplicate or backwards
// samples cannot advance geometry, cold acquisition, or the filters.
if (elapsed == 0 || elapsed > 0x7fffffffu) return lose();
}
sample_us_ = now_us;
have_timestamp_ = true;
if (have_geometry_ && now_us - geometry_us_ > kMaximumGapUs) {
have_geometry_ = false;
have_candidate_ = false;
output_valid_ = false;
}
Point points[4];
uint8_t masks[4];
unsigned count = 0;
for (unsigned i = 0; i < 4; ++i) {
if (!(valid_mask & (1u << i)) || x[i] > 1023 || y[i] > 767) continue;
Point point;
point.x = static_cast<float>(x[i]);
point.y = static_cast<float>(y[i]);
point.nx = (point.x - model_.cx) * inverse_fx_;
point.ny = (point.y - model_.cy) * inverse_fy_;
if (!isfinite(point.nx) || !isfinite(point.ny)) continue;
points[count] = point;
masks[count] = static_cast<uint8_t>(1u << i);
++count;
}
if (count == 0) return lose();
if (relative_motion_) return follow_spots(points, masks, count, now_us);
// The caller gates acceleration magnitude and projection. Invalid dynamic
// acceleration (or a nonfinite angle) must never switch off optical tracking.
gravity_valid = count >= 2 && gravity_valid && isfinite(gravity_roll_radians);
const float gravity_x = gravity_valid ? cosf(gravity_roll_radians) : 0.0f;
const float gravity_y = gravity_valid ? sinf(gravity_roll_radians) : 0.0f;
if (!have_geometry_) {
// With no temporal reference, three or four points do not identify the
// physical bar reliably. Wait rather than guess a plausible reflection.
if (count != 2) return lose();
if (have_candidate_ && now_us - candidate_us_ <= kMaximumGapUs) {
Pair matched;
if (match_pair(points, masks, count, candidate_, endpoint_gate(now_us - candidate_us_),
kColdMinimumSpan, gravity_valid, gravity_x, gravity_y, matched)) {
return accept(matched, now_us, false);
}
}
Pair pair;
if (!make_pair(points[0], points[1], masks[0] | masks[1], kColdMinimumSpan,
gravity_valid, gravity_x, gravity_y, pair)) {
return lose();
}
const float dx = pair.second.nx - pair.first.nx;
const float dy = pair.second.ny - pair.first.ny;
const bool reverse = gravity_valid ? dx * gravity_x + dy * gravity_y < 0 :
(dx < 0 || (dx == 0 && dy < 0));
if (reverse) {
const Point temporary = pair.first;
pair.first = pair.second;
pair.second = temporary;
}
candidate_ = pair;
candidate_us_ = now_us;
have_candidate_ = true;
output_valid_ = false;
return {};
}
const float gate = endpoint_gate(now_us - geometry_us_);
if (count >= 2) {
Pair matched;
if (!match_pair(points, masks, count, geometry_, gate, kTrackingMinimumSpan,
gravity_valid, gravity_x, gravity_y, matched)) {
return lose();
}
return accept(matched, now_us, false);
}
if (now_us - full_pair_us_ > kSingleMarkerUs) return lose();
const float first_distance = distance_squared(
points[0].x, points[0].y, geometry_.first.x, geometry_.first.y);
const float second_distance = distance_squared(
points[0].x, points[0].y, geometry_.second.x, geometry_.second.y);
const bool first_visible = first_distance < second_distance;
const float best = first_visible ? first_distance : second_distance;
const float runner_up = first_visible ? second_distance : first_distance;
if (best > gate * gate || !unambiguous(best, runner_up)) return lose();
// Translate the last accepted full-pair vector without changing its span or
// rotation. Repeated inferred frames do NOT extend the full-pair deadline.
Pair inferred = geometry_;
const Point& previous = first_visible ? geometry_.first : geometry_.second;
const float delta_x = points[0].x - previous.x;
const float delta_y = points[0].y - previous.y;
const float delta_nx = delta_x * inverse_fx_;
const float delta_ny = delta_y * inverse_fy_;
inferred.first.x += delta_x;
inferred.first.y += delta_y;
inferred.first.nx += delta_nx;
inferred.first.ny += delta_ny;
inferred.second.x += delta_x;
inferred.second.y += delta_y;
inferred.second.nx += delta_nx;
inferred.second.ny += delta_ny;
inferred.mask = masks[0];
return accept(inferred, now_us, true);
}

95
src/firmware/input/wii_ir_tracker.h Normal file
View file

@ -0,0 +1,95 @@
#pragma once
#include <stdint.h>
// Nominal WiiBrew field of view (33 degrees horizontal, 23 vertical), not
// calibration of the attached camera. Coordinates are the native 1024 x 768.
struct WiiIrCameraModel {
float fx = 1726.7951f;
float fy = 1884.9627f;
float cx = 511.5f;
float cy = 383.5f;
};
struct WiiIrTrackingResult {
bool tracked = false;
bool rebased = false;
bool inferred = false;
// Matched spots: relative mode may use one to four, not just a bar pair.
uint8_t pair_mask = 0;
float yaw_radians = 0;
float pitch_radians = 0;
// Reciprocal normalized image span: a frontal bar-width proxy, not meters.
float range_in_bar_widths = 0;
};
// Caller owns synchronization. All history is bounded; no allocation or gyro
// measurements are used. A rebased result must not generate an angular delta.
class WiiIrTracker {
public:
// Relative motion follows persistent spots without identifying bar ends.
// Its bearings are accumulated in the camera frame; range is unavailable.
explicit WiiIrTracker(WiiIrCameraModel model = {}, bool relative_motion = false);
void reset();
WiiIrTrackingResult update(const uint16_t x[4], const uint16_t y[4],
uint8_t valid_mask, uint32_t now_us,
float gravity_roll_radians = 0,
bool gravity_valid = false);
private:
struct Point {
float x = 0;
float y = 0;
float nx = 0;
float ny = 0;
};
struct Pair {
Point first{};
Point second{};
float pixel_span = 0;
uint8_t mask = 0;
};
struct AxisFilter {
float raw = 0;
float filtered = 0;
float velocity = 0;
float output = 0;
};
static bool make_pair(const Point& first, const Point& second,
uint8_t mask, float minimum_span,
bool gravity_valid, float gravity_x, float gravity_y,
Pair& pair);
static bool match_pair(const Point points[4], const uint8_t masks[4],
unsigned count, const Pair& reference,
float endpoint_gate, float minimum_span,
bool gravity_valid, float gravity_x, float gravity_y,
Pair& matched);
static float filter_axis(AxisFilter& state, float radians, float dt,
float inverse_dt, float velocity_alpha, bool rebased);
WiiIrTrackingResult accept(const Pair& pair, uint32_t now_us, bool inferred);
WiiIrTrackingResult lose();
WiiIrTrackingResult follow_spots(const Point points[4], const uint8_t masks[4],
unsigned count, uint32_t now_us);
WiiIrCameraModel model_;
float inverse_fx_ = 0;
float inverse_fy_ = 0;
bool model_valid_ = false;
bool relative_motion_ = false;
Point previous_points_[4]{};
unsigned previous_count_ = 0;
Pair geometry_{};
Pair candidate_{};
AxisFilter yaw_{};
AxisFilter pitch_{};
uint32_t geometry_us_ = 0;
uint32_t full_pair_us_ = 0;
uint32_t candidate_us_ = 0;
uint32_t sample_us_ = 0;
bool have_geometry_ = false;
bool have_candidate_ = false;
bool have_timestamp_ = false;
bool output_valid_ = false;
bool was_inferred_ = false;
};

6
src/firmware/main.cpp
View file

@ -13,6 +13,9 @@
#include "platform/pico/system_clock.h"
#include "profile/controller_profile_runtime.h"
#endif
#ifdef SWITCH_PICO_WII_IR_MOUSE
#include "usb/wii_ir_mouse_usb.h"
#endif
#ifdef SWITCH_PICO_LOG
#define LOG_PRINTF(...) printf(__VA_ARGS__)
@ -258,6 +261,9 @@ int main() {
while (true) {
tud_task(); // USB device tasks
#ifdef SWITCH_PICO_WII_IR_MOUSE
usb_wii_ir_mouse_task();
#endif
#ifdef SWITCH_PICO_BLUEPAD32
switch (bootsel_pairing_button_task()) {

4
src/firmware/platform/pico/tusb_config.h
View file

@ -31,7 +31,11 @@ extern "C" {
#define CFG_TUD_ENDPOINT0_SIZE 64
// Device class configuration
#ifdef SWITCH_PICO_WII_IR_MOUSE
#define CFG_TUD_HID (SWITCH_PICO_HID_INSTANCE_COUNT + 1)
#else
#define CFG_TUD_HID SWITCH_PICO_HID_INSTANCE_COUNT
#endif
#define CFG_TUD_CDC 0
#define CFG_TUD_MSC 0
#define CFG_TUD_MIDI 0

73
src/firmware/usb/switch/switch_pro_descriptors.h
View file

@ -373,11 +373,54 @@ static const uint8_t switch_pro_hid_descriptor[] =
0xCB, 0x00, // wDescriptorLength[0] 86
};
#ifdef SWITCH_PICO_WII_IR_MOUSE
static const uint8_t wii_ir_mouse_report_descriptor[] =
{
0x05, 0x01, // Usage Page (Generic Desktop)
0x09, 0x02, // Usage (Mouse)
0xA1, 0x01, // Collection (Application)
0x09, 0x01, // Usage (Pointer)
0xA1, 0x00, // Collection (Physical)
0x05, 0x09, // Usage Page (Button)
0x19, 0x01, // Usage Minimum (Button 1)
0x29, 0x03, // Usage Maximum (Button 3)
0x15, 0x00, // Logical Minimum (0)
0x25, 0x01, // Logical Maximum (1)
0x95, 0x03, // Report Count (3)
0x75, 0x01, // Report Size (1)
0x81, 0x02, // Input (Data, Variable, Absolute)
0x95, 0x01, // Report Count (1)
0x75, 0x05, // Report Size (5)
0x81, 0x03, // Input (Constant padding)
0x05, 0x01, // Usage Page (Generic Desktop)
0x09, 0x30, // Usage (X)
0x09, 0x31, // Usage (Y)
0x15, 0x81, // Logical Minimum (-127)
0x25, 0x7F, // Logical Maximum (127)
0x75, 0x08, // Report Size (8)
0x95, 0x02, // Report Count (2)
0x81, 0x06, // Input (Data, Variable, Relative)
0xC0, // End Collection
0x06, 0x00, 0xFF, // Usage Page (Vendor 0xFF00)
0x09, 0x01, // Usage (IR diagnostics)
0x15, 0x00, // Logical Minimum (0)
0x26, 0xFF, 0x00, // Logical Maximum (255)
0x75, 0x08, // Report Size (8)
0x95, 0x30, // Report Count (48)
0xB1, 0x02, // Feature (Data, Variable, Absolute)
0xC0, // End Collection
};
#endif
static const uint8_t switch_pro_configuration_descriptor[] =
{
0x09, // bLength
0x02, // bDescriptorType (Configuration)
#if SWITCH_PICO_HID_INSTANCE_COUNT == 1
#ifdef SWITCH_PICO_WII_IR_MOUSE
(9 + 32 * SWITCH_PICO_HID_INSTANCE_COUNT + 25) & 0xFF,
(9 + 32 * SWITCH_PICO_HID_INSTANCE_COUNT + 25) >> 8,
SWITCH_PICO_HID_INSTANCE_COUNT + 1, // Controllers plus mouse
#elif SWITCH_PICO_HID_INSTANCE_COUNT == 1
0x29, 0x00, // wTotalLength 41
0x01, // bNumInterfaces 1
#elif SWITCH_PICO_HID_INSTANCE_COUNT == 2
@ -528,6 +571,34 @@ static const uint8_t switch_pro_configuration_descriptor[] =
0x40, 0x00, // wMaxPacketSize 64
0x08, // bInterval 8 (unit depends on device speed)
#endif
#ifdef SWITCH_PICO_WII_IR_MOUSE
0x09, // bLength
0x04, // bDescriptorType (Interface)
SWITCH_PICO_HID_INSTANCE_COUNT, // Appended after controller interfaces
0x00, // bAlternateSetting
0x01, // bNumEndpoints 1
0x03, // bInterfaceClass (HID)
0x01, // bInterfaceSubClass (Boot)
0x02, // bInterfaceProtocol (Mouse)
0x00, // iInterface (String Index)
0x09, // bLength
0x21, // bDescriptorType (HID)
0x11, 0x01, // bcdHID 1.11
0x00, // bCountryCode
0x01, // bNumDescriptors
0x22, // bDescriptorType[0] (Report)
sizeof(wii_ir_mouse_report_descriptor) & 0xFF,
sizeof(wii_ir_mouse_report_descriptor) >> 8,
0x07, // bLength
0x05, // bDescriptorType (Endpoint)
0x80 | (SWITCH_PICO_HID_INSTANCE_COUNT + 1), // IN, 0x85 with four controllers
0x03, // bmAttributes (Interrupt)
0x03, 0x00, // wMaxPacketSize 3 (buttons, dx, dy)
0x08, // bInterval 8 ms
#endif
};
static const uint8_t switch_pro_report_descriptor[] =

17
src/firmware/usb/switch/switch_pro_driver.cpp
View file

@ -8,6 +8,9 @@
#include "pico/rand.h"
#include "pico/time.h"
#include "tusb.h"
#ifdef SWITCH_PICO_WII_IR_GYRO
#include "input/wii_ir_pointer.h"
#endif
#ifdef SWITCH_PICO_LOG
#define LOG_PRINTF(...) printf(__VA_ARGS__)
@ -875,10 +878,24 @@ bool switch_pro_task(uint8_t instance) {
const uint8_t previous_timestamp = context->switch_report.timestamp;
uint8_t previous_imu[sizeof(context->switch_report.imuData)];
memcpy(previous_imu, context->switch_report.imuData, sizeof(previous_imu));
#ifdef SWITCH_PICO_WII_IR_GYRO
// Only IMU fields are read by fill_imu_report_data. Avoid copying
// unrelated controls, and retain the original real-gyro state.
ControllerState ir_motion;
WiiIrGyroReport ir_report{};
const bool ir_override = wii_ir_gyro_prepare(
instance, time_us_32(), &ir_motion, &ir_report);
fill_imu_report_data(*context,
ir_override ? ir_motion : context->input_state, now);
#else
fill_imu_report_data(*context, context->input_state, now);
#endif
context->switch_report.timestamp += static_cast<uint8_t>(periods * 3);
if (send_report(instance, *context, 0, &context->switch_report,
sizeof(context->switch_report))) {
#ifdef SWITCH_PICO_WII_IR_GYRO
if (ir_override) wii_ir_gyro_commit(ir_report);
#endif
context->input_state.motion_sample_count = 0;
// Stay on the 15 ms clock across 8 ms USB polling quantization.
// Long stalls skip obsolete periods, never replay a motion burst.

26
src/firmware/usb/usb_configuration_management.cpp
View file

@ -12,6 +12,9 @@
#ifdef SWITCH_PICO_HAPTICS_EXPERIMENT
#include "input/haptics_transport_probe.h"
#endif
#ifdef SWITCH_PICO_WII_IR_GYRO
#include "input/wii_ir_pointer.h"
#endif
#include "tusb.h"
#include "usb/usb_output_driver.h"
@ -396,6 +399,21 @@ size_t encode_native_switch_rumble(uint8_t* output, size_t output_size) {
#endif
}
size_t encode_wii_ir_gyro(uint8_t* output, size_t output_size) {
#ifdef SWITCH_PICO_WII_IR_GYRO
if (output == nullptr || output_size < kResponseHeaderSize) return 0;
uint8_t* payload = output + kResponseHeaderSize;
const size_t size = wii_ir_gyro_diagnostics(
payload, output_size - kResponseHeaderSize);
if (size == 0) return 0;
return encode_response(Operation::kWiiIrGyro, Status::kOk, 0, 2, 0,
payload, size, output, output_size);
#else
return encode_response(Operation::kWiiIrGyro, Status::kUnsupportedSchema,
0, 2, 0, nullptr, 0, output, output_size);
#endif
}
} // namespace
bool decode_request(Operation setup_operation, const uint8_t* input,
@ -441,7 +459,7 @@ size_t encode_response(Operation operation, Status status, uint8_t flags,
write_u32(&output[12], generation);
write_u32(&output[16],
configuration_crc32(payload, payload_size));
if (payload_size != 0) {
if (payload_size != 0 && payload != &output[kResponseHeaderSize]) {
memcpy(&output[kResponseHeaderSize], payload, payload_size);
}
return required;
@ -957,7 +975,8 @@ bool usb_configuration_management_vendor_control(
const Operation operation =
static_cast<Operation>(request->bRequest);
if (operation == Operation::kHapticsTransportProbe &&
if ((operation == Operation::kHapticsTransportProbe ||
operation == Operation::kWiiIrGyro) &&
request->bmRequestType_bit.direction != TUSB_DIR_IN) {
return false;
}
@ -1039,6 +1058,9 @@ bool usb_configuration_management_vendor_control(
case Operation::kNativeSwitchRumble:
response_size = encode_native_switch_rumble(response, sizeof(response));
break;
case Operation::kWiiIrGyro:
response_size = encode_wii_ir_gyro(response, sizeof(response));
break;
case Operation::kMacroCapture:
response_size = encode_macro_capture(response, sizeof(response));
break;

2
src/firmware/usb/usb_configuration_management.h
View file

@ -79,6 +79,7 @@ enum class Operation : uint8_t {
kHapticsTransportProbe = 0x41,
kMacroCapture = 0x42,
kNativeSwitchRumble = 0x43,
kWiiIrGyro = 0x45,
};
enum class Status : uint8_t {
@ -101,6 +102,7 @@ struct DecodedRequest {
bool decode_request(Operation setup_operation, const uint8_t* input,
size_t input_size, DecodedRequest* output);
// Payload may already occupy output + kResponseHeaderSize for in-place encoding.
size_t encode_response(Operation operation, Status status, uint8_t flags,
uint16_t schema_version, uint32_t generation,
const uint8_t* payload, size_t payload_size,

40
src/firmware/usb/usb_output_driver.cpp
View file

@ -8,6 +8,13 @@
#include "usb/switch/switch_pro_descriptors.h"
#include "tusb.h"
#ifdef SWITCH_PICO_WII_IR_MOUSE
#include "usb/wii_ir_mouse_usb.h"
#endif
#ifdef SWITCH_PICO_WII_IR_GYRO
#include "input/wii_ir_pointer.h"
#endif
#ifdef SWITCH_PICO_BLUEPAD32
#include "usb/usb_configuration_management.h"
#endif
@ -189,6 +196,12 @@ void usb_output_driver_set_rumble_callback(
extern "C" uint16_t tud_hid_get_report_cb(
uint8_t instance, uint8_t report_id, hid_report_type_t report_type,
uint8_t* buffer, uint16_t requested_length) {
#ifdef SWITCH_PICO_WII_IR_MOUSE
if (instance == SWITCH_PICO_HID_INSTANCE_COUNT) {
return usb_wii_ir_mouse_get_report(report_id, report_type, buffer,
requested_length);
}
#endif
#ifdef SWITCH_PICO_USB_OUTPUT_MODES
if (xinput_selected()) {
return 0;
@ -205,6 +218,11 @@ extern "C" uint16_t tud_hid_get_report_cb(
extern "C" void tud_hid_set_report_cb(
uint8_t instance, uint8_t report_id, hid_report_type_t report_type,
const uint8_t* buffer, uint16_t buffer_size) {
#ifdef SWITCH_PICO_WII_IR_MOUSE
if (instance == SWITCH_PICO_HID_INSTANCE_COUNT) {
return;
}
#endif
if (switch_selected()) {
switch_pro_hid_set_report(instance, report_id, report_type, buffer,
buffer_size);
@ -214,6 +232,11 @@ extern "C" void tud_hid_set_report_cb(
extern "C" void tud_hid_report_received_cb(
uint8_t instance, uint8_t report_id, const uint8_t* buffer,
uint16_t buffer_size) {
#ifdef SWITCH_PICO_WII_IR_MOUSE
if (instance == SWITCH_PICO_HID_INSTANCE_COUNT) {
return;
}
#endif
if (switch_selected()) {
switch_pro_hid_report_received(instance, report_id, buffer,
buffer_size);
@ -221,6 +244,11 @@ extern "C" void tud_hid_report_received_cb(
}
extern "C" uint8_t const* tud_hid_descriptor_report_cb(uint8_t instance) {
#ifdef SWITCH_PICO_WII_IR_MOUSE
if (instance == SWITCH_PICO_HID_INSTANCE_COUNT) {
return wii_ir_mouse_report_descriptor;
}
#endif
#ifdef SWITCH_PICO_USB_OUTPUT_MODES
if (xinput_selected()) {
return nullptr;
@ -378,6 +406,12 @@ extern "C" bool tud_control_request_cb(
extern "C" void tud_mount_cb() {
LOG_PRINTF("[USB] mount_cb\n");
#ifdef SWITCH_PICO_WII_IR_GYRO
wii_ir_gyro_reset_output();
#endif
#ifdef SWITCH_PICO_WII_IR_MOUSE
usb_wii_ir_mouse_reset();
#endif
if (switch_selected()) {
switch_pro_mount();
}
@ -385,6 +419,12 @@ extern "C" void tud_mount_cb() {
extern "C" void tud_umount_cb() {
LOG_PRINTF("[USB] umount_cb\n");
#ifdef SWITCH_PICO_WII_IR_GYRO
wii_ir_gyro_reset_output();
#endif
#ifdef SWITCH_PICO_WII_IR_MOUSE
usb_wii_ir_mouse_reset();
#endif
#ifdef SWITCH_PICO_USB_OUTPUT_MODES
if (xinput_selected()) xinput_stop_rumble();
#endif

72
src/firmware/usb/wii_ir_mouse_usb.cpp Normal file
View file

@ -0,0 +1,72 @@
#include "usb/wii_ir_mouse_usb.h"
#ifdef SWITCH_PICO_WII_IR_MOUSE
#include <cstring>
#include "input/wii_ir_pointer.h"
namespace {
constexpr uint8_t kMouseInstance = SWITCH_PICO_HID_INSTANCE_COUNT;
uint8_t g_buttons = 0;
static_assert(WII_IR_MOUSE_DIAGNOSTIC_SIZE == 48,
"Mouse Feature report descriptor must match diagnostics size");
} // namespace
void usb_wii_ir_mouse_reset() {
g_buttons = 0;
wii_ir_pointer_reset();
}
void usb_wii_ir_mouse_task() {
if (!tud_mounted()) {
return;
}
WiiIrMouseReport pending{};
if (!wii_ir_mouse_peek(&pending) || !tud_hid_n_ready(kMouseInstance)) {
return;
}
// Boot and report protocols share these exact three bytes. TinyUSB's
// mouse helper also sends wheels, which are not part of this descriptor.
const uint8_t report[] = {
static_cast<uint8_t>(pending.buttons & 0x03),
static_cast<uint8_t>(pending.dx),
static_cast<uint8_t>(pending.dy),
};
if (tud_hid_n_report(kMouseInstance, 0, report, sizeof(report))) {
g_buttons = report[0];
wii_ir_mouse_commit(pending);
}
}
uint16_t usb_wii_ir_mouse_get_report(uint8_t report_id,
hid_report_type_t report_type,
uint8_t* buffer,
uint16_t requested_length) {
if (report_id != 0 || buffer == nullptr || requested_length == 0) {
return 0;
}
if (report_type == HID_REPORT_TYPE_FEATURE) {
const size_t capacity = requested_length < WII_IR_MOUSE_DIAGNOSTIC_SIZE
? requested_length
: WII_IR_MOUSE_DIAGNOSTIC_SIZE;
return static_cast<uint16_t>(wii_ir_pointer_diagnostics(buffer, capacity));
}
if (report_type == HID_REPORT_TYPE_INPUT) {
// Control requests must not consume or replay relative movement.
const uint8_t report[] = {g_buttons, 0, 0};
const uint16_t length = requested_length < sizeof(report)
? requested_length
: sizeof(report);
memcpy(buffer, report, length);
return length;
}
return 0;
}
#endif

17
src/firmware/usb/wii_ir_mouse_usb.h Normal file
View file

@ -0,0 +1,17 @@
#pragma once
#ifdef SWITCH_PICO_WII_IR_MOUSE
#include <stdint.h>
#include "tusb.h"
// Called on the USB core. Mount/unmount discard movement from the old session.
void usb_wii_ir_mouse_reset();
void usb_wii_ir_mouse_task();
uint16_t usb_wii_ir_mouse_get_report(uint8_t report_id,
hid_report_type_t report_type,
uint8_t* buffer,
uint16_t requested_length);
#endif

33
tests/test_wii_ir_aiming_native.py Normal file
View file

@ -0,0 +1,33 @@
from __future__ import annotations
import shutil
import subprocess
from pathlib import Path
def test_wii_ir_aiming_output_contract(tmp_path: Path) -> None:
root = Path(__file__).resolve().parents[1]
compiler = shutil.which("c++") or shutil.which("g++")
assert compiler is not None, "a host C++ compiler is required"
executable = tmp_path / "wii_ir_aiming_test"
subprocess.run(
[
compiler,
"-std=c++17",
"-Wall",
"-Wextra",
"-Werror",
"-pedantic",
"-DSWITCH_PICO_WII_IR_GYRO=1",
f"-I{root / 'tests' / 'wii_ir_aiming_native_stubs'}",
f"-I{root / 'src' / 'firmware'}",
str(root / "tests" / "wii_ir_aiming_test.cpp"),
str(root / "src" / "firmware" / "input" / "wii_ir_pointer.cpp"),
str(root / "src" / "firmware" / "input" / "wii_ir_tracker.cpp"),
"-o",
str(executable),
],
check=True,
cwd=root,
)
subprocess.run([str(executable)], check=True, cwd=root)

31
tests/test_wii_ir_tracker_native.py Normal file
View file

@ -0,0 +1,31 @@
from __future__ import annotations
import shutil
import subprocess
from pathlib import Path
def test_wii_ir_tracker_native(tmp_path: Path) -> None:
root = Path(__file__).resolve().parents[1]
compiler = shutil.which("c++") or shutil.which("g++")
assert compiler is not None, "a host C++ compiler is required"
executable = tmp_path / "wii_ir_tracker_test"
subprocess.run(
[
compiler,
"-std=c++17",
"-Wall",
"-Wextra",
"-Werror",
"-pedantic",
f"-I{root / 'src' / 'firmware'}",
str(root / "tests" / "wii_ir_tracker_test.cpp"),
str(root / "src" / "firmware" / "input" / "wii_ir_tracker.cpp"),
"-o",
str(executable),
],
check=True,
cwd=root,
)
subprocess.run([str(executable)], check=True, cwd=root)

6
tests/wii_ir_aiming_native_stubs/pico/critical_section.h Normal file
View file

@ -0,0 +1,6 @@
#pragma once
struct critical_section_t {};
inline unsigned next_striped_spin_lock_num() { return 16; }
inline void critical_section_init_with_lock_num(critical_section_t*, unsigned) {}
inline void critical_section_enter_blocking(critical_section_t*) {}
inline void critical_section_exit(critical_section_t*) {}

3
tests/wii_ir_aiming_native_stubs/pico/time.h Normal file
View file

@ -0,0 +1,3 @@
#pragma once
#include <stdint.h>
uint32_t time_us_32();

174
tests/wii_ir_aiming_test.cpp Normal file
View file

@ -0,0 +1,174 @@
#include "input/wii_ir_pointer.h"
#include <cmath>
#include <cstdint>
#include <iostream>
static uint32_t clock_us;
uint32_t time_us_32() { return clock_us; }
namespace {
int failures;
void expect(bool okay, const char* message) {
if (!okay) { std::cerr << message << '\n'; ++failures; }
}
struct Rotation {
int64_t x = 0;
int64_t y = 0;
int64_t z = 0;
};
struct Rig {
uint32_t sequence = 0;
uint8_t visible_mask;
explicit Rig(uint8_t mask = 3) : visible_mask(mask) {
wii_ir_pointer_init();
wii_ir_pointer_reset();
clock_us = 100000;
update_motion();
expect(wii_ir_gyro_select(0, 1, true), "IR source must be selectable");
send(); // Arm output before establishing a fresh camera baseline.
for (int i = 0; i < 12; ++i) {
input(0, 0);
if (i % 3 == 2) send();
}
}
void update_motion() {
const ControllerMotionSample physical{0, 0, 4096, 11, 22, 33};
wii_ir_gyro_update_motion(0, 1, true, physical);
}
void input(int dx, int dy, uint16_t buttons = 0, bool nunchuk_c = false) {
clock_us += 5000;
update_motion();
const uint16_t x[4] = {static_cast<uint16_t>(400 + dx), static_cast<uint16_t>(624 + dx), 0, 0};
const uint16_t y[4] = {static_cast<uint16_t>(384 + dy), static_cast<uint16_t>(384 + dy), 0, 0};
wii_ir_pointer_observe(0, 1, ++sequence, buttons, x, y, visible_mask, nunchuk_c);
}
ControllerState prepare(WiiIrGyroReport& ticket) {
ControllerState state{};
expect(wii_ir_gyro_prepare(0, clock_us, &state, &ticket), "selected IR must supply motion");
expect(state.motion_sample_count == 3, "IR reports contain three 5ms samples");
for (const auto& sample : state.motion_samples) {
expect(sample.accel_x == 0 && sample.accel_y == 0 && sample.accel_z == 4096,
"changing aiming axes must preserve the accelerometer");
}
return state;
}
ControllerMotionSample send() {
WiiIrGyroReport ticket{};
const ControllerState state = prepare(ticket);
const auto first = state.motion_samples[0];
for (const auto& sample : state.motion_samples) {
expect(sample.gyro_x == first.gyro_x && sample.gyro_y == first.gyro_y && sample.gyro_z == first.gyro_z,
"the report's angular displacement must be spread consistently over 15ms");
}
wii_ir_gyro_commit(ticket);
return first;
}
Rotation sweep(int horizontal, int vertical, uint16_t buttons = 0, bool nunchuk_c = false) {
Rotation total;
for (int i = 1; i <= 300; ++i) {
const int travel = i < 100 ? i : 100;
input(horizontal * travel, vertical * travel, buttons, nunchuk_c);
if (i % 3 == 0) {
const auto sample = send();
total.x += sample.gyro_x;
total.y += sample.gyro_y;
total.z += sample.gyro_z;
}
}
return total;
}
};
void horizontal_aim_matches_physical_yaw() {
for (int direction : {-1, 1}) {
Rig rig;
const auto rotation = rig.sweep(direction, 0);
// The physical Wii reference has gravity and horizontal yaw on Z.
// Check angular displacement, not merely a nonzero output channel.
const double expected = std::atan(100.0 / 1726.7951);
const double yaw = rotation.z * 0.015 / 818.5;
expect(std::abs(yaw - direction * expected) < expected * 0.05,
"IR yaw must match the physical rotation implied by camera travel");
expect(rotation.x == 0 && std::abs(rotation.y) < std::abs(rotation.z) / 50 + 3,
"horizontal pointing must not produce roll or vertical aiming");
}
}
void vertical_aim_retains_second_gyro_axis() {
for (int direction : {-1, 1}) {
Rig rig;
const auto rotation = rig.sweep(0, direction);
expect(rotation.y * direction < 0, "vertical IR direction and gyro Y must remain unchanged");
expect(rotation.x == 0 && rotation.z == 0, "vertical IR must not generate horizontal/roll movement");
}
}
void reposition_requires_c_and_one() {
{
Rig rig;
expect(rig.sweep(1, 0, 0x0002, false).z > 0,
"1 alone must no longer pause IR aiming");
}
{
Rig rig;
expect(rig.sweep(1, 0, 0, true).z > 0,
"Nunchuk C alone must not pause IR aiming");
}
Rig rig;
const auto held = rig.sweep(1, 1, 0x0002, true);
expect(held.x == 0 && held.y == 0 && held.z == 0,
"C + 1 must suppress aiming on every gyro axis while repositioning");
for (int i = 0; i < 6; ++i) {
rig.input(100, 100);
if (i % 3 == 2) {
const auto released = rig.send();
expect(released.gyro_x == 0 && released.gyro_y == 0 && released.gyro_z == 0,
"releasing C + 1 must establish a new baseline, not replay held motion");
}
}
int64_t movement = 0;
for (int i = 1; i <= 30; ++i) {
rig.input(100 + i, 100);
if (i % 3 == 0) movement += rig.send().gyro_z;
}
expect(movement > 0, "horizontal aiming must resume after reposition release");
}
void failed_send_preserves_horizontal_motion_once() {
Rig rig;
for (int i = 0; i < 3; ++i) rig.input(20, 0);
WiiIrGyroReport first{};
const auto waiting = rig.prepare(first).motion_samples[0];
expect(waiting.gyro_z > 0, "horizontal motion must be pending before the failed send");
WiiIrGyroReport retry{};
const auto repeated = rig.prepare(retry).motion_samples[0];
expect(repeated.gyro_z == waiting.gyro_z && repeated.gyro_x == 0,
"an uncommitted send must retain horizontal movement on its correct axis");
wii_ir_gyro_commit(first);
const auto consumed = rig.send();
expect(consumed.gyro_x == 0 && consumed.gyro_y == 0 && consumed.gyro_z == 0,
"successfully sent motion must not be emitted twice");
}
void single_spot_retains_horizontal_aiming() {
Rig rig(1);
const auto rotation = rig.sweep(1, 0);
const double expected = std::atan((500 - 511.5) / 1726.7951) -
std::atan((400 - 511.5) / 1726.7951);
expect(std::abs(rotation.z * 0.015 / 818.5 - expected) < expected * 0.05,
"one persistent spot must deliver its full yaw without an 80ms timeout");
expect(rotation.x == 0 && rotation.y == 0,
"single-spot horizontal travel must not create roll or pitch");
}
} // namespace
int main() {
horizontal_aim_matches_physical_yaw();
vertical_aim_retains_second_gyro_axis();
reposition_requires_c_and_one();
failed_send_preserves_horizontal_motion_once();
single_spot_retains_horizontal_aiming();
if (failures) return 1;
std::cout << "IR aiming output axes passed\n";
}

378
tests/wii_ir_tracker_test.cpp Normal file
View file

@ -0,0 +1,378 @@
#include "input/wii_ir_tracker.h"
#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <limits>
namespace {
int failures = 0;
void expect(bool condition, const char* message) {
if (!condition) {
std::cerr << message << '\n';
++failures;
}
}
bool near(float actual, float expected, float tolerance) {
return std::isfinite(actual) && std::fabs(actual - expected) <= tolerance;
}
struct Frame {
std::array<uint16_t, 4> x{};
std::array<uint16_t, 4> y{};
uint8_t mask = 0;
};
Frame pair(uint16_t ax, uint16_t ay, uint16_t bx, uint16_t by) {
return {{ax, bx, 0, 0}, {ay, by, 0, 0}, 0x03};
}
struct Camera {
WiiIrTracker tracker;
uint32_t now;
explicit Camera(WiiIrCameraModel model = {}, uint32_t start = 100000,
bool relative_motion = false)
: tracker(model, relative_motion), now(start) {}
WiiIrTrackingResult update(const Frame& frame, uint32_t elapsed = 10000,
float gravity_roll = 0, bool gravity_valid = false) {
now += elapsed;
return tracker.update(frame.x.data(), frame.y.data(), frame.mask, now,
gravity_roll, gravity_valid);
}
WiiIrTrackingResult acquire(const Frame& frame, float gravity_roll = 0,
bool gravity_valid = false) {
expect(!update(frame, 10000, gravity_roll, gravity_valid).tracked,
"one cold sample must not start tracking");
const auto result = update(frame, 10000, gravity_roll, gravity_valid);
expect(result.tracked && result.rebased && !result.inferred,
"two consistent full-pair samples must acquire with a rebase");
return result;
}
WiiIrTrackingResult settle(const Frame& frame) {
WiiIrTrackingResult result;
for (int i = 0; i < 40; ++i) {
result = update(frame);
expect(result.tracked && !result.rebased,
"an unchanged acquired pair must remain continuously tracked");
}
return result;
}
};
void test_slot_permutation_and_far_reflection() {
Camera camera;
const auto original = pair(420, 350, 620, 390);
camera.acquire(original);
const auto baseline = camera.settle(original);
// Swap the endpoint slots, then move them to different slots. The final
// frame also has a distant reflection that must not replace either end.
const std::array<Frame, 3> frames{{
pair(620, 390, 420, 350),
{{620, 0, 420, 0}, {390, 0, 350, 0}, 0x05},
{{620, 30, 420, 0}, {390, 700, 350, 0}, 0x07},
}};
const std::array<uint8_t, 3> selected_masks{{0x03, 0x05, 0x05}};
for (size_t i = 0; i < frames.size(); ++i) {
const auto result = camera.update(frames[i]);
expect(result.tracked && !result.rebased && !result.inferred,
"slot changes and a far reflection must not interrupt a matched pair");
expect(result.pair_mask == selected_masks[i],
"the selected pair must exclude the reflection and identify current slots");
expect(near(result.yaw_radians, baseline.yaw_radians, 0.001f) &&
near(result.pitch_radians, baseline.pitch_radians, 0.001f),
"unchanged physical spots must retain their bearings after slot changes");
expect(near(result.range_in_bar_widths, baseline.range_in_bar_widths, 0.02f),
"a reflection must not corrupt the tracked range");
}
}
void test_single_marker_expires_despite_continued_reports() {
Camera camera;
const auto full = pair(400, 384, 624, 384);
camera.acquire(full);
const auto baseline = camera.settle(full);
WiiIrTrackingResult result;
for (int i = 1; i <= 7; ++i) {
// The surviving endpoint also changes slot from the full-pair frame.
const Frame single{{0, 0, 0, static_cast<uint16_t>(400 + 2 * i)},
{0, 0, 0, 384}, 0x08};
result = camera.update(single);
expect(result.tracked && result.inferred && !result.rebased,
"an unambiguous moving endpoint must track briefly using inferred geometry");
expect(result.pair_mask == 0x08,
"inferred tracking must identify only the currently visible endpoint");
}
expect(result.yaw_radians < baseline.yaw_radians - 0.0005f,
"single-marker inference must produce the surviving endpoint's motion");
const Frame single{{0, 0, 0, 418}, {0, 0, 0, 384}, 0x08};
expect(!camera.update(single, 20000).tracked,
"single-marker inference must expire after 80ms from the last full pair");
for (int i = 0; i < 4; ++i) {
expect(!camera.update(single).tracked,
"continued single-marker reports must not restart the inference lifetime");
}
}
void test_full_pair_return_rebases_inference_correction() {
Camera camera;
camera.acquire(pair(400, 384, 624, 384));
const Frame single{{408, 0, 0, 0}, {384, 0, 0, 0}, 0x01};
const auto inferred = camera.update(single);
expect(inferred.tracked && inferred.inferred,
"a briefly missing endpoint must enter inferred tracking");
// Inference placed the missing endpoint at x=632. Its real return corrects
// that assumption, which must not become an emitted aiming delta.
const auto returned = camera.update(pair(408, 384, 650, 384));
expect(returned.tracked && !returned.inferred && returned.rebased,
"a correcting full-pair return must rebase rather than emit an inference jump");
expect(camera.update(pair(408, 384, 650, 384)).tracked,
"the returned full pair must remain usable after the correction");
}
void test_zero_marker_loss_and_reacquisition() {
Camera camera;
const auto original = pair(400, 350, 620, 350);
camera.acquire(original);
camera.settle(original);
expect(!camera.update(Frame{}).tracked,
"zero visible markers must stop tracking on the first empty frame");
const auto returned = pair(430, 350, 650, 350);
auto result = camera.update(returned);
if (!result.tracked) {
result = camera.update(returned);
}
expect(result.tracked && result.rebased && !result.inferred,
"the first tracked pair after loss must rebase, not bridge the missing interval");
result = camera.update(returned);
expect(result.tracked && !result.rebased,
"reacquisition must resume continuous tracking after the initial rebase");
}
void test_configured_pinhole_bearings_and_range() {
const WiiIrCameraModel model{1000.0f, 800.0f, 480.0f, 360.0f};
Camera camera(model);
const auto frame = pair(480, 440, 680, 560);
camera.acquire(frame);
const auto result = camera.settle(frame);
// Normalized endpoints (0,.1), (.2,.25) have span .25. Rotating their
// midpoint (.1,.175) into the bar frame gives (.185,.08).
const float expected_yaw = -std::atan2(0.185f, 1.0f);
const float expected_pitch = -std::atan2(0.08f, std::sqrt(1.0f + 0.185f * 0.185f));
expect(near(result.yaw_radians, expected_yaw, 0.001f),
"settled yaw must use the configured center and normalized bar-frame bearing");
expect(near(result.pitch_radians, expected_pitch, 0.001f),
"settled pitch must use independent vertical focal length and spherical bearing");
expect(near(result.range_in_bar_widths, 4.0f, 0.02f),
"range must be reciprocal normalized span, in bar widths rather than pixels");
}
void test_jitter_attenuation_preserves_slow_motion() {
Camera camera(WiiIrCameraModel{1000.0f, 1000.0f, 512.0f, 384.0f});
const auto stationary = pair(412, 384, 612, 384);
camera.acquire(stationary);
const auto baseline = camera.settle(stationary);
float peak_jitter = 0.0f;
for (int i = 0; i < 64; ++i) {
// One-pixel endpoint noise gives half-pixel midpoint noise even though
// the public camera coordinates are integers.
const auto jitter = pair(static_cast<uint16_t>(i % 2 == 0 ? 411 : 413),
384, 612, 384);
const auto result = camera.update(jitter, 5000);
expect(result.tracked && !result.rebased,
"subpixel midpoint jitter must not cause tracking loss or rebases");
peak_jitter = std::max(peak_jitter, std::fabs(result.yaw_radians - baseline.yaw_radians));
}
const float raw_jitter = std::atan2(0.5f, 1000.0f);
expect(peak_jitter < raw_jitter * 0.6f,
"high-frequency subpixel midpoint jitter must be substantially attenuated");
const auto resting = camera.settle(stationary);
float previous_yaw = resting.yaw_radians;
for (int step = 1; step <= 16; ++step) {
const auto moving = pair(static_cast<uint16_t>(412 - step), 384, 612, 384);
for (int hold = 0; hold < 8; ++hold) {
const auto result = camera.update(moving);
expect(result.tracked && !result.rebased,
"slow accumulated movement must remain continuously tracked");
expect(result.yaw_radians >= previous_yaw - 0.00001f,
"monotonic slow motion must not cause filter-driven direction reversals");
previous_yaw = result.yaw_radians;
}
}
expect(previous_yaw > resting.yaw_radians + 0.006f,
"half-pixel motion steps must accumulate instead of dying in a per-frame dead zone");
}
void test_optional_gravity_rejects_wrong_pair() {
const auto vertical = pair(512, 260, 512, 500);
Camera constrained;
for (int i = 0; i < 3; ++i) {
expect(!constrained.update(vertical, 10000, 0.0f, true).tracked,
"a vertical pair must not acquire against a valid horizontal gravity prior");
}
Camera unconstrained;
unconstrained.acquire(vertical);
const auto free_result = unconstrained.update(vertical, 10000, 0.0f, false);
expect(free_result.tracked && !free_result.rebased,
"the same vertical geometry must remain usable without a valid gravity prior");
Camera dynamic;
const auto horizontal = pair(400, 384, 624, 384);
dynamic.acquire(horizontal, 0.0f, true);
const auto result = dynamic.update(pair(410, 384, 634, 384), 10000, 1.5707963f, false);
expect(result.tracked && !result.rebased,
"an invalid dynamic gravity hint must not disable otherwise continuous IR tracking");
}
Frame rolled_pair(float radians, const WiiIrCameraModel& model) {
const float cosine = std::cos(radians);
const float sine = std::sin(radians);
Frame frame;
for (size_t i = 0; i < 2; ++i) {
const float bar_x = i == 0 ? -0.02f : 0.18f;
const float bar_y = 0.04f;
const float camera_x = bar_x * cosine - bar_y * sine;
const float camera_y = bar_x * sine + bar_y * cosine;
frame.x[i] = static_cast<uint16_t>(std::lround(model.cx + model.fx * camera_x));
frame.y[i] = static_cast<uint16_t>(std::lround(model.cy + model.fy * camera_y));
}
frame.mask = 0x03;
return frame;
}
void test_roll_through_vertical_preserves_endpoint_order() {
const WiiIrCameraModel model{1000.0f, 800.0f, 512.0f, 384.0f};
Camera camera(model);
const auto initial = rolled_pair(0.0f, model);
camera.acquire(initial);
const auto baseline = camera.settle(initial);
for (int degrees = 5; degrees <= 120; degrees += 5) {
const float radians = static_cast<float>(degrees) * 0.01745329252f;
const auto result = camera.update(rolled_pair(radians, model));
expect(result.tracked && !result.rebased && !result.inferred,
"rolling a matched pair through vertical must preserve continuous tracking");
expect(near(result.yaw_radians, baseline.yaw_radians, 0.0025f) &&
near(result.pitch_radians, baseline.pitch_radians, 0.0025f),
"passing 90 degrees of roll must not flip endpoint order or invert bearings");
expect(near(result.range_in_bar_widths, 5.0f, 0.08f),
"roll with unequal focal lengths must preserve normalized pair range");
}
}
void test_timestamp_wrap_preserves_continuous_motion() {
Camera ordinary({}, 100000);
Camera wrapping({}, std::numeric_limits<uint32_t>::max() - 50000u);
const auto initial = pair(400, 384, 624, 384);
ordinary.acquire(initial);
const auto baseline = wrapping.acquire(initial);
float previous_yaw = baseline.yaw_radians;
for (int step = 1; step <= 16; ++step) {
const auto frame = pair(static_cast<uint16_t>(400 - 3 * step), 384,
static_cast<uint16_t>(624 - 3 * step), 384);
const auto reference = ordinary.update(frame);
const auto result = wrapping.update(frame);
expect(result.tracked && !result.rebased,
"uint32 timestamp wrap must not interrupt continuous pair motion");
expect(near(result.yaw_radians, reference.yaw_radians, 0.00001f) &&
near(result.pitch_radians, reference.pitch_radians, 0.00001f),
"wrapped and ordinary clocks must yield the same time-filtered bearings");
expect(std::fabs(result.yaw_radians - previous_yaw) < 0.02f,
"timestamp wrap must not create an angular discontinuity");
previous_yaw = result.yaw_radians;
}
expect(previous_yaw > baseline.yaw_radians + 0.015f,
"motion across timestamp wrap must advance the bearing rather than freeze it");
}
void test_relative_tracking_without_recognizable_bar() {
const WiiIrCameraModel model{1000.0f, 1000.0f, 512.0f, 384.0f};
// One endpoint alone, and the four-spot/reflection layout captured on
// hardware. Neither provides a uniquely identifiable full sensor bar.
for (unsigned count : {1u, 4u}) {
Camera camera(model, 100000, true);
const Frame scene{{184, 193, 687, 739}, {623, 536, 453, 549}, 15};
WiiIrTrackingResult result;
for (unsigned step = 0; step <= 160; ++step) {
Frame frame;
const unsigned travel = std::min(step, 80u);
for (unsigned i = 0; i < count; ++i) {
const unsigned slot = (i + step) % 4;
frame.x[slot] = scene.x[i] + travel;
frame.y[slot] = scene.y[i];
frame.mask |= 1u << slot;
}
result = camera.update(frame, 10000, 0.0f, true);
expect(result.tracked && (step == 0 || !result.rebased),
"persistent spots must retain motion beyond 80ms without a full bar");
}
float expected = 0;
for (unsigned i = 0; i < count; ++i) {
expected -= std::atan((scene.x[i] + 80 - model.cx) / model.fx) -
std::atan((scene.x[i] - model.cx) / model.fx);
}
expected /= count;
expect(near(result.yaw_radians, expected, 0.001f),
"single/multiple-spot motion must retain the measured angular travel");
expect(near(result.pitch_radians, 0, 0.0001f),
"slot changes and extra spots must not create vertical motion");
}
}
void test_relative_loss_and_conflicting_matches_rebase() {
Camera camera({}, 100000, true);
const Frame first{{400, 0, 0, 0}, {384, 0, 0, 0}, 1};
camera.update(first);
camera.update(Frame{{410, 0, 0, 0}, {384, 0, 0, 0}, 1});
expect(!camera.update(Frame{}).tracked,
"relative aiming must stop immediately when every spot disappears");
auto result = camera.update(Frame{{700, 0, 0, 0}, {384, 0, 0, 0}, 1});
expect(result.tracked && result.rebased,
"reacquisition must discard movement across the missing interval");
result = camera.update(first);
expect(result.rebased, "an implausible reflection jump must not generate aiming");
Camera conflicting({}, 100000, true);
conflicting.update(pair(400, 384, 600, 384));
result = conflicting.update(pair(410, 384, 590, 384));
expect(result.rebased,
"two contradictory matches must not choose an arbitrary aiming direction");
result = conflicting.update(pair(410, 384, 590, 384));
expect(result.tracked && !result.rebased && near(result.yaw_radians, 0, 0.0001f),
"settled observations must resume without replaying rejected movement");
}
} // namespace
int main() {
test_slot_permutation_and_far_reflection();
test_single_marker_expires_despite_continued_reports();
test_full_pair_return_rebases_inference_correction();
test_zero_marker_loss_and_reacquisition();
test_configured_pinhole_bearings_and_range();
test_jitter_attenuation_preserves_slow_motion();
test_optional_gravity_rejects_wrong_pair();
test_roll_through_vertical_preserves_endpoint_order();
test_timestamp_wrap_preserves_continuous_motion();
test_relative_tracking_without_recognizable_bar();
test_relative_loss_and_conflicting_matches_rebase();
if (failures != 0) {
std::cerr << failures << " Wii IR tracker test(s) failed\n";
return 1;
}
return 0;
}

792
tools/dolphinbar_keepalive.py Executable file
View file

@ -0,0 +1,792 @@
#!/usr/bin/env python3
"""Temporarily emulate a neutral Wii Remote for a DolphinBar on Linux.
Run with sudo and press SYNC on the DolphinBar after the READY message.
Runs until Ctrl+C by default; --seconds N optionally sets a runtime limit.
No MAC spoofing, firmware changes, or persistent Bluetooth pairing keys.
The normal Bluetooth daemon is temporarily runtime-masked; settings and service
are restored on exit. --restore recovers an interrupted run from its journal.
"""
from __future__ import annotations
import argparse
import fcntl
import json
import os
import selectors
import signal
import socket
import struct
import subprocess
import sys
import time
from collections import deque
from contextlib import ExitStack, contextmanager
from pathlib import Path
from typing import ClassVar, TypedDict, cast
# Keep this executable directly as well as through python -m tools....
if __package__ in (None, ""):
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
AdapterInfo = TypedDict(
"AdapterInfo",
{
"address": str,
"version": int,
"manufacturer": int,
"supported": int,
"settings": int,
"class": str,
"name": str,
},
)
class RecoveryState(TypedDict):
schema: int
index: int
info: AdapterInfo
service_active: bool
hci: dict[str, str]
POWERED = 1 << 0
CONNECTABLE = 1 << 1
DISCOVERABLE = 1 << 3
BONDABLE = 1 << 4
SSP = 1 << 6
BREDR = 1 << 7
ADVERTISING = 1 << 10
SECURE_CONN = 1 << 11
stop_requested = False
def log(event: str, **fields) -> None:
print(
json.dumps({"time": round(time.monotonic(), 3), "event": event, **fields}),
flush=True,
)
def run(*args: str, check: bool = True) -> subprocess.CompletedProcess[str]:
return subprocess.run(args, check=check, text=True, capture_output=True, timeout=20)
def address(raw: bytes) -> str:
return ":".join(f"{b:02X}" for b in raw[::-1])
class Management:
def __init__(self, index: int):
self.index = index
self.socket = socket.socket(
socket.AF_BLUETOOTH, socket.SOCK_RAW, socket.BTPROTO_HCI
)
self.socket.bind((0xFFFF, 3)) # HCI_CHANNEL_CONTROL
self.socket.settimeout(5)
def close(self):
self.socket.close()
def request(self, opcode: int, payload: bytes = b"") -> bytes:
self.socket.send(
struct.pack("<HHH", opcode, self.index, len(payload)) + payload
)
deadline = time.monotonic() + 5
while time.monotonic() < deadline:
packet = self.socket.recv(4096)
if len(packet) < 9:
continue
event, index, size = struct.unpack_from("<HHH", packet)
if index != self.index or size + 6 != len(packet) or event not in (1, 2):
continue
command, status = struct.unpack_from("<HB", packet, 6)
if command != opcode:
continue
if status:
raise RuntimeError(
f"MGMT command 0x{opcode:04x}: status 0x{status:02x}"
)
return packet[9:]
raise TimeoutError(f"MGMT command 0x{opcode:04x} timed out")
def info(self) -> AdapterInfo:
data = self.request(4)
if len(data) != 280:
raise RuntimeError(f"Unexpected MGMT ReadInfo size {len(data)}")
return {
"address": address(data[:6]),
"version": data[6],
"manufacturer": struct.unpack_from("<H", data, 7)[0],
"supported": struct.unpack_from("<I", data, 9)[0],
"settings": struct.unpack_from("<I", data, 13)[0],
"class": data[17:20].hex(),
"name": data[20:269].split(b"\0", 1)[0].decode(errors="replace"),
}
def connections(self) -> int:
data = self.request(0x15)
if len(data) < 2:
raise RuntimeError("Short MGMT connection list")
count = struct.unpack_from("<H", data)[0]
if len(data) != 2 + count * 7:
raise RuntimeError("Malformed MGMT connection list")
return count
def setting(self, opcode: int, enabled: bool) -> None:
self.request(opcode, bytes([int(enabled)]))
def discoverable(self, mode: int, timeout: int = 0) -> None:
self.request(6, struct.pack("<BH", mode, timeout))
class Hci:
# read opcode -> (write opcode, expected payload length; -1 is variable)
SETTINGS: ClassVar[dict[int, tuple[int, int]]] = {
0x0C14: (0x0C13, 248),
0x0C23: (0x0C24, 3),
0x0C19: (0x0C1A, 1),
0x0C39: (0x0C3A, -1),
0x0C51: (0x0C52, 241),
0x0C09: (0x0C0A, 1),
}
def __init__(self, index: int):
self.socket = socket.socket(
socket.AF_BLUETOOTH, socket.SOCK_RAW, socket.BTPROTO_HCI
)
self.socket.bind((index,))
self.socket.setsockopt(
0, 2, struct.pack("<IIIH2x", 1 << 4, 0xFFFFFFFF, 0xFFFFFFFF, 0)
)
self.socket.settimeout(4)
self.pending_events: deque[bytes] = deque()
def close(self):
self.socket.close()
def send(self, opcode: int, payload: bytes = b"") -> None:
if len(payload) > 255:
raise ValueError("HCI command too large")
self.socket.send(
bytes([1]) + struct.pack("<HB", opcode, len(payload)) + payload
)
def command(self, opcode: int, payload: bytes = b"") -> bytes:
self.send(opcode, payload)
deadline = time.monotonic() + 4
while time.monotonic() < deadline:
packet = self.socket.recv(1024)
if len(packet) < 7 or packet[0] != 4 or len(packet) != packet[2] + 3:
continue
if packet[1] == 0x0E and struct.unpack_from("<H", packet, 4)[0] == opcode:
if packet[6]:
raise RuntimeError(
f"HCI command 0x{opcode:04x}: status 0x{packet[6]:02x}"
)
return packet[7:]
if (
packet[1] == 0x0F
and struct.unpack_from("<H", packet, 5)[0] == opcode
and packet[3]
):
raise RuntimeError(
f"HCI command 0x{opcode:04x}: status 0x{packet[3]:02x}"
)
if packet[1] in (3, 4, 5, 6, 0x16, 0x18):
self.pending_events.append(packet)
raise TimeoutError(f"HCI command 0x{opcode:04x} timed out")
def snapshot(self) -> dict[str, str]:
result = {}
for read_opcode, (write_opcode, size) in self.SETTINGS.items():
data = self.command(read_opcode)
if (size >= 0 and len(data) != size) or (
size < 0 and (not data or len(data) != 1 + data[0] * 3)
):
raise RuntimeError(
f"Unexpected HCI setting length for 0x{read_opcode:04x}"
)
result[str(write_opcode)] = data.hex()
return result
class AdapterSession:
def __init__(self, index: int, journal: Path):
self.index = index
self.journal = journal
self.management = Management(index)
self.hci = Hci(index)
self.state: RecoveryState | None = None
self.lock_resources = ExitStack()
def acquire_lock(self) -> None:
@contextmanager
def held_lock():
with open(
f"/run/lock/switch-pico-dolphinbar-hci{self.index}.lock", "a"
) as lock:
fcntl.flock(lock, fcntl.LOCK_EX | fcntl.LOCK_NB)
yield
self.lock_resources.enter_context(held_lock())
def prepare(self) -> None:
self.acquire_lock()
if self.journal.exists():
raise RuntimeError(
f"Recovery journal exists. Run --restore {self.journal} first"
)
info = self.management.info()
if not info["settings"] & POWERED or not info["settings"] & BREDR:
raise RuntimeError(
"Enable the adapter and Bluetooth Classic before running this tool"
)
if self.management.connections():
raise RuntimeError(
"Refusing to interrupt connected laptop Bluetooth devices"
)
if info["settings"] & (ADVERTISING | DISCOVERABLE):
raise RuntimeError(
"Stop existing laptop Bluetooth advertising/discovery before this test"
)
active = (
run("systemctl", "is-active", "bluetooth.service", check=False).returncode
== 0
)
masked = run(
"systemctl", "is-enabled", "bluetooth.service", check=False
).stdout.strip()
if masked not in ("enabled", "disabled", "static", "indirect", "alias"):
raise RuntimeError(
f"Unexpected Bluetooth service state {masked!r}; not changing it"
)
self.state = {
"schema": 1,
"index": self.index,
"info": info,
"service_active": active,
"hci": self.hci.snapshot(),
}
self.journal.parent.mkdir(parents=True, exist_ok=True)
fd = os.open(self.journal, os.O_WRONLY | os.O_CREAT | os.O_EXCL, 0o600)
with os.fdopen(fd, "w") as stream:
json.dump(self.state, stream, indent=2)
stream.flush()
os.fsync(stream.fileno())
log(
"adapter_saved",
adapter=f"hci{self.index}",
address=info["address"],
journal=str(self.journal),
)
run("systemctl", "mask", "--runtime", "bluetooth.service")
run("systemctl", "stop", "bluetooth.service")
self.management.setting(5, False)
if info["supported"] & SECURE_CONN:
self.management.setting(0x2D, False)
self.management.setting(0x0B, False) # Classic legacy PIN, not SSP.
self.management.setting(9, True)
self.management.setting(7, True)
self.management.setting(5, True)
# Discovery starts only after all L2CAP listeners are bound.
self.management.discoverable(0)
self.hci.command(0x0C13, b"Nintendo RVL-CNT-01".ljust(248, b"\0"))
self.hci.command(0x0C0A, b"\0") # Variable PIN.
def advertise(self, seconds: int | None) -> None:
assert self.state is not None
# Discovery is bounded independently of an established connection.
self.management.discoverable(
2, min(seconds, 180) if seconds is not None else 180
)
self.hci.command(0x0C24, bytes.fromhex("042500"))
supported_iac = self.hci.command(0x0C38)
laps = (
bytes.fromhex("008b9e338b9e")
if supported_iac and supported_iac[0] >= 2
else bytes.fromhex("008b9e")
)
self.hci.command(0x0C3A, bytes([len(laps) // 3]) + laps)
name = b"Nintendo RVL-CNT-01"
eir = bytes([len(name) + 1, 9]) + name + bytes.fromhex("050324110012")
self.hci.command(0x0C52, b"\0" + eir.ljust(240, b"\0"))
self.hci.command(0x0C1A, b"\x03")
log(
"identity_ready",
address=self.state["info"]["address"],
name=name.decode(),
device_class="002504",
address_spoofed=False,
)
def restore(self) -> None:
if self.state is None:
return
# Recovery can be invoked after a partial cleanup restarted BlueZ.
# Prevent it from racing these controller commands in either path.
run("systemctl", "mask", "--runtime", "bluetooth.service")
run("systemctl", "stop", "bluetooth.service")
self.hci.socket.settimeout(4)
errors = []
settings = self.state["info"]["settings"]
def attempt(label, fn):
try:
fn()
except (
OSError,
RuntimeError,
ValueError,
subprocess.SubprocessError,
) as exc:
errors.append(f"{label}: {exc}")
def restore_discoverable():
wanted = bool(settings & DISCOVERABLE)
current = bool(self.management.info()["settings"] & DISCOVERABLE)
# Clearing connectable already clears discoverable. The kernel
# rejects a redundant discoverable request when not connectable.
if current != wanted:
self.management.discoverable(int(wanted))
attempt("power down", lambda: self.management.setting(5, False))
attempt("SSP", lambda: self.management.setting(0x0B, bool(settings & SSP)))
if self.state["info"]["supported"] & SECURE_CONN:
attempt(
"secure connections",
lambda: self.management.setting(0x2D, bool(settings & SECURE_CONN)),
)
attempt(
"bondable", lambda: self.management.setting(9, bool(settings & BONDABLE))
)
attempt(
"connectable",
lambda: self.management.setting(7, bool(settings & CONNECTABLE)),
)
attempt("power up", lambda: self.management.setting(5, True))
attempt("discoverable", restore_discoverable)
for opcode, value in self.state["hci"].items():
attempt(
f"HCI {opcode}",
lambda opcode=opcode, value=value: self.hci.command(
int(opcode), bytes.fromhex(value)
),
)
attempt(
"original power",
lambda: self.management.setting(5, bool(settings & POWERED)),
)
attempt(
"unmask service",
lambda: run("systemctl", "unmask", "--runtime", "bluetooth.service"),
)
if self.state["service_active"]:
attempt(
"restart service",
lambda: run("systemctl", "start", "bluetooth.service"),
)
if errors:
log(
"restore_failed",
errors=errors,
recovery=f"sudo {sys.executable} {__file__} --restore {self.journal}",
)
raise RuntimeError("Restoration incomplete; recovery journal retained")
self.journal.unlink(missing_ok=True)
log(
"adapter_restored",
adapter=f"hci{self.index}",
service_active=self.state["service_active"],
)
self.state = None
def close(self):
self.hci.close()
self.management.close()
self.lock_resources.close()
class Peripheral:
def __init__(self, session: AdapterSession, pin_mode: str, peer: str | None):
from tools.dolphinbar_sdp import SdpResponder
from tools.dolphinbar_wiimote import HID_DESCRIPTOR, Wiimote
self.session = session
self.pin_mode = pin_mode
self.peer = peer.upper() if peer else None
self.sdp_factory = lambda: SdpResponder(HID_DESCRIPTOR)
self.sdp_sessions: dict[socket.socket, SdpResponder] = {}
self.peer_mtus: dict[socket.socket, int] = {}
self.wiimote = Wiimote()
self.selector = selectors.DefaultSelector()
self.listeners: list[socket.socket] = []
self.clients: dict[socket.socket, int] = {}
self.pending: dict[socket.socket, deque[bytes]] = {}
self.waiting_interrupt: deque[bytes] = deque()
self.interrupt: socket.socket | None = None
self.commands = 0
self.sent = 0
self.pins = 0
self.authenticated: bool | None = None
self.generated_link_key = False
self.last_ir = False
assert session.state is not None
local = session.state["info"]["address"]
self.local_address = local
try:
for psm in (1, 0x11, 0x13):
listener = socket.socket(
socket.AF_BLUETOOTH, socket.SOCK_SEQPACKET, socket.BTPROTO_L2CAP
)
self.listeners.append(listener)
listener.setsockopt(274, 4, bytes([1, 0])) # BT_SECURITY_LOW
listener.bind((local, psm))
listener.listen(1)
listener.setblocking(False)
self.selector.register(listener, selectors.EVENT_READ, ("listen", psm))
session.hci.socket.setblocking(False)
self.selector.register(session.hci.socket, selectors.EVENT_READ, ("hci", 0))
except BaseException:
self.close()
raise
def close_client(self, client):
psm = self.clients.pop(client, None)
self.pending.pop(client, None)
self.sdp_sessions.pop(client, None)
self.peer_mtus.pop(client, None)
try:
self.selector.unregister(client)
except (KeyError, ValueError):
pass
if client is self.interrupt:
self.interrupt = None
client.close()
if psm is not None:
log("channel_closed", psm=hex(psm))
def close(self):
for client in list(self.clients):
self.close_client(client)
for listener in self.listeners:
listener.close()
self.selector.close()
def queue(self, client, packet: bytes):
if client is None:
if len(self.waiting_interrupt) >= 512:
raise RuntimeError("Peer exhausted pre-interrupt response queue")
self.waiting_interrupt.append(packet)
return
queue = self.pending[client]
if len(queue) >= 512:
raise RuntimeError("Peer exhausted bounded response queue")
queue.append(packet)
self.selector.modify(
client,
selectors.EVENT_READ | selectors.EVENT_WRITE,
("client", self.clients[client]),
)
def hci_event(self, packet: bytes):
if len(packet) < 3 or packet[0] != 4 or len(packet) != packet[2] + 3:
return
event, data = packet[1], packet[3:]
if event == 0x16 and len(data) == 6: # PIN_CODE_REQUEST
remote = address(data)
if self.peer and remote != self.peer:
self.session.management.request(0x0017, data + b"\0")
log("pin_rejected_other_peer", peer=remote)
return
self.peer = remote
pin = (
data
if self.pin_mode == "host"
else bytes.fromhex(self.local_address.replace(":", ""))[::-1]
)
# MGMT accepts binary PINs and updates the kernel's pairing state.
self.session.management.request(0x0016, data + b"\0\x06" + pin + bytes(10))
self.pins += 1
log("legacy_pin_replied", peer=remote, mode=self.pin_mode)
elif event == 3 and len(data) >= 11:
log(
"acl_connection",
status=data[0],
peer=address(data[3:9]),
encryption=data[10],
)
elif event == 6 and len(data) == 3:
self.authenticated = data[0] == 0
log(
"authentication",
status=data[0],
handle=struct.unpack_from("<H", data, 1)[0],
)
elif event == 5 and len(data) == 4:
log("acl_disconnected", reason=data[3])
elif event == 0x18 and len(data) == 23:
self.generated_link_key = True
log("link_key_generated", peer=address(data[:6]), persisted=False)
elif event == 4 and len(data) == 10:
log(
"connection_requested",
peer=address(data[:6]),
device_class=data[6:9].hex(),
)
def receive(self, client, psm: int, packet: bytes):
if psm == 1:
reply = self.sdp_sessions[client].reply(packet, self.peer_mtus[client])
self.queue(client, reply)
log("sdp", request=packet.hex(), response_bytes=len(reply))
return
if not packet:
return
prefix = packet[0]
if prefix in (0xA2, 0x52):
log("wiimote_command", psm=hex(psm), report=packet[1:].hex())
replies = self.wiimote.handle_output(packet[1:])
self.commands += 1
if prefix == 0x52 and psm == 0x11:
self.queue(client, b"\0")
for reply in replies:
self.queue(self.interrupt, reply)
if self.wiimote.ir_enabled != self.last_ir:
self.last_ir = self.wiimote.ir_enabled
log(
"emulated_camera",
enabled=self.last_ir,
mode=hex(self.wiimote.report_mode),
)
elif psm == 0x11:
if prefix == 0x15: # Virtual cable unplug.
self.close_client(client)
elif prefix == 0x60:
self.queue(client, b"\xa0\x01")
elif prefix == 0x71 or prefix & 0xF0 == 0x90:
self.queue(client, b"\0")
elif prefix == 0x80:
self.queue(client, b"\xa0\0")
elif prefix == 0x41 and len(packet) >= 2 and packet[1] == 0x20:
self.queue(client, self.wiimote.status_report())
else:
self.queue(client, b"\x03")
log("unsupported_hid_control", packet=packet.hex())
def serve(self, seconds: int | None):
self.session.hci.socket.settimeout(4)
self.session.advertise(seconds)
self.session.hci.socket.setblocking(False)
while self.session.hci.pending_events:
self.hci_event(self.session.hci.pending_events.popleft())
log(
"READY",
message="Listening; press only the DolphinBar SYNC button now",
seconds=seconds,
lifetime="until Ctrl+C" if seconds is None else f"{seconds} seconds",
pin_mode=self.pin_mode,
)
start = time.monotonic()
next_report = start
next_status = start + 10
while not stop_requested and (
seconds is None or time.monotonic() - start < seconds
):
for key, events in self.selector.select(0.01):
category, psm = key.data
channel_socket = cast(socket.socket, key.fileobj)
if category == "hci":
try:
self.hci_event(channel_socket.recv(1024))
except BlockingIOError:
pass
elif category == "listen":
client, remote = channel_socket.accept()
remote_address = remote[0].upper()
if self.peer and self.peer != remote_address:
client.close()
log("rejected_other_peer", peer=remote_address)
continue
if psm != 1:
self.peer = remote_address
if any(value == psm for value in self.clients.values()):
client.close()
continue
client.setblocking(False)
self.clients[client] = psm
self.pending[client] = deque()
options = client.getsockopt(6, 1, 12) # L2CAP_OPTIONS
self.peer_mtus[client] = struct.unpack_from("<H", options)[0]
if psm == 1:
self.sdp_sessions[client] = self.sdp_factory()
self.selector.register(
client, selectors.EVENT_READ, ("client", psm)
)
log("channel_connected", psm=hex(psm), peer=remote_address)
if psm == 0x13:
self.interrupt = client
while self.waiting_interrupt:
self.queue(client, self.waiting_interrupt.popleft())
self.queue(client, self.wiimote.status_report())
else:
client = channel_socket
try:
if events & selectors.EVENT_READ:
packet = client.recv(4096)
if not packet:
self.close_client(client)
continue
try:
self.receive(client, psm, packet)
except ValueError as exc:
log(
"malformed_peer_request",
psm=hex(psm),
error=str(exc),
)
self.close_client(client)
continue
if client in self.clients and events & selectors.EVENT_WRITE:
queue = self.pending[client]
if queue:
packet = queue[0]
if client.send(packet) != len(packet):
raise RuntimeError(
"Partial L2CAP sequenced-packet write"
)
queue.popleft()
self.sent += 1
if not queue:
self.selector.modify(
client, selectors.EVENT_READ, ("client", psm)
)
except BlockingIOError:
pass
except OSError as exc:
log("channel_error", psm=hex(psm), error=str(exc))
self.close_client(client)
now = time.monotonic()
if now >= next_report:
next_report = now + 0.01
if self.interrupt and not self.pending[self.interrupt]:
report = self.wiimote.periodic_report()
if report:
self.queue(self.interrupt, report)
if now >= next_status:
next_status = now + 10
log(
"status",
elapsed=round(now - start, 1),
peer=self.peer,
commands=self.commands,
sent=self.sent,
channels=sorted(self.clients.values()),
ir_enabled=self.wiimote.ir_enabled,
)
log(
"finished",
peer=self.peer,
commands=self.commands,
sent=self.sent,
pin_requests=self.pins,
authenticated=self.authenticated,
link_key_generated=self.generated_link_key,
ir_enabled=self.wiimote.ir_enabled,
)
def main() -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--adapter", default="hci0")
parser.add_argument(
"--seconds",
type=int,
help="Stop after N seconds; omitted means run until Ctrl+C",
)
parser.add_argument("--pin-mode", choices=("host", "device"), default="host")
parser.add_argument(
"--peer", help="Restrict the experiment to one Bluetooth address"
)
parser.add_argument("--state-file", type=Path)
parser.add_argument(
"--inspect",
action="store_true",
help="Read adapter information without changing it",
)
parser.add_argument(
"--restore",
type=Path,
help="Restore settings from an interrupted run's journal",
)
args = parser.parse_args()
if not sys.platform.startswith("linux") or os.geteuid() != 0:
parser.error("Linux and root/CAP_NET_ADMIN access are required; run with sudo")
if not args.adapter.startswith("hci") or not args.adapter[3:].isdigit():
parser.error("--adapter must be hciN")
index = int(args.adapter[3:])
if index > 65534 or (args.seconds is not None and args.seconds < 1):
parser.error("Invalid adapter index or duration (must be positive)")
if args.peer:
try:
if len(bytes.fromhex(args.peer.replace(":", ""))) != 6:
raise ValueError
except ValueError:
parser.error("--peer must be a Bluetooth address")
journal = args.state_file or Path("build-dolphinbar") / f"hci{index}-state.json"
state: RecoveryState | None = None
if args.restore:
journal = args.restore
state = cast(RecoveryState, json.loads(journal.read_text()))
if state.get("schema") != 1:
parser.error("Unsupported recovery journal")
index = state["index"]
if type(index) is not int or not 0 <= index <= 65534:
parser.error("Invalid adapter index in recovery journal")
session = AdapterSession(index, journal)
peripheral = None
try:
if args.restore:
assert state is not None
session.acquire_lock()
if session.management.info()["address"] != state["info"]["address"]:
raise RuntimeError(
"Recovery journal belongs to a different Bluetooth adapter"
)
session.state = state
session.restore()
elif args.inspect:
log(
"inspection",
**session.management.info(),
connections=session.management.connections(),
hci=session.hci.snapshot(),
)
else:
def stop(_signal, _frame):
global stop_requested
stop_requested = True
for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGHUP):
signal.signal(sig, stop)
session.prepare()
peripheral = Peripheral(session, args.pin_mode, args.peer)
peripheral.serve(args.seconds)
return 0
except (OSError, RuntimeError, ValueError, subprocess.SubprocessError) as exc:
log("error", error=str(exc))
return 1
finally:
if peripheral:
peripheral.close()
try:
session.restore()
finally:
session.close()
if __name__ == "__main__":
raise SystemExit(main())

382
tools/dolphinbar_sdp.py Normal file
View file

@ -0,0 +1,382 @@
"""Small, in-memory SDP server for an original Wii Remote identity.
Wire-format reference: Bluetooth Core, Vol 3, Part B. HID attribute values
are factual values from the original remote record documented in
https://github.com/rnconrad/WiimoteEmulator/blob/master/sdp.c and the identity
and channel assignments at https://wiibrew.org/wiki/Wiimote . No upstream
response arrays or implementation are used here.
"""
from __future__ import annotations
import struct
from collections import OrderedDict
from typing import NamedTuple
_MAX_REQUEST = 4096
_MAX_DESCRIPTOR = 4096
_MAX_ELEMENTS = 512
_MAX_DEPTH = 8
_MAX_CONTINUATIONS = 64
_TOKEN_BYTES = 8
_UUID_BASE_SUFFIX = bytes.fromhex("00001000800000805f9b34fb")
_VALID_SIZES = (
(0,), # Nil
(0, 1, 2, 3, 4), # Unsigned integer
(0, 1, 2, 3, 4), # Signed integer
(1, 2, 4), # UUID
(5, 6, 7), # Text string
(0,), # Boolean
(5, 6, 7), # Sequence
(5, 6, 7), # Alternative
(5, 6, 7), # URL
)
class _SdpError(Exception):
def __init__(self, code: int):
self.code = code
super().__init__(code)
def _encode(kind: int, payload: bytes) -> bytes:
"""Encode a data element, choosing a legal shortest length header."""
size = len(payload)
if kind == 0:
if size:
raise ValueError("Nil data elements have no payload")
return b"\x00"
if kind in (1, 2, 3, 5):
sizes = {1: 0, 2: 1, 4: 2, 8: 3, 16: 4}
index = sizes.get(size, -1)
if index not in _VALID_SIZES[kind]:
raise ValueError("Invalid scalar data element size")
return bytes([(kind << 3) | index]) + payload
if kind not in (4, 6, 7, 8):
raise ValueError("Invalid data element type")
for width, index in ((1, 5), (2, 6), (4, 7)):
if size < 1 << (8 * width):
return bytes([(kind << 3) | index]) + size.to_bytes(width, "big") + payload
raise ValueError("Data element is too large")
def _uint(value: int, width: int = 2) -> bytes:
return _encode(1, value.to_bytes(width, "big"))
def _uuid(value: int) -> bytes:
return _encode(3, value.to_bytes(2, "big"))
def _boolean(value: bool) -> bytes:
return _encode(5, bytes([int(value)]))
def _text(value: str) -> bytes:
return _encode(4, value.encode("utf-8"))
def _sequence(*elements: bytes) -> bytes:
return _encode(6, b"".join(elements))
class _Element(NamedTuple):
kind: int
size: int
value: int | bytes | tuple[_Element, ...] | None
class _Reader:
def __init__(self, data: bytes):
self.data = data
self.position = 0
self.elements = 0
def integer(self, width: int, end: int | None = None) -> int:
if end is None:
end = len(self.data)
if self.position + width > end:
raise _SdpError(0x0003)
value = int.from_bytes(self.data[self.position : self.position + width], "big")
self.position += width
return value
def element(self, end: int | None = None, depth: int = 0) -> _Element:
if end is None:
end = len(self.data)
self.elements += 1
if depth > _MAX_DEPTH or self.elements > _MAX_ELEMENTS:
raise _SdpError(0x0006)
header = self.integer(1, end)
kind, index = header >> 3, header & 7
if kind >= len(_VALID_SIZES) or index not in _VALID_SIZES[kind]:
raise _SdpError(0x0003)
if kind == 0:
return _Element(kind, 0, None)
size = (1 << index) if index < 5 else self.integer(1 << (index - 5), end)
stop = self.position + size
if stop > end:
raise _SdpError(0x0003)
if kind in (6, 7):
children = []
while self.position < stop:
children.append(self.element(stop, depth + 1))
return _Element(kind, size, tuple(children))
raw = self.data[self.position : stop]
self.position = stop
if kind in (1, 2, 5):
value = int.from_bytes(raw, "big", signed=kind == 2)
if kind == 5:
value = int(bool(value))
return _Element(kind, size, value)
return _Element(kind, size, raw)
def search_pattern(self) -> tuple[bytes, ...]:
element = self.element()
if element.kind != 6 or not 1 <= len(element.value) <= 12:
raise _SdpError(0x0003)
uuids = []
for child in element.value:
if child.kind != 3:
raise _SdpError(0x0003)
uuid = child.value
if child.size != 16:
uuid = uuid.rjust(4, b"\x00") + _UUID_BASE_SUFFIX
uuids.append(uuid)
return tuple(uuids)
def attributes(self) -> tuple[tuple[int, int], ...]:
element = self.element()
if element.kind != 6:
raise _SdpError(0x0003)
ranges = []
previous = -1
for child in element.value:
if child.kind != 1 or child.size not in (2, 4):
raise _SdpError(0x0003)
if child.size == 2:
first = last = child.value
else:
first, last = child.value >> 16, child.value & 0xFFFF
if first > last or first <= previous:
raise _SdpError(0x0003)
ranges.append((first, last))
previous = last
return tuple(ranges)
def continuation(self) -> bytes:
size = self.integer(1)
if size > 16:
raise _SdpError(0x0005)
if self.position + size != len(self.data):
raise _SdpError(0x0003)
return self.data[self.position :]
def _records(descriptor: bytes) -> dict[int, dict[int, bytes]]:
language = _sequence(_uint(0x656E), _uint(106), _uint(0x0100))
browse = _sequence(_uuid(0x1002))
def hid_protocol(psm: int) -> bytes:
return _sequence(_sequence(_uuid(0x0100), _uint(psm)), _sequence(_uuid(0x0011)))
hid = {
0x0000: _uint(0x00010000, 4),
0x0001: _sequence(_uuid(0x1124)),
0x0004: hid_protocol(0x0011),
0x0005: browse,
0x0006: language,
0x0009: _sequence(_sequence(_uuid(0x1124), _uint(0x0100))),
0x000D: _sequence(hid_protocol(0x0013)),
0x0100: _text("Nintendo RVL-CNT-01"),
0x0101: _text("Nintendo RVL-CNT-01"),
0x0102: _text("Nintendo"),
0x0200: _uint(0x0100), # HIDDeviceReleaseNumber
0x0201: _uint(0x0111), # HIDParserVersion
0x0202: _uint(0x04, 1), # HIDDeviceSubclass
0x0203: _uint(0x33, 1), # HIDCountryCode in the original record
0x0204: _boolean(False), # HIDVirtualCable
0x0205: _boolean(True), # HIDReconnectInitiate
0x0206: _sequence(_sequence(_uint(0x22, 1), _encode(4, descriptor))),
0x0207: _sequence(_sequence(_uint(0x0409), _uint(0x0100))),
0x0208: _boolean(False), # HIDSDPDisable
0x0209: _boolean(True), # HIDBatteryPower
0x020A: _boolean(True), # HIDRemoteWake
0x020B: _uint(0x0100), # HIDProfileVersion
0x020C: _uint(0x0C80), # HIDSupervisionTimeout
0x020D: _boolean(False), # HIDNormallyConnectable
0x020E: _boolean(False), # HIDBootDevice
}
pnp = {
0x0000: _uint(0x00010001, 4),
0x0001: _sequence(_uuid(0x1200)),
0x0005: browse,
0x0006: language,
0x0009: _sequence(_sequence(_uuid(0x1200), _uint(0x0103))),
0x0100: _text("Nintendo RVL-CNT-01"),
0x0102: _text("Nintendo"),
0x0200: _uint(0x0103), # Device ID specification version
0x0201: _uint(0x057E), # USB vendor: Nintendo
0x0202: _uint(0x0306), # Original Wii Remote
0x0203: _uint(0x0100), # Device version
0x0204: _boolean(True), # PrimaryRecord
0x0205: _uint(0x0002), # VendorIDSource: USB-IF
}
return {0x00010000: hid, 0x00010001: pnp}
def _collect_uuids(element: _Element, result: set[bytes]) -> None:
if element.kind == 3:
uuid = element.value
if element.size != 16:
uuid = uuid.rjust(4, b"\x00") + _UUID_BASE_SUFFIX
result.add(uuid)
elif element.kind in (6, 7):
for child in element.value:
_collect_uuids(child, result)
def _attribute_list(
record: dict[int, bytes], ranges: tuple[tuple[int, int], ...]
) -> bytes:
return _encode(
6,
b"".join(
_uint(attribute) + value
for attribute, value in sorted(record.items())
if any(first <= attribute <= last for first, last in ranges)
),
)
class SdpResponder:
"""Respond to one complete SDP request PDU at a time, without socket IO.
``mtu`` is the peer's L2CAP receive MTU, including the five-byte SDP
header, but not an L2CAP header. Keep this instance for the lifetime of
an SDP connection. At most 64 recently issued continuation states are
retained; evicted or mismatched states receive Invalid Continuation
State. Transaction IDs and attribute byte limits may change between
fragments; the actual query may not.
"""
def __init__(self, report_descriptor: bytes):
if not isinstance(report_descriptor, bytes):
raise TypeError("report_descriptor must be bytes")
if not 1 <= len(report_descriptor) <= _MAX_DESCRIPTOR:
raise ValueError("report_descriptor must contain 1 to 4096 bytes")
self._records = _records(report_descriptor)
self._record_uuids = {}
for handle, record in self._records.items():
uuids = set()
for value in record.values():
_collect_uuids(_Reader(value).element(), uuids)
self._record_uuids[handle] = uuids
self._continuations: OrderedDict[bytes, tuple[tuple, bytes, int]] = (
OrderedDict()
)
self._serial = 0
def _save(self, query: tuple, data: bytes, offset: int) -> bytes:
if self._serial == (1 << (8 * _TOKEN_BYTES)) - 1:
raise _SdpError(0x0006)
self._serial += 1
token = self._serial.to_bytes(_TOKEN_BYTES, "big")
self._continuations[token] = (query, data, offset)
if len(self._continuations) > _MAX_CONTINUATIONS:
self._continuations.popitem(last=False)
return token
def _data(
self,
pdu: int,
target: int | tuple[bytes, ...],
ranges: tuple[tuple[int, int], ...],
maximum: int,
) -> bytes:
if pdu == 0x04:
record = self._records.get(target)
if record is None:
raise _SdpError(0x0002)
return _attribute_list(record, ranges)
handles = [
handle
for handle, uuids in self._record_uuids.items()
if all(uuid in uuids for uuid in target)
]
if pdu == 0x02:
return b"".join(handle.to_bytes(4, "big") for handle in handles[:maximum])
return _sequence(
*(_attribute_list(self._records[handle], ranges) for handle in handles)
)
def _response(self, pdu: int, reader: _Reader, mtu: int) -> bytes:
if pdu not in (0x02, 0x04, 0x06):
raise _SdpError(0x0003)
target = reader.integer(4) if pdu == 0x04 else reader.search_pattern()
maximum = reader.integer(2)
if maximum < (1 if pdu == 0x02 else 7):
raise _SdpError(0x0003)
ranges = () if pdu == 0x02 else reader.attributes()
token = reader.continuation()
query = (pdu, target, ranges, maximum if pdu == 0x02 else None)
if token:
state = self._continuations.get(token)
if state is None or state[0] != query:
raise _SdpError(0x0005)
_, data, offset = state
self._continuations.move_to_end(token)
else:
data = self._data(pdu, target, ranges, maximum)
offset = 0
# Attribute fragments may split any data element, but must each
# contain at least two bytes; search fragments contain whole handles.
search = pdu == 0x02
overhead = 10 if search else 8
limit = 0xFFFF if search else maximum
remaining = len(data) - offset
final_capacity = min(limit, mtu - overhead)
if remaining <= final_capacity:
count = remaining
continuation = b"\x00"
else:
count = min(limit, mtu - overhead - _TOKEN_BYTES)
if search:
count -= count % 4
elif remaining - count == 1:
count -= 1
if count < (4 if search else 2):
raise _SdpError(0x0006)
next_token = self._save(query, data, offset + count)
continuation = bytes([len(next_token)]) + next_token
fragment = data[offset : offset + count]
if search:
prefix = struct.pack(">HH", len(data) // 4, count // 4)
else:
prefix = struct.pack(">H", count)
return prefix + fragment + continuation
def reply(self, packet: bytes, mtu: int = 672) -> bytes:
"""Return the matching response or an SDP ErrorResponse.
Invalid local MTUs below seven bytes raise ValueError: even an SDP
error cannot fit. Requests over 4096 bytes or overly complex data
elements receive Insufficient Resources. A truncated header lacking
a complete transaction ID is answered with transaction ID zero.
"""
if mtu < 7:
raise ValueError("MTU cannot fit an SDP ErrorResponse")
mtu = min(mtu, 0xFFFF + 5)
transaction = int.from_bytes(packet[1:3], "big") if len(packet) >= 3 else 0
try:
if len(packet) < 5 or int.from_bytes(packet[3:5], "big") != len(packet) - 5:
raise _SdpError(0x0004)
if len(packet) > _MAX_REQUEST:
raise _SdpError(0x0006)
response = self._response(packet[0], _Reader(packet[5:]), mtu)
response_pdu = packet[0] + 1
except _SdpError as error:
response_pdu = 0x01
response = struct.pack(">H", error.code)
return struct.pack(">BHH", response_pdu, transaction, len(response)) + response

314
tools/dolphinbar_wiimote.py Normal file
View file

@ -0,0 +1,314 @@
"""RAM-only, neutral RVL-CNT-01 protocol for the DolphinBar experiment.
Wire formats: https://wiibrew.org/wiki/Wiimote . Report handling facts were
cross-checked against rnconrad/WiimoteEmulator and Dolphin's WiimoteEmu.
This module contains no transport, physical inputs, audio output, or file I/O.
"""
from __future__ import annotations
_OUTPUT_LENGTHS = {
0x10: 1,
0x11: 1,
0x12: 2,
0x13: 1,
0x14: 1,
0x15: 1,
0x16: 21,
0x17: 6,
0x18: 21,
0x19: 1,
0x1A: 1,
}
_INPUT_LENGTHS = {
0x20: 6,
0x21: 21,
0x22: 4,
0x30: 2,
0x31: 5,
0x32: 10,
0x33: 17,
0x34: 21,
0x35: 21,
0x36: 21,
0x37: 21,
0x3D: 21,
0x3E: 21,
0x3F: 21,
}
def _hid_descriptor() -> bytes:
# Generic Desktop/Game Pad application; vendor-defined, opaque byte arrays.
descriptor = bytearray.fromhex("05 01 09 05 a1 01 15 00 26 ff 00 75 08 06 00 ff")
for lengths, item in ((_OUTPUT_LENGTHS, 0x91), (_INPUT_LENGTHS, 0x81)):
for report_id, size in lengths.items():
descriptor.extend((0x85, report_id, 0x95, size, 0x09, 0x01, item, 0x00))
descriptor.append(0xC0)
return bytes(descriptor)
HID_DESCRIPTOR = _hid_descriptor()
# High eight accelerometer bits: zero-g = 0x80, one-g = 0x9a.
# A stationary, face-up remote measures (0g, 0g, +1g); low bits are zero.
_ACCEL_ZERO = 0x80
_ACCEL_ONE = 0x9A
_ACCEL = bytes((_ACCEL_ZERO, _ACCEL_ZERO, _ACCEL_ONE))
_BUTTONS = b"\x00\x00"
_ABSENT = b"\xff" * 21
_NEUTRAL_REPORTS = {
0x30: b"\xa1\x30" + _BUTTONS,
0x31: b"\xa1\x31" + _BUTTONS + _ACCEL,
0x32: b"\xa1\x32" + _BUTTONS + _ABSENT[:8],
0x33: b"\xa1\x33" + _BUTTONS + _ACCEL + _ABSENT[:12],
0x34: b"\xa1\x34" + _BUTTONS + _ABSENT[:19],
0x35: b"\xa1\x35" + _BUTTONS + _ACCEL + _ABSENT[:16],
0x36: b"\xa1\x36" + _BUTTONS + _ABSENT[:19],
0x37: b"\xa1\x37" + _BUTTONS + _ACCEL + _ABSENT[:16],
0x3D: b"\xa1\x3d" + _ABSENT,
# The interleaved button fields carry the high and low nibbles of Z's
# eight-bit value, not the low bits used by normal accelerometer reports.
0x3E: bytes(
(
0xA1,
0x3E,
((_ACCEL_ONE >> 4) & 3) << 5,
((_ACCEL_ONE >> 6) & 3) << 5,
_ACCEL_ZERO,
)
)
+ _ABSENT[:18],
0x3F: bytes(
(0xA1, 0x3F, (_ACCEL_ONE & 3) << 5, ((_ACCEL_ONE >> 2) & 3) << 5, _ACCEL_ZERO)
)
+ _ABSENT[:18],
}
def _calibration_block(data: bytes) -> bytes:
return data + bytes(((sum(data) + 0x55) & 0xFF,))
def _new_eeprom() -> bytearray:
eeprom = bytearray(0x1700)
# Symmetric factory reference points in all four camera quadrants. These
# are calibration constants only, never emitted as observed IR spots.
points = ((128, 128), (896, 128), (128, 640), (896, 640))
packed = bytearray()
for index in (0, 2):
x1, y1 = points[index]
x2, y2 = points[index + 1]
high = ((y1 >> 8) << 6) | ((x1 >> 8) << 4) | ((y2 >> 8) << 2) | (x2 >> 8)
packed.extend((x1 & 0xFF, y1 & 0xFF, high, x2 & 0xFF, y2 & 0xFF))
ir_calibration = _calibration_block(bytes(packed))
accel_calibration = _calibration_block(
bytes(
(
_ACCEL_ZERO,
_ACCEL_ZERO,
_ACCEL_ZERO,
0,
_ACCEL_ONE,
_ACCEL_ONE,
_ACCEL_ONE,
0,
0x40,
)
)
)
eeprom[0x00:0x0B] = ir_calibration
eeprom[0x0B:0x16] = ir_calibration
eeprom[0x16:0x20] = accel_calibration
eeprom[0x20:0x2A] = accel_calibration
return eeprom
class Wiimote:
"""An original remote with no buttons pressed, extension, or visible IR.
``handle_output`` consumes report ID + its exact descriptor-sized payload;
callers remove the Bluetooth 0xa2/0x52 header. Every returned packet already
includes 0xa1. Drain command replies before requesting periodic reports.
There is no internal reply queue: a read returns at most 368 packets.
EEPROM writes change only this instance's RAM, not the simulated sensor.
"""
def __init__(self) -> None:
self.ir_enabled = False # Report 0x13 also controls the status flag/I2C.
self.ir_secondary_enabled = False # Report 0x1a, the second camera gate.
self.report_mode = 0x30
self.rumble = False
self.leds = 0
self.speaker_enabled = False
self.speaker_muted = False
self.speaker_data = b"" # Last accepted FIFO packet only; never played.
self._continuous = False
self._report_pending = True
self._interleaved_next = 0x3E
self._pair_pending = False
self._eeprom = _new_eeprom()
self._speaker = bytearray(0x0A)
self._camera = bytearray(0x5B)
self._camera[0x37:0x5B] = b"\xff" * 36
@staticmethod
def _ack(report_id: int, error: int = 0) -> bytes:
return bytes((0xA1, 0x22, 0, 0, report_id, error))
@staticmethod
def _read_reply(address: int, data: bytes = b"", error: int = 0) -> bytes:
size = 16 if error else len(data)
return bytes(
(
0xA1,
0x21,
0,
0,
((size - 1) << 4) | error,
(address >> 8) & 0xFF,
address & 0xFF,
)
) + data.ljust(16, b"\x00")
def status_report(self) -> bytes:
"""Return full battery, no extension, and the actual LED/feature flags."""
flags = (
self.leds | (int(self.speaker_enabled) << 2) | (int(self.ir_enabled) << 3)
)
return bytes((0xA1, 0x20, 0, 0, flags, 0, 0, 0xC0))
def _memory(
self, flags: int, address: int, size: int, writing: bool = False
) -> tuple[bytearray | None, int, int]:
"""Resolve the whole transfer before mutation; never resize a bank."""
space = flags & 0x0C
if space == 0x0C:
return None, 0, 6 # Invalid address space, not EEPROM or I2C.
if space == 0:
offset = address & 0xFFFF # EEPROM mirrors every 64 KiB.
if offset + size <= len(self._eeprom):
return self._eeprom, offset, 0
return None, 0, 8
peripheral = (address >> 16) & 0xFE
offset = address & 0xFF # Peripheral register high address byte is ignored.
if peripheral in (0xA4, 0xA6):
return None, 0, 7 # No extension or MotionPlus on the I2C bus.
if peripheral == 0xA2:
if offset + size <= len(self._speaker):
return self._speaker, offset, 0
elif peripheral == 0xB0:
if not self.ir_enabled:
return None, 0, 7
if offset + size <= 0x34:
return self._camera, offset, 0
if 0x37 <= offset and offset + size <= len(self._camera):
if writing:
return None, 0, 7 # Sensor output is read-only.
return self._camera, offset, 0
return None, 0, 8
def handle_output(self, report: bytes) -> list[bytes]:
"""Apply a well-formed command atomically and return its wire replies.
Unsupported/malformed commands return error 3, absent I2C extensions
error 7, invalid addresses error 8, invalid address-space selection 6.
Malformed reports never even change rumble. Empty input has no report
ID to acknowledge and is ignored. Valid writes always ACK; feature/mode
commands ACK only when requested by bit 1. Status, reads, rumble and
speaker streaming have their own reply rules, as on the original remote.
"""
if not report:
return []
report_id = report[0]
expected = _OUTPUT_LENGTHS.get(report_id)
if expected is None or len(report) != expected + 1:
return [self._ack(report_id, 3)]
flags = report[1]
if report_id == 0x12 and report[2] not in _NEUTRAL_REPORTS:
return [self._ack(report_id, 3)]
if report_id == 0x18 and not 1 <= flags >> 3 <= 20:
return [self._ack(report_id, 3)]
if report_id in (0x16, 0x17):
address = int.from_bytes(report[2:5], "big")
size = (
report[5] if report_id == 0x16 else int.from_bytes(report[5:7], "big")
)
if size == 0 or (report_id == 0x16 and size > 16):
if report_id == 0x17:
return [self._read_reply(address, error=3)]
return [self._ack(report_id, 3)]
# Register zero of the speaker is a streaming FIFO, not a write
# spanning configuration registers. Keep only the latest samples.
speaker_fifo = (
report_id == 0x16
and flags & 0x0C in (4, 8)
and (address >> 16) & 0xFE == 0xA2
and address & 0xFF == 0
)
if speaker_fifo:
self.speaker_data = report[6 : 6 + size]
self.rumble = bool(flags & 1)
return [self._ack(report_id)]
bank, offset, error = self._memory(flags, address, size, report_id == 0x16)
if error:
if report_id == 0x17:
return [self._read_reply(address, error=error)]
return [self._ack(report_id, error)]
self.rumble = bool(flags & 1)
if report_id == 0x16:
bank[offset : offset + size] = report[6 : 6 + size]
return [self._ack(report_id)]
return [
self._read_reply(
address + index,
bytes(bank[offset + index : offset + min(index + 16, size)]),
)
for index in range(0, size, 16)
]
self.rumble = bool(flags & 1)
if report_id == 0x10:
return []
if report_id == 0x11:
self.leds = flags & 0xF0
elif report_id == 0x12:
self.report_mode = report[2]
self._continuous = bool(flags & 4)
self._report_pending = True
self._interleaved_next = 0x3E
self._pair_pending = False
elif report_id == 0x13:
self.ir_enabled = bool(flags & 4)
elif report_id == 0x14:
self.speaker_enabled = bool(flags & 4)
elif report_id == 0x15:
return [self.status_report()]
elif report_id == 0x18:
self.speaker_data = report[2 : 2 + (flags >> 3)]
return []
elif report_id == 0x19:
self.speaker_muted = bool(flags & 4)
elif report_id == 0x1A:
self.ir_secondary_enabled = bool(flags & 4)
return [self._ack(report_id)] if flags & 2 else []
def periodic_report(self) -> bytes | None:
"""Return one 100-Hz tick's neutral report, or None if unchanged.
Interleaved modes always finish their 0x3e/0x3f pair, even with continuous
reporting disabled. The requested mode remains stable for logging.
Immutable packets are shared; idle/continuous ticks do not allocate.
"""
if not (self._continuous or self._report_pending or self._pair_pending):
return None
self._report_pending = False
if self.report_mode in (0x3E, 0x3F):
report_id = self._interleaved_next
self._pair_pending = report_id == 0x3E
self._interleaved_next = 0x3F if self._pair_pending else 0x3E
return _NEUTRAL_REPORTS[report_id]
return _NEUTRAL_REPORTS[self.report_mode]