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

feat: support Switch 2 controllers and expanded profiles

Browse source
This commit is contained in:
Joey Yakimowich-Payne 2026-09-06 23:17:26 -06:00
commit a87917bf0b
51 changed files with 4371 additions and 365 deletions
Download patch file Download diff file Expand all files Collapse all files

977
bluepad32_config/parser/uni_hid_parser_switch2.c Normal file
View file

@ -0,0 +1,977 @@
// SPDX-License-Identifier: Apache-2.0
// Proprietary, unencrypted Switch 2 BLE transport. No SMP or bond-store changes.
// Protocol descriptions: https://github.com/ndeadly/switch2_controller_research
// Pairing/calibration/rumble payloads: https://github.com/Nadeflore/switch2-controllers
// Hold-frame cadence: https://github.com/TommyWabg/Switch2Connect
// Sensor conversion independently adapted from SDL_hidapi_switch2.c (SDL/Valve).
#include "parser/uni_hid_parser_switch2.h"
#include <math.h>
#include <string.h>
#include <btstack.h>
#include "bt/uni_bt_defines.h"
#include "parser/uni_hid_parser_imu.h"
#include "parser/uni_switch2_pairing.h"
#include "sdkconfig.h"
#include "uni_hid_device.h"
#include "uni_log.h"
#define SW2_TIMEOUT_MS 2000
#define SW2_OUTPUT_INTERVAL_MS 13
#define SW2_REPORT_SIZE 63
#define SW2_ACK 0x78
#define SW2_CCCD_UUID 0x2902
// BTstack deserializes UUIDs into canonical (not ATT wire) byte order.
static const uint8_t sw2_service_uuid[16] = {
0xab, 0x7d, 0xe9, 0xbe, 0x89, 0xfe, 0x49, 0xad, 0x82, 0x8f, 0x11, 0x8f, 0x09, 0xdf, 0x7f, 0xd0};
static const uint8_t sw2_input_uuid[16] = {
0xab, 0x7d, 0xe9, 0xbe, 0x89, 0xfe, 0x49, 0xad, 0x82, 0x8f, 0x11, 0x8f, 0x09, 0xdf, 0x7f, 0xd2};
static const uint8_t sw2_command_uuid[16] = {
0x64, 0x9d, 0x4a, 0xc9, 0x8e, 0xb7, 0x4e, 0x6c, 0xaf, 0x44, 0x1e, 0xa5, 0x4f, 0xe5, 0xf0, 0x05};
static const uint8_t sw2_response_uuid[16] = {
0xc7, 0x65, 0xa9, 0x61, 0xd9, 0xd8, 0x4d, 0x36, 0xa2, 0x0a, 0x53, 0x15, 0xb1, 0x11, 0x83, 0x6a};
static const uint8_t sw2_rumble_pro_uuid[16] = {
0xcc, 0x48, 0x3f, 0x51, 0x92, 0x58, 0x42, 0x7d, 0xa9, 0x39, 0x63, 0x0c, 0x31, 0xf7, 0x2b, 0x05};
static const uint8_t sw2_rumble_left_uuid[16] = {
0x28, 0x93, 0x26, 0xcb, 0xa4, 0x71, 0x48, 0x5d, 0xa8, 0xf4, 0x24, 0x0c, 0x14, 0xf1, 0x82, 0x41};
static const uint8_t sw2_rumble_right_uuid[16] = {
0xfa, 0x19, 0xb0, 0xfb, 0xcd, 0x1f, 0x46, 0xa7, 0x84, 0xa1, 0xbb, 0xb0, 0x9e, 0x00, 0xc1, 0x49};
// Public application-protocol payloads from Nadeflore Controller.pair(), NOT
// secret keys or an authenticated BLE pairing mechanism. No security claim.
static const uint8_t sw2_pair_exchange[17] = {
0x00, 0xea, 0xbd, 0x47, 0x13, 0x89, 0x35, 0x42, 0xc6, 0x79, 0xee, 0x07, 0xf2, 0x53, 0x2c, 0x6c, 0x31};
static const uint8_t sw2_pair_confirm[17] = {
0x00, 0x40, 0xb0, 0x8a, 0x5f, 0xcd, 0x1f, 0x9b, 0x41, 0x12, 0x5c, 0xac, 0xc6, 0x3f, 0x38, 0xa0, 0x73};
// Every state transition requires its own completed ATT operation and/or a
// matching application ACK. A timeout disconnects, never skips a setup step.
typedef enum {
SW2_OFF, SW2_ADMITTED, SW2_SERVICE, SW2_CHARACTERISTICS,
SW2_RESPONSE_DESCRIPTOR, SW2_INPUT_DESCRIPTOR, SW2_SUBSCRIBE_RESPONSE,
SW2_INFO, SW2_PAIR, SW2_CALIBRATION, SW2_GYRO_CALIBRATION,
SW2_SUBSCRIBE_INPUT, SW2_FEATURES, SW2_READY,
} sw2_state_t;
typedef enum { SW2_QUERY_NONE, SW2_QUERY_DISCOVERY, SW2_QUERY_CCCD, SW2_QUERY_COMMAND, SW2_QUERY_RUMBLE } sw2_query_t;
typedef struct {
uint16_t center[2];
uint16_t positive[2];
uint16_t negative[2];
} sw2_stick_t;
typedef struct {
uni_hid_device_t* device;
bd_addr_t address;
hci_con_handle_t handle;
bool allocated;
sw2_state_t state;
sw2_query_t query;
uint8_t address_type;
bool needs_pair;
gatt_client_service_t service;
gatt_client_characteristic_t input, response, command, rumble;
uint16_t response_cccd, input_cccd;
gatt_client_notification_t response_listener, input_listener;
bool response_listening, input_listening;
btstack_timer_source_t timeout_timer, output_timer;
bool timeout_active, output_active;
// ATT write-request buffers must outlive the asynchronous call.
uint8_t command_data[32], rumble_data[33], cccd_data[2];
uint8_t command_length;
bool command_pending, command_sent, command_acked;
uint8_t step, calibration_slot;
bool factory_calibration, calibration_done;
uint32_t memory_address;
uint8_t memory_length;
sw2_stick_t sticks[2];
int32_t gyro_bias[3];
uint8_t extra_buttons, leds;
bool leds_pending;
uint8_t rumble_id, weak, strong;
bool rumble_scheduled, rumble_held;
uint32_t rumble_start, rumble_end;
uint32_t sensor_start, sensor_host_start, sensor_last;
uint8_t sensor_warmup;
int32_t gyro_full_scale;
} sw2_instance_t;
// Separate bounded storage: never squeeze transport resources into parser_data
// (256 bytes). Retired buffers are not reused until their old BLE link is gone.
static sw2_instance_t sw2_instances[CONFIG_BLUEPAD32_MAX_DEVICES];
static void sw2_gatt_handler(uint8_t packet_type, uint16_t channel, uint8_t* packet, uint16_t size);
static void sw2_output_tick(btstack_timer_source_t* timer);
static void sw2_continue(sw2_instance_t* ins);
static void sw2_complete_command(sw2_instance_t* ins);
static bool sw2_product(uint16_t pid) {
return pid == UNI_SW2_PRO_PID || pid == UNI_SW2_JOYCON_L_PID || pid == UNI_SW2_JOYCON_R_PID;
}
bool uni_hid_parser_switch2_is_ble_device(const uni_hid_device_t* d) {
return d && d->conn.protocol == UNI_BT_CONN_PROTOCOL_BLE && d->vendor_id == UNI_SW2_NINTENDO_VID &&
sw2_product(d->product_id);
}
static sw2_instance_t* sw2_instance(const uni_hid_device_t* d) {
if (!uni_hid_parser_switch2_is_ble_device(d))
return NULL;
for (unsigned i = 0; i < CONFIG_BLUEPAD32_MAX_DEVICES; ++i) {
sw2_instance_t* ins = &sw2_instances[i];
if (ins->device == d && ins->state != SW2_OFF && memcmp(ins->address, d->conn.btaddr, 6) == 0)
return ins;
}
return NULL;
}
static bool sw2_live(sw2_instance_t* ins) {
return ins->device && sw2_instance(ins->device) == ins && ins->handle == ins->device->conn.handle;
}
static void sw2_disarm_timeout(sw2_instance_t* ins) {
if (ins->timeout_active)
btstack_run_loop_remove_timer(&ins->timeout_timer);
ins->timeout_active = false;
}
void uni_hid_parser_switch2_teardown(uni_hid_device_t* d) {
sw2_instance_t* ins = sw2_instance(d);
if (!ins)
return;
sw2_disarm_timeout(ins);
if (ins->output_active)
btstack_run_loop_remove_timer(&ins->output_timer);
ins->output_active = false;
if (ins->response_listening)
gatt_client_stop_listening_for_characteristic_value_updates(&ins->response_listener);
if (ins->input_listening)
gatt_client_stop_listening_for_characteristic_value_updates(&ins->input_listener);
ins->response_listening = ins->input_listening = false;
ins->command_pending = ins->rumble_scheduled = false;
ins->extra_buttons = 0;
ins->state = SW2_OFF;
ins->device = NULL;
}
static void sw2_fail(sw2_instance_t* ins, const char* reason, unsigned status) {
if (!ins->device)
return;
uni_hid_device_t* d = ins->device;
loge("Switch2: %s (state=%u, status=%u)\n", reason, (unsigned)ins->state, status);
uni_hid_parser_switch2_teardown(d);
uni_hid_device_disconnect(d);
uni_hid_device_delete(d);
}
static void sw2_timeout(btstack_timer_source_t* timer) {
sw2_instance_t* ins = btstack_run_loop_get_timer_context(timer);
ins->timeout_active = false;
if (sw2_live(ins))
sw2_fail(ins, "transaction timeout", 0);
}
static void sw2_arm_timeout(sw2_instance_t* ins) {
sw2_disarm_timeout(ins);
btstack_run_loop_set_timer_handler(&ins->timeout_timer, sw2_timeout);
btstack_run_loop_set_timer_context(&ins->timeout_timer, ins);
btstack_run_loop_set_timer(&ins->timeout_timer, SW2_TIMEOUT_MS);
ins->timeout_active = true;
btstack_run_loop_add_timer(&ins->timeout_timer);
}
static void sw2_schedule_output(sw2_instance_t* ins, uint32_t ms) {
if (ins->output_active)
btstack_run_loop_remove_timer(&ins->output_timer);
btstack_run_loop_set_timer_handler(&ins->output_timer, sw2_output_tick);
btstack_run_loop_set_timer_context(&ins->output_timer, ins);
btstack_run_loop_set_timer(&ins->output_timer, ms ? ms : 1);
ins->output_active = true;
btstack_run_loop_add_timer(&ins->output_timer);
}
static bool sw2_check_query(sw2_instance_t* ins, uint8_t status) {
if (status != ERROR_CODE_SUCCESS) {
sw2_fail(ins, "GATT request rejected", status);
return false;
}
return true;
}
static void sw2_discover_descriptors(sw2_instance_t* ins, bool input) {
ins->state = input ? SW2_INPUT_DESCRIPTOR : SW2_RESPONSE_DESCRIPTOR;
ins->query = SW2_QUERY_DISCOVERY;
sw2_arm_timeout(ins);
sw2_check_query(ins, gatt_client_discover_characteristic_descriptors(
sw2_gatt_handler, ins->handle, input ? &ins->input : &ins->response));
}
static void sw2_subscribe(sw2_instance_t* ins, bool input) {
ins->state = input ? SW2_SUBSCRIBE_INPUT : SW2_SUBSCRIBE_RESPONSE;
ins->query = SW2_QUERY_CCCD;
ins->cccd_data[0] = 1;
ins->cccd_data[1] = 0;
if (input) {
gatt_client_listen_for_characteristic_value_updates(&ins->input_listener, sw2_gatt_handler, ins->handle, &ins->input);
ins->input_listening = true;
} else {
gatt_client_listen_for_characteristic_value_updates(&ins->response_listener, sw2_gatt_handler, ins->handle, &ins->response);
ins->response_listening = true;
}
sw2_arm_timeout(ins);
sw2_check_query(ins, gatt_client_write_characteristic_descriptor_using_descriptor_handle(
sw2_gatt_handler, ins->handle, input ? ins->input_cccd : ins->response_cccd, 2, ins->cccd_data));
}
static bool sw2_transient_write_error(uint8_t status) {
return status == BTSTACK_ACL_BUFFERS_FULL || status == GATT_CLIENT_IN_WRONG_STATE;
}
static void sw2_try_command(sw2_instance_t* ins) {
if (!ins->command_pending || ins->command_sent || ins->query != SW2_QUERY_NONE)
return;
// Re-check immediately before every application-pairing write, including a
// deferred write after ACL backpressure. A closed window cannot write MACs.
if (ins->command_data[0] == 0x15 && !switch_pico_switch2_pairing_allowed()) {
sw2_fail(ins, "pairing window closed", 0);
return;
}
bool no_response = (ins->command.properties & ATT_PROPERTY_WRITE_WITHOUT_RESPONSE) != 0;
uint8_t status;
if (no_response) {
status = gatt_client_write_value_of_characteristic_without_response(
ins->handle, ins->command.value_handle, ins->command_length, ins->command_data);
} else {
ins->query = SW2_QUERY_COMMAND;
status = gatt_client_write_value_of_characteristic(sw2_gatt_handler, ins->handle,
ins->command.value_handle, ins->command_length, ins->command_data);
}
if (status != ERROR_CODE_SUCCESS) {
ins->query = SW2_QUERY_NONE;
if (sw2_transient_write_error(status)) {
sw2_schedule_output(ins, SW2_OUTPUT_INTERVAL_MS);
return;
}
sw2_fail(ins, "command write failed", status);
return;
}
ins->command_sent = true;
}
static void sw2_send_command(sw2_instance_t* ins, uint8_t command, uint8_t subcommand,
const uint8_t* data, uint8_t length) {
if (ins->command_pending || length > sizeof(ins->command_data) - 8) {
sw2_fail(ins, "overlapping or oversized command", length);
return;
}
uint8_t* out = ins->command_data;
memset(out, 0, 8);
out[0] = command;
out[1] = 0x91;
out[2] = 1;
out[3] = subcommand;
out[5] = length;
if (length)
memcpy(out + 8, data, length);
ins->command_length = 8 + length;
ins->command_pending = true;
ins->command_sent = ins->command_acked = false;
sw2_arm_timeout(ins);
sw2_try_command(ins);
}
static void sw2_read_memory(sw2_instance_t* ins, uint32_t address, uint8_t length) {
uint8_t data[8] = {length, 0x7e, 0, 0};
little_endian_store_32(data, 4, address);
ins->memory_address = address;
ins->memory_length = length;
sw2_send_command(ins, 0x02, 0x04, data, sizeof(data));
}
static void sw2_unpack_stick(const uint8_t* data, uint16_t out[2]) {
out[0] = data[0] | ((uint16_t)(data[1] & 15) << 8);
out[1] = (data[1] >> 4) | ((uint16_t)data[2] << 4);
}
static bool sw2_calibration(sw2_stick_t* out, const uint8_t* data) {
sw2_stick_t value;
sw2_unpack_stick(data, value.center);
sw2_unpack_stick(data + 3, value.positive);
sw2_unpack_stick(data + 6, value.negative);
for (unsigned i = 0; i < 2; ++i) {
if (!value.center[i] || value.center[i] == 4095 || !value.positive[i] || !value.negative[i] ||
value.positive[i] > 4095 - value.center[i] || value.negative[i] > value.center[i])
return false;
}
*out = value;
return true;
}
static void sw2_continue(sw2_instance_t* ins) {
switch (ins->state) {
case SW2_INFO:
sw2_read_memory(ins, 0x13000, 0x40);
break;
case SW2_PAIR: {
uint8_t data[14] = {0, 2};
if (ins->step == 0) {
bd_addr_t local;
gap_local_bd_addr(local);
for (unsigned i = 0; i < 6; ++i)
data[2 + i] = data[8 + i] = local[5 - i];
sw2_send_command(ins, 0x15, 0x01, data, sizeof(data));
} else if (ins->step == 1) {
sw2_send_command(ins, 0x15, 0x04, sw2_pair_exchange, sizeof(sw2_pair_exchange));
} else if (ins->step == 2) {
sw2_send_command(ins, 0x15, 0x02, sw2_pair_confirm, sizeof(sw2_pair_confirm));
} else {
sw2_send_command(ins, 0x15, 0x03, data, 1);
}
break;
}
case SW2_CALIBRATION: {
uint32_t address = ins->factory_calibration ? 0x130a8 : 0x1fc042;
if (ins->calibration_slot)
address += ins->factory_calibration ? 0x40 : 0x20;
sw2_read_memory(ins, address, 0x0b);
break;
}
case SW2_GYRO_CALIBRATION:
sw2_read_memory(ins, 0x13044, 12);
break;
case SW2_FEATURES: {
const uint8_t features[4] = {0x04, 0, 0, 0};
sw2_send_command(ins, 0x0c, ins->step ? 0x04 : 0x02, features, sizeof(features));
break;
}
case SW2_READY:
if (ins->leds_pending && !ins->command_pending) {
const uint8_t data[8] = {ins->leds, 0, 0, 0, 0, 0, 0, 0};
ins->leds_pending = false;
sw2_send_command(ins, 0x09, 0x07, data, sizeof(data));
}
break;
default:
break;
}
}
static void sw2_complete_command(sw2_instance_t* ins) {
if (!ins->command_pending || !ins->command_acked || ins->query == SW2_QUERY_COMMAND)
return;
ins->command_pending = false;
sw2_disarm_timeout(ins);
switch (ins->state) {
case SW2_INFO:
ins->state = ins->needs_pair ? SW2_PAIR : SW2_CALIBRATION;
break;
case SW2_PAIR:
if (++ins->step == 4) {
if (!uni_switch2_pairing_remember(ins->address_type, ins->address)) {
sw2_fail(ins, "could not persist application pairing", 0);
return;
}
ins->needs_pair = false;
ins->step = 0;
ins->state = SW2_CALIBRATION;
}
break;
case SW2_CALIBRATION:
if (ins->calibration_done)
ins->state = SW2_GYRO_CALIBRATION;
break;
case SW2_GYRO_CALIBRATION:
sw2_subscribe(ins, true);
return;
case SW2_FEATURES:
if (++ins->step == 2) {
ins->state = SW2_READY;
ins->device->controller.klass = UNI_CONTROLLER_CLASS_GAMEPAD;
if (!uni_hid_device_set_ready_complete(ins->device))
return; // Platform may have destroyed the device.
sw2_schedule_output(ins, 1);
}
break;
default:
break;
}
sw2_continue(ins);
}
static void sw2_response(sw2_instance_t* ins, const uint8_t* data, uint16_t length) {
if (!ins->command_pending || !ins->command_sent || ins->command_acked || length < 8 ||
data[0] != ins->command_data[0] || data[3] != ins->command_data[3] || data[1] != 1 || data[2] != 1)
return; // Unsolicited, truncated or stale ACK is not progress.
if (data[5] != SW2_ACK) {
sw2_fail(ins, "negative application ACK", data[5]);
return;
}
if (data[0] == 0x02) {
if (length < 16 || data[8] != ins->memory_length ||
little_endian_read_32(data, 12) != ins->memory_address || length < 16 + ins->memory_length)
return; // Includes a stale memory response with the same cmd/subcmd.
const uint8_t* value = data + 16;
if (ins->state == SW2_INFO) {
if (little_endian_read_16(value, 18) != UNI_SW2_NINTENDO_VID ||
little_endian_read_16(value, 20) != ins->device->product_id) {
sw2_fail(ins, "controller identity mismatch", 0);
return;
}
} else if (ins->state == SW2_CALIBRATION) {
// Both solo Joy-Cons store their one stick in calibration slot 1.
unsigned stick = ins->device->product_id == UNI_SW2_JOYCON_R_PID ? 1 : ins->calibration_slot;
bool valid = sw2_calibration(&ins->sticks[stick], value);
if (!valid && !ins->factory_calibration) {
ins->factory_calibration = true;
} else {
if (!valid)
logi("Switch2: invalid factory stick calibration, using full-range mapping\n");
ins->factory_calibration = false;
if (ins->device->product_id == UNI_SW2_PRO_PID && ins->calibration_slot == 0)
ins->calibration_slot = 1;
else
ins->calibration_done = true;
}
} else if (ins->state == SW2_GYRO_CALIBRATION) {
for (unsigned i = 0; i < 3; ++i) {
uint32_t bits = little_endian_read_32(value, 4 * i);
float bias;
memcpy(&bias, &bits, sizeof(bias));
// Erased/invalid flash is not a floating-point sensor value.
if (isfinite(bias) && bias >= -40.0f && bias <= 40.0f)
ins->gyro_bias[i] = (int32_t)(bias * (57.295779513f * UNI_IMU_GYRO_RES_PER_DEG_S));
}
}
} else if (ins->state == SW2_PAIR) {
unsigned payload_length = ins->step == 0 ? 9 : ins->step == 3 ? 1 : 17;
if (length < 8 + payload_length)
return;
if (data[8] != 1) {
sw2_fail(ins, "pairing command rejected", data[8]);
return;
}
}
ins->command_acked = true;
sw2_complete_command(ins);
}
static void sw2_query_complete(sw2_instance_t* ins, uint8_t status) {
sw2_query_t query = ins->query;
if (query == SW2_QUERY_NONE)
return;
ins->query = SW2_QUERY_NONE;
if (status != ATT_ERROR_SUCCESS) {
sw2_fail(ins, "GATT transaction failed", status);
return;
}
if (query == SW2_QUERY_COMMAND) {
sw2_complete_command(ins);
return;
}
if (query == SW2_QUERY_RUMBLE) {
++ins->rumble_id;
// A command queued behind this write has its own timeout already.
if (!ins->command_pending)
sw2_disarm_timeout(ins);
sw2_try_command(ins);
return;
}
sw2_disarm_timeout(ins);
switch (ins->state) {
case SW2_SERVICE:
if (!ins->service.start_group_handle) {
sw2_fail(ins, "missing proprietary service", 0);
return;
}
ins->state = SW2_CHARACTERISTICS;
ins->query = SW2_QUERY_DISCOVERY;
sw2_arm_timeout(ins);
sw2_check_query(ins, gatt_client_discover_characteristics_for_service(sw2_gatt_handler, ins->handle, &ins->service));
break;
case SW2_CHARACTERISTICS:
if (!ins->input.value_handle || !ins->response.value_handle || !ins->command.value_handle || !ins->rumble.value_handle) {
sw2_fail(ins, "missing required characteristic", 0);
return;
}
sw2_discover_descriptors(ins, false);
break;
case SW2_RESPONSE_DESCRIPTOR:
if (!ins->response_cccd) {
sw2_fail(ins, "missing response CCCD", 0);
return;
}
sw2_discover_descriptors(ins, true);
break;
case SW2_INPUT_DESCRIPTOR:
if (!ins->input_cccd) {
sw2_fail(ins, "missing input CCCD", 0);
return;
}
sw2_subscribe(ins, false);
break;
case SW2_SUBSCRIBE_RESPONSE:
ins->state = SW2_INFO;
sw2_continue(ins);
break;
case SW2_SUBSCRIBE_INPUT:
ins->state = SW2_FEATURES;
ins->step = 0;
sw2_continue(ins);
break;
default:
sw2_fail(ins, "unexpected query completion", query);
break;
}
}
static void sw2_characteristic(sw2_instance_t* ins, const gatt_client_characteristic_t* characteristic) {
if (ins->state != SW2_CHARACTERISTICS)
return;
const uint8_t* rumble_uuid = ins->device->product_id == UNI_SW2_PRO_PID ? sw2_rumble_pro_uuid :
ins->device->product_id == UNI_SW2_JOYCON_L_PID ? sw2_rumble_left_uuid : sw2_rumble_right_uuid;
gatt_client_characteristic_t* target = NULL;
uint16_t properties = ATT_PROPERTY_NOTIFY;
if (memcmp(characteristic->uuid128, sw2_input_uuid, 16) == 0)
target = &ins->input;
else if (memcmp(characteristic->uuid128, sw2_response_uuid, 16) == 0)
target = &ins->response;
else if (memcmp(characteristic->uuid128, sw2_command_uuid, 16) == 0) {
target = &ins->command;
properties = ATT_PROPERTY_WRITE | ATT_PROPERTY_WRITE_WITHOUT_RESPONSE;
} else if (memcmp(characteristic->uuid128, rumble_uuid, 16) == 0) {
target = &ins->rumble;
properties = ATT_PROPERTY_WRITE | ATT_PROPERTY_WRITE_WITHOUT_RESPONSE;
}
if (!target)
return;
if (target->value_handle || !(characteristic->properties & properties) ||
characteristic->start_handle <= ins->service.start_group_handle ||
characteristic->value_handle <= characteristic->start_handle ||
characteristic->end_handle < characteristic->value_handle ||
characteristic->end_handle > ins->service.end_group_handle) {
sw2_fail(ins, "invalid or ambiguous characteristic", characteristic->value_handle);
return;
}
*target = *characteristic;
}
static void sw2_gatt_handler(uint8_t packet_type, uint16_t channel, uint8_t* packet, uint16_t size) {
(void)channel;
if (packet_type != HCI_EVENT_PACKET || !packet || size < 4)
return;
uint8_t event = packet[0];
// BTstack GATT events have extended lengths: notification length is uint16
// and event[1] may wrap. Validate the actual callback size before accessors.
uint16_t required;
switch (event) {
case GATT_EVENT_SERVICE_QUERY_RESULT: required = 28; break;
case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT: required = 32; break;
case GATT_EVENT_ALL_CHARACTERISTIC_DESCRIPTORS_QUERY_RESULT: required = 26; break;
case GATT_EVENT_QUERY_COMPLETE: required = 9; break;
case GATT_EVENT_NOTIFICATION: required = 12; break;
default: return;
}
if (size < required)
return;
uni_hid_device_t* d = uni_hid_device_get_instance_for_connection_handle(little_endian_read_16(packet, 2));
sw2_instance_t* ins = sw2_instance(d);
if (!ins || !sw2_live(ins))
return;
switch (event) {
case GATT_EVENT_SERVICE_QUERY_RESULT: {
if (ins->state != SW2_SERVICE)
return;
gatt_client_service_t service;
gatt_event_service_query_result_get_service(packet, &service);
if (ins->service.start_group_handle || memcmp(service.uuid128, sw2_service_uuid, 16) != 0 ||
!service.start_group_handle || service.end_group_handle <= service.start_group_handle) {
sw2_fail(ins, "invalid or ambiguous service", 0);
return;
}
ins->service = service;
break;
}
case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT: {
gatt_client_characteristic_t characteristic;
gatt_event_characteristic_query_result_get_characteristic(packet, &characteristic);
sw2_characteristic(ins, &characteristic);
break;
}
case GATT_EVENT_ALL_CHARACTERISTIC_DESCRIPTORS_QUERY_RESULT: {
if (ins->state != SW2_RESPONSE_DESCRIPTOR && ins->state != SW2_INPUT_DESCRIPTOR)
return;
gatt_client_characteristic_descriptor_t descriptor;
gatt_event_all_characteristic_descriptors_query_result_get_characteristic_descriptor(packet, &descriptor);
if (descriptor.uuid16 != SW2_CCCD_UUID)
return;
bool input = ins->state == SW2_INPUT_DESCRIPTOR;
gatt_client_characteristic_t* characteristic = input ? &ins->input : &ins->response;
uint16_t* handle = input ? &ins->input_cccd : &ins->response_cccd;
if (*handle || descriptor.handle <= characteristic->value_handle || descriptor.handle > characteristic->end_handle) {
sw2_fail(ins, "invalid or ambiguous CCCD", descriptor.handle);
return;
}
*handle = descriptor.handle;
break;
}
case GATT_EVENT_QUERY_COMPLETE:
sw2_query_complete(ins, gatt_event_query_complete_get_att_status(packet));
break;
case GATT_EVENT_NOTIFICATION: {
uint16_t length = gatt_event_notification_get_value_length(packet);
if (length > size - 12)
return;
uint16_t handle = gatt_event_notification_get_value_handle(packet);
const uint8_t* value = gatt_event_notification_get_value(packet);
if (ins->response_listening && handle == ins->response.value_handle)
sw2_response(ins, value, length);
else if (ins->state == SW2_READY && ins->input_listening && handle == ins->input.value_handle && length == SW2_REPORT_SIZE) {
uni_hid_parser_switch2_parse_input_report(d, value, length);
uni_hid_device_process_controller(d);
}
break;
}
}
}
bool uni_bt_le_switch2_handle_advertisement(const uint8_t* packet, uint16_t size) {
if (!packet || size < 12 || packet[0] != GAP_EVENT_ADVERTISING_REPORT ||
(unsigned)packet[1] + 2 > size || packet[1] < 10 || packet[11] > packet[1] - 10)
return false;
// Standard ADV_IND only; never connect to scan responses or wake-host data.
if (gap_event_advertising_report_get_advertising_event_type(packet) != 0)
return false;
const uint8_t* data = gap_event_advertising_report_get_data(packet);
unsigned length = gap_event_advertising_report_get_data_length(packet);
const uint8_t* manufacturer = NULL;
for (unsigned offset = 0; offset < length;) {
unsigned field_length = data[offset];
if (!field_length)
break;
if (field_length > length - offset - 1)
return false;
if (data[offset + 1] == 0xff && field_length >= 19) {
const uint8_t* candidate = data + offset + 2;
if (little_endian_read_16(candidate, 0) == 0x0553 &&
little_endian_read_16(candidate, 5) == UNI_SW2_NINTENDO_VID &&
sw2_product(little_endian_read_16(candidate, 7))) {
if (manufacturer)
return false;
manufacturer = candidate;
}
}
offset += field_length + 1;
}
if (!manufacturer)
return false;
bd_addr_t local, address;
gap_local_bd_addr(local);
gap_event_advertising_report_get_address(packet, address);
bool fresh = true, reconnect = true;
for (unsigned i = 0; i < 6; ++i) {
fresh &= manufacturer[12 + i] == 0;
reconnect &= manufacturer[12 + i] == local[5 - i];
}
// Returning true consumes recognized but disallowed Switch2 advertisements,
// preventing generic HID/SMP admission from bypassing this protocol policy.
if ((!fresh && !reconnect) || (fresh && !switch_pico_switch2_pairing_allowed()))
return true;
uint8_t address_type = gap_event_advertising_report_get_address_type(packet);
if (address_type != BD_ADDR_TYPE_LE_PUBLIC &&
!(address_type == BD_ADDR_TYPE_LE_RANDOM && (address[0] & 0xc0) == 0xc0))
return true;
if (!fresh && !uni_switch2_pairing_known(address_type, address))
return true;
if (uni_hid_device_get_instance_for_address(address))
return true;
uint8_t rssi = gap_event_advertising_report_get_rssi(packet);
const uint16_t cod = UNI_BT_COD_MAJOR_PERIPHERAL | UNI_BT_COD_MINOR_GAMEPAD;
if (uni_hid_device_on_device_discovered(address, "Switch2", cod, rssi) != UNI_ERROR_SUCCESS)
return true;
sw2_instance_t* ins = NULL;
for (unsigned i = 0; i < CONFIG_BLUEPAD32_MAX_DEVICES; ++i) {
if (sw2_instances[i].state == SW2_OFF &&
(!sw2_instances[i].allocated || gap_get_connection_type(sw2_instances[i].handle) == GAP_CONNECTION_INVALID)) {
ins = &sw2_instances[i];
break;
}
}
if (!ins)
return true;
uni_hid_device_t* d = uni_hid_device_create(address);
if (!d)
return true;
memset(ins, 0, sizeof(*ins));
ins->allocated = true;
ins->device = d;
memcpy(ins->address, address, sizeof(ins->address));
ins->address_type = address_type;
ins->handle = UNI_BT_CONN_HANDLE_INVALID;
ins->needs_pair = fresh;
ins->state = SW2_ADMITTED;
for (unsigned stick = 0; stick < 2; ++stick) {
for (unsigned axis = 0; axis < 2; ++axis) {
ins->sticks[stick].center[axis] = 2048;
ins->sticks[stick].positive[axis] = 2047;
ins->sticks[stick].negative[axis] = 2048;
}
}
uni_hid_device_set_vendor_id(d, UNI_SW2_NINTENDO_VID);
uni_hid_device_set_product_id(d, little_endian_read_16(manufacturer, 7));
uni_bt_conn_set_protocol(&d->conn, UNI_BT_CONN_PROTOCOL_BLE);
uni_hid_device_guess_controller_type_from_pid_vid(d);
d->sdp_query_type = SDP_QUERY_NOT_NEEDED;
uni_hid_device_set_cod(d, cod);
uni_hid_device_set_name(d, "Switch2");
uni_bt_conn_set_state(&d->conn, UNI_BT_CONN_STATE_DEVICE_DISCOVERED);
d->conn.rssi = rssi;
gap_stop_scan(); // Existing BLE lifecycle owns resuming scan under policy.
uint8_t status = gap_connect(address, address_type);
if (status != ERROR_CODE_SUCCESS)
sw2_fail(ins, "BLE connection request failed", status);
return true;
}
void uni_hid_parser_switch2_on_le_connected(uni_hid_device_t* d) {
sw2_instance_t* ins = sw2_instance(d);
if (!ins || ins->state != SW2_ADMITTED)
return;
ins->handle = d->conn.handle;
if (!d->conn.connected)
uni_hid_device_connect(d);
// A platform callback can reject/disconnect and destroy this device.
if (sw2_instance(d) == ins && d->conn.connected)
uni_hid_device_set_ready(d);
}
void uni_hid_parser_switch2_setup(uni_hid_device_t* d) {
sw2_instance_t* ins = sw2_instance(d);
if (!ins || ins->state != SW2_ADMITTED)
return;
ins->handle = d->conn.handle;
// Standard-compliant 7.5ms minimum; negotiation failure is not setup failure.
int status = gap_update_connection_parameters(ins->handle, 6, 6, 0, 600);
if (status != ERROR_CODE_SUCCESS)
logi("Switch2: connection interval request declined (%d)\n", status);
ins->state = SW2_SERVICE;
ins->query = SW2_QUERY_DISCOVERY;
sw2_arm_timeout(ins);
sw2_check_query(ins, gatt_client_discover_primary_services_by_uuid128(sw2_gatt_handler, ins->handle, sw2_service_uuid));
}
uint8_t uni_hid_parser_switch2_extra_buttons(const uni_hid_device_t* d) {
sw2_instance_t* ins = sw2_instance(d);
return ins ? ins->extra_buttons : 0;
}
bool uni_hid_parser_switch2_identity_address_type(const uni_hid_device_t* d, uint8_t* out) {
sw2_instance_t* ins = sw2_instance(d);
if (!ins || !out)
return false;
*out = ins->address_type;
return true;
}
void uni_hid_parser_switch2_init_report(uni_hid_device_t* d) {
(void)d; // Full snapshots replace state only after their complete length is validated.
}
static int32_t sw2_axis(uint16_t raw, const sw2_stick_t* stick, unsigned axis, bool invert) {
int32_t delta = (int32_t)raw - stick->center[axis];
int32_t result = delta * 512 / (delta < 0 ? stick->negative[axis] : stick->positive[axis]);
if (invert)
result = -result;
return result < -512 ? -512 : result > 511 ? 511 : result;
}
static void sw2_motion(sw2_instance_t* ins, uni_gamepad_t* gp, const uint8_t* report) {
uint32_t timestamp = little_endian_read_32(report, 42);
if (!timestamp || (timestamp == ins->sensor_last && !ins->gyro_full_scale))
return;
ins->sensor_last = timestamp;
if (!ins->gyro_full_scale) {
// SDL detects two sensor clock/range variants. BLE reports need not be
// 4ms apart as USB reports are: compare sensor ticks against elapsed
// host time over >=400ms instead of assuming USB's 100 * 4ms interval.
uint32_t now = btstack_run_loop_get_time_ms();
if (ins->sensor_warmup < 5) {
if (++ins->sensor_warmup == 5) {
ins->sensor_start = timestamp;
ins->sensor_host_start = now;
}
return;
}
uint32_t elapsed = now - ins->sensor_host_start;
if (elapsed < 400)
return;
uint32_t ticks = timestamp - ins->sensor_start; // Defined wraparound.
if (!ticks || elapsed > 2000) {
ins->sensor_start = timestamp;
ins->sensor_host_start = now;
return;
}
uint32_t rate = ticks / elapsed;
// round(34.8 or 40 radians/s * 180/pi * q10).
ins->gyro_full_scale = rate >= 900 && rate <= 1100 ? 2041747 : 2346835;
}
// The common report is SDL's USB 0x05 report without the report-ID byte.
// SDL vertical/paired axes: X, Z, -Y. Backend alone rotates solo Joy-Cons.
static const uint8_t axes[3] = {0, 2, 1};
for (unsigned i = 0; i < 3; ++i) {
unsigned axis = axes[i];
int32_t accel = (int16_t)little_endian_read_16(report, 48 + 2 * axis);
int32_t gyro = (int16_t)little_endian_read_16(report, 54 + 2 * axis);
gp->accel[i] = uni_imu_scale(accel, 32767, 8 * UNI_IMU_ACCEL_RES_PER_G);
gp->gyro[i] = uni_imu_scale(gyro, 32767, ins->gyro_full_scale) - ins->gyro_bias[axis];
if (i == 2) {
gp->accel[i] = -gp->accel[i];
gp->gyro[i] = -gp->gyro[i];
}
}
}
void uni_hid_parser_switch2_parse_input_report(uni_hid_device_t* d, const uint8_t* report, uint16_t len) {
sw2_instance_t* ins = sw2_instance(d);
if (!ins || ins->state != SW2_READY || !report || len != SW2_REPORT_SIZE)
return;
uni_gamepad_t* gp = &d->controller.gamepad;
memset(gp, 0, sizeof(*gp));
uint32_t buttons = little_endian_read_32(report, 4);
gp->buttons = ((buttons & 0x04) ? BUTTON_A : 0) | ((buttons & 0x08) ? BUTTON_B : 0) |
((buttons & 0x01) ? BUTTON_X : 0) | ((buttons & 0x02) ? BUTTON_Y : 0) |
((buttons & 0x40) ? BUTTON_SHOULDER_R : 0) | ((buttons & 0x400000) ? BUTTON_SHOULDER_L : 0) |
((buttons & 0x80) ? BUTTON_TRIGGER_R : 0) | ((buttons & 0x800000) ? BUTTON_TRIGGER_L : 0) |
((buttons & 0x400) ? BUTTON_THUMB_R : 0) | ((buttons & 0x800) ? BUTTON_THUMB_L : 0);
gp->dpad = ((buttons & 0x10000) ? DPAD_DOWN : 0) | ((buttons & 0x20000) ? DPAD_UP : 0) |
((buttons & 0x40000) ? DPAD_RIGHT : 0) | ((buttons & 0x80000) ? DPAD_LEFT : 0);
gp->misc_buttons = ((buttons & 0x100) ? MISC_BUTTON_SELECT : 0) | ((buttons & 0x200) ? MISC_BUTTON_START : 0) |
((buttons & 0x1000) ? MISC_BUTTON_SYSTEM : 0) | ((buttons & 0x2000) ? MISC_BUTTON_CAPTURE : 0);
gp->brake = (buttons & 0x800000) ? 1023 : 0;
gp->throttle = (buttons & 0x80) ? 1023 : 0;
ins->extra_buttons = (buttons & 0x4000) ? UNI_SW2_BUTTON_C : 0;
if (d->product_id == UNI_SW2_PRO_PID)
ins->extra_buttons |= ((buttons & 0x2000000) ? UNI_SW2_BUTTON_GL : 0) | ((buttons & 0x1000000) ? UNI_SW2_BUTTON_GR : 0);
else if (d->product_id == UNI_SW2_JOYCON_L_PID)
ins->extra_buttons |= ((buttons & 0x200000) ? UNI_SW2_BUTTON_LEFT_SL : 0) | ((buttons & 0x100000) ? UNI_SW2_BUTTON_LEFT_SR : 0);
else
ins->extra_buttons |= ((buttons & 0x20) ? UNI_SW2_BUTTON_RIGHT_SL : 0) | ((buttons & 0x10) ? UNI_SW2_BUTTON_RIGHT_SR : 0);
uint16_t axes[2];
if (d->product_id != UNI_SW2_JOYCON_R_PID) {
sw2_unpack_stick(report + 10, axes);
gp->axis_x = sw2_axis(axes[0], &ins->sticks[0], 0, false);
gp->axis_y = sw2_axis(axes[1], &ins->sticks[0], 1, true);
}
if (d->product_id != UNI_SW2_JOYCON_L_PID) {
sw2_unpack_stick(report + 13, axes);
gp->axis_rx = sw2_axis(axes[0], &ins->sticks[1], 0, false);
gp->axis_ry = sw2_axis(axes[1], &ins->sticks[1], 1, true);
}
sw2_motion(ins, gp, report);
d->controller.klass = UNI_CONTROLLER_CLASS_GAMEPAD;
}
void uni_hid_parser_switch2_set_player_leds(uni_hid_device_t* d, uint8_t leds) {
sw2_instance_t* ins = sw2_instance(d);
if (!ins)
return;
ins->leds = leds & 0x0f; // Bluepad32 passes a bitmask, not a player number.
ins->leds_pending = true;
if (ins->state == SW2_READY)
sw2_continue(ins);
}
static void sw2_rumble_block(uint8_t* out, uint8_t id, uint8_t weak, uint8_t strong) {
// Three consecutive equal frames form a safe sustained HOLD. Weak controls
// the high-frequency amplitude, strong the low-frequency amplitude.
uint64_t frame = 0x0e1u | ((uint64_t)strong * 4 << 10) | ((uint64_t)0x1e1 << 20) |
((uint64_t)weak * 4 << 30);
out[0] = 0x50 | (id & 15);
for (unsigned i = 0; i < 5; ++i)
out[1 + i] = (uint8_t)(frame >> (8 * i));
memcpy(out + 6, out + 1, 5);
memcpy(out + 11, out + 1, 5);
}
static void sw2_send_rumble(sw2_instance_t* ins, uint32_t now) {
if (ins->query != SW2_QUERY_NONE || ins->command_pending)
return;
uint8_t weak = 0, strong = 0;
if (ins->rumble_scheduled) {
if (!ins->rumble_held && (int32_t)(now - ins->rumble_end) >= 0)
ins->rumble_scheduled = false;
else if ((int32_t)(now - ins->rumble_start) >= 0) {
weak = ins->weak;
strong = ins->strong;
}
}
ins->rumble_data[0] = 0;
sw2_rumble_block(ins->rumble_data + 1, ins->rumble_id, weak, strong);
uint16_t length = 17;
if (ins->device->product_id == UNI_SW2_PRO_PID) {
memcpy(ins->rumble_data + 17, ins->rumble_data + 1, 16);
length = 33;
}
bool no_response = (ins->rumble.properties & ATT_PROPERTY_WRITE_WITHOUT_RESPONSE) != 0;
uint8_t status;
if (no_response) {
status = gatt_client_write_value_of_characteristic_without_response(ins->handle, ins->rumble.value_handle,
length, ins->rumble_data);
} else {
ins->query = SW2_QUERY_RUMBLE;
if (!ins->command_pending)
sw2_arm_timeout(ins);
status = gatt_client_write_value_of_characteristic(sw2_gatt_handler, ins->handle, ins->rumble.value_handle,
length, ins->rumble_data);
}
if (status == ERROR_CODE_SUCCESS) {
if (no_response)
++ins->rumble_id;
} else {
ins->query = SW2_QUERY_NONE;
if (!ins->command_pending)
sw2_disarm_timeout(ins);
if (!sw2_transient_write_error(status))
sw2_fail(ins, "rumble write failed", status);
}
}
static void sw2_output_tick(btstack_timer_source_t* timer) {
sw2_instance_t* ins = btstack_run_loop_get_timer_context(timer);
ins->output_active = false;
if (!sw2_live(ins))
return;
sw2_try_command(ins);
if (!ins->device)
return;
if (ins->state != SW2_READY)
return; // A blocked setup command rescheduled itself, or awaits its ACK.
uint32_t now = btstack_run_loop_get_time_ms();
sw2_send_rumble(ins, now);
if (!ins->device)
return;
uint32_t next = SW2_OUTPUT_INTERVAL_MS;
if (ins->rumble_scheduled && (!ins->rumble_held || (int32_t)(now - ins->rumble_start) < 0)) {
uint32_t boundary = (int32_t)(now - ins->rumble_start) < 0 ? ins->rumble_start : ins->rumble_end;
if ((int32_t)(boundary - now) > 0 && boundary - now < next)
next = boundary - now;
}
sw2_schedule_output(ins, next);
}
void uni_hid_parser_switch2_play_dual_rumble(uni_hid_device_t* d, uint16_t delay_ms, uint16_t duration_ms,
uint8_t weak, uint8_t strong) {
sw2_instance_t* ins = sw2_instance(d);
if (!ins || ins->state != SW2_READY)
return;
uint32_t now = btstack_run_loop_get_time_ms();
ins->weak = weak;
ins->strong = strong;
ins->rumble_start = now + delay_ms;
ins->rumble_end = ins->rumble_start + duration_ms;
ins->rumble_held = duration_ms == UINT16_MAX;
ins->rumble_scheduled = duration_ms != 0 && (weak != 0 || strong != 0);
// UINT16_MAX is the host's stateful/held sentinel; every other duration is
// finite. Keepalive preserves either effect until replacement or stop.
sw2_schedule_output(ins, 1);
}

47
bluepad32_config/parser/uni_hid_parser_switch2.h Normal file
View file

@ -0,0 +1,47 @@
// SPDX-License-Identifier: Apache-2.0
#pragma once
#include <stdbool.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
struct uni_hid_device_s;
#define UNI_SW2_NINTENDO_VID 0x057e
#define UNI_SW2_PRO_PID 0x2069
#define UNI_SW2_JOYCON_L_PID 0x2067
#define UNI_SW2_JOYCON_R_PID 0x2066
enum {
UNI_SW2_BUTTON_C = 1u << 0,
UNI_SW2_BUTTON_GL = 1u << 1,
UNI_SW2_BUTTON_GR = 1u << 2,
UNI_SW2_BUTTON_LEFT_SL = 1u << 3,
UNI_SW2_BUTTON_LEFT_SR = 1u << 4,
UNI_SW2_BUTTON_RIGHT_SL = 1u << 5,
UNI_SW2_BUTTON_RIGHT_SR = 1u << 6,
};
// Supplied by the platform's existing bounded pairing-window policy.
bool switch_pico_switch2_pairing_allowed(void);
bool uni_bt_le_switch2_handle_advertisement(const uint8_t* packet, uint16_t size);
bool uni_hid_parser_switch2_is_ble_device(const struct uni_hid_device_s* d);
void uni_hid_parser_switch2_on_le_connected(struct uni_hid_device_s* d);
void uni_hid_parser_switch2_setup(struct uni_hid_device_s* d);
void uni_hid_parser_switch2_teardown(struct uni_hid_device_s* d);
void uni_hid_parser_switch2_init_report(struct uni_hid_device_s* d);
void uni_hid_parser_switch2_parse_input_report(struct uni_hid_device_s* d, const uint8_t* report, uint16_t len);
void uni_hid_parser_switch2_set_player_leds(struct uni_hid_device_s* d, uint8_t leds);
void uni_hid_parser_switch2_play_dual_rumble(struct uni_hid_device_s* d, uint16_t delay_ms, uint16_t duration_ms,
uint8_t weak, uint8_t strong);
uint8_t uni_hid_parser_switch2_extra_buttons(const struct uni_hid_device_s* d);
// Validated public/static-random advertisement address, never an RPA or SMP identity.
bool uni_hid_parser_switch2_identity_address_type(const struct uni_hid_device_s* d, uint8_t* out);
#ifdef __cplusplus
}
#endif

110
bluepad32_config/parser/uni_switch2_pairing.c Normal file
View file

@ -0,0 +1,110 @@
// SPDX-License-Identifier: Apache-2.0
#include "parser/uni_switch2_pairing.h"
#include <stddef.h>
#include <string.h>
#include <btstack_tlv.h>
#define PAIRING_TAG 0x53325052u // S2PR, separate from BTstack BTD/BTL records.
#define HEADER_SIZE 2u
#define RECORD_SIZE 7u
#define STORE_SIZE (HEADER_SIZE + UNI_SWITCH2_PAIRING_CAPACITY * RECORD_SIZE)
static bool stable_address(uint8_t type, const uint8_t address[6]) {
if (address == NULL || type > 1) return false;
unsigned any = 0;
unsigned not_ff = 0;
for (unsigned i = 0; i < 6; ++i) {
any |= address[i];
not_ff |= address[i] ^ 0xffu;
}
if (any == 0 || not_ff == 0) return false;
if (type == 0) return true;
if ((address[0] & 0xc0u) != 0xc0u) return false;
// The random portion of a static random address cannot be all zero/one.
any = address[0] & 0x3fu;
not_ff = (address[0] & 0x3fu) ^ 0x3fu;
for (unsigned i = 1; i < 6; ++i) {
any |= address[i];
not_ff |= address[i] ^ 0xffu;
}
return any != 0 && not_ff != 0;
}
static bool load(const btstack_tlv_t** tlv, void** context, uint8_t data[STORE_SIZE]) {
btstack_tlv_get_instance(tlv, context);
if (*tlv == NULL || (*tlv)->get_tag == NULL || (*tlv)->store_tag == NULL) return false;
int size = (*tlv)->get_tag(*context, PAIRING_TAG, data, STORE_SIZE);
if (size == 0) {
data[0] = 1;
data[1] = 0;
return true;
}
if (size < (int)HEADER_SIZE || data[0] != 1 || data[1] > UNI_SWITCH2_PAIRING_CAPACITY ||
size != (int)(HEADER_SIZE + data[1] * RECORD_SIZE)) return false;
for (uint8_t i = 0; i < data[1]; ++i) {
const uint8_t* row = data + HEADER_SIZE + i * RECORD_SIZE;
if (!stable_address(row[0], row + 1)) return false;
if (i != 0 && memcmp(row - RECORD_SIZE, row, RECORD_SIZE) >= 0) return false;
}
return true;
}
bool uni_switch2_pairing_known(uint8_t type, const uint8_t address[6]) {
if (!stable_address(type, address)) return false;
const btstack_tlv_t* tlv;
void* context;
uint8_t data[STORE_SIZE];
if (!load(&tlv, &context, data)) return false;
for (uint8_t i = 0; i < data[1]; ++i) {
const uint8_t* row = data + HEADER_SIZE + i * RECORD_SIZE;
if (row[0] == type && memcmp(row + 1, address, 6) == 0) return true;
}
return false;
}
bool uni_switch2_pairing_remember(uint8_t type, const uint8_t address[6]) {
if (!stable_address(type, address)) return false;
const btstack_tlv_t* tlv;
void* context;
uint8_t data[STORE_SIZE];
if (!load(&tlv, &context, data)) return false;
uint8_t record[RECORD_SIZE];
record[0] = type;
memcpy(record + 1, address, 6);
uint8_t index = 0;
for (; index < data[1]; ++index) {
int order = memcmp(record, data + HEADER_SIZE + index * RECORD_SIZE, RECORD_SIZE);
if (order == 0) return true;
if (order < 0) break;
}
if (data[1] == UNI_SWITCH2_PAIRING_CAPACITY) return false;
uint8_t* insertion = data + HEADER_SIZE + index * RECORD_SIZE;
memmove(insertion + RECORD_SIZE, insertion, (data[1] - index) * RECORD_SIZE);
memcpy(insertion, record, RECORD_SIZE);
++data[1];
return tlv->store_tag(context, PAIRING_TAG, data, HEADER_SIZE + data[1] * RECORD_SIZE) == 0;
}
bool uni_switch2_pairing_clear(void) {
const btstack_tlv_t* tlv;
void* context;
btstack_tlv_get_instance(&tlv, &context);
if (tlv == NULL || tlv->store_tag == NULL) return false;
// Store an empty record rather than the void delete API: callers must know
// whether forgetting was persisted before acknowledging clear-pairings.
const uint8_t empty[HEADER_SIZE] = {1, 0};
return tlv->store_tag(context, PAIRING_TAG, empty, sizeof(empty)) == 0;
}
bool uni_switch2_pairing_get(uint8_t index, uint8_t* type, uint8_t address[6]) {
if (type == NULL || address == NULL) return false;
const btstack_tlv_t* tlv;
void* context;
uint8_t data[STORE_SIZE];
if (!load(&tlv, &context, data) || index >= data[1]) return false;
const uint8_t* row = data + HEADER_SIZE + index * RECORD_SIZE;
*type = row[0];
memcpy(address, row + 1, 6);
return true;
}

21
bluepad32_config/parser/uni_switch2_pairing.h Normal file
View file

@ -0,0 +1,21 @@
// SPDX-License-Identifier: Apache-2.0
#pragma once
#include <stdbool.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
// Application-level trust, not SMP bonds or cryptographic authentication.
// Call on the Bluetooth/storage core after the BTstack TLV backend is ready.
#define UNI_SWITCH2_PAIRING_CAPACITY 16
bool uni_switch2_pairing_known(uint8_t address_type, const uint8_t address[6]);
bool uni_switch2_pairing_remember(uint8_t address_type, const uint8_t address[6]);
bool uni_switch2_pairing_clear(void);
bool uni_switch2_pairing_get(uint8_t index, uint8_t* address_type, uint8_t address[6]);
#ifdef __cplusplus
}
#endif