From 3f90d04a5020750f0d94ffa04f1df637b53b2b92 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Wed, 2 Sep 2026 16:52:59 -0600 Subject: [PATCH] Add persistent controller profile foundation --- CMakeLists.txt | 5 + bluepad32_input_backend.cpp | 358 ++++- bluepad32_input_backend.h | 11 +- configuration_service.cpp | 4 + configuration_service.h | 1 + controller_identity.cpp | 82 ++ controller_identity.h | 30 + controller_profile.cpp | 725 ++++++++++ controller_profile.h | 191 +++ pico_profile_storage.cpp | 126 ++ pico_profile_storage.h | 5 + profile_service.cpp | 548 ++++++++ profile_service.h | 83 ++ profile_storage.cpp | 301 ++++ profile_storage.h | 78 ++ src/switch_pico_bridge/config_manager.py | 1376 ++++++++++++++++++- switch-pico.cpp | 6 +- tests/bluepad32_backend_lifecycle_test.cpp | 466 ++++++- tests/bluepad32_native_stubs/uni.h | 50 + tests/controller_profile_test.cpp | 162 +++ tests/profile_service_test.cpp | 186 +++ tests/profile_storage_test.cpp | 235 ++++ tests/test_config_manager.py | 620 ++++++++- tests/test_controller_profile_native.py | 30 + tests/test_profile_service_native.py | 33 + tests/test_profile_storage_native.py | 31 + tests/usb_configuration_management_test.cpp | 259 ++++ usb_configuration_management.cpp | 201 +++ usb_configuration_management.h | 24 +- 29 files changed, 6157 insertions(+), 70 deletions(-) create mode 100644 controller_identity.cpp create mode 100644 controller_identity.h create mode 100644 controller_profile.cpp create mode 100644 controller_profile.h create mode 100644 pico_profile_storage.cpp create mode 100644 pico_profile_storage.h create mode 100644 profile_service.cpp create mode 100644 profile_service.h create mode 100644 profile_storage.cpp create mode 100644 profile_storage.h create mode 100644 tests/controller_profile_test.cpp create mode 100644 tests/profile_service_test.cpp create mode 100644 tests/profile_storage_test.cpp create mode 100644 tests/test_controller_profile_native.py create mode 100644 tests/test_profile_service_native.py create mode 100644 tests/test_profile_storage_native.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 5fb3461..167d8b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,6 +99,11 @@ add_executable(switch-pico if(SWITCH_PICO_INPUT_BACKEND STREQUAL "BLUEPAD32") target_sources(switch-pico PRIVATE bluepad32_input_backend.cpp + controller_identity.cpp + controller_profile.cpp + profile_storage.cpp + profile_service.cpp + pico_profile_storage.cpp bootsel_pairing_button.cpp adapter_configuration.cpp configuration_storage.cpp diff --git a/bluepad32_input_backend.cpp b/bluepad32_input_backend.cpp index 057669d..b78ed88 100644 --- a/bluepad32_input_backend.cpp +++ b/bluepad32_input_backend.cpp @@ -1,6 +1,7 @@ #include "bluepad32_input_backend.h" #include "controller_hotkey_config.h" #include "configuration_service.h" +#include "profile_service.h" #include #include @@ -111,9 +112,21 @@ struct FeedbackEnvelope { uint8_t strong_magnitude; }; +// Security Manager identity events arrive before Bluepad32 publishes a ready +// device. Retain only the four live handle/address associations so a BLE RPA +// is never promoted to a stable identity on its own. +struct BleIdentityMapping { + bool used; + hci_con_handle_t connection_handle; + bd_addr_t connection_address; + uint8_t identity_address_type; + bd_addr_t identity_address; +}; + struct BackendSlot { ControllerState state; + ControllerIdentity identity; // Non-null with active=false is a connected device still becoming ready. uni_hid_device_t* device; uint32_t state_generation; @@ -132,6 +145,7 @@ struct BackendSlot { critical_section_t g_state_lock; BackendSlot g_slots[kSlotCount]; +BleIdentityMapping g_ble_identity_mappings[kSlotCount]{}; // These acknowledgement generations and the pairing request producer are only // used by Core 0. The request is transferred under the cross-core state lock. @@ -148,6 +162,7 @@ btstack_timer_source_t g_rumble_timer{}; btstack_timer_source_t g_configuration_timer{}; ConnectionStatus g_connection_status = ConnectionStatus::Initializing; btstack_packet_callback_registration_t g_pairing_event_callback{}; +btstack_packet_callback_registration_t g_identity_event_callback{}; ConnectionPolicyState g_connection_policy_state = ConnectionPolicyState::Uninitialized; uint32_t g_pairing_window_deadline_ms = 0; @@ -203,6 +218,186 @@ int slot_for_device(const uni_hid_device_t* device) { const int slot = uni_hid_device_get_idx_for_instance(device); return slot >= 0 && slot < kSlotCount ? slot : -1; } + +bool addresses_equal(const bd_addr_t first, const bd_addr_t second) { + return memcmp(first, second, sizeof(bd_addr_t)) == 0; +} + +BleIdentityMapping* find_ble_identity_mapping( + hci_con_handle_t connection_handle, + const bd_addr_t connection_address) { + for (BleIdentityMapping& mapping : g_ble_identity_mappings) { + if (mapping.used && + mapping.connection_handle == connection_handle && + addresses_equal(mapping.connection_address, + connection_address)) { + return &mapping; + } + } + return nullptr; +} + +BleIdentityMapping* find_ble_identity_mapping_for_handle( + hci_con_handle_t connection_handle) { + for (BleIdentityMapping& mapping : g_ble_identity_mappings) { + if (mapping.used && + mapping.connection_handle == connection_handle) { + return &mapping; + } + } + return nullptr; +} + +BleIdentityMapping* reserve_ble_identity_mapping( + hci_con_handle_t connection_handle) { + BleIdentityMapping* available = nullptr; + for (BleIdentityMapping& mapping : g_ble_identity_mappings) { + if (mapping.used && + mapping.connection_handle == connection_handle) { + return &mapping; + } + if (!mapping.used && available == nullptr) { + available = &mapping; + } + } + return available; +} + +ControllerIdentity make_ble_identity( + const BleIdentityMapping& mapping, const uni_hid_device_t* device) { + ControllerIdentity identity{}; + identity.stable = true; + identity.transport = ControllerTransport::kBle; + identity.address_type = mapping.identity_address_type; + memcpy(identity.address, mapping.identity_address, + sizeof(identity.address)); + identity.vendor_id = device->vendor_id; + identity.product_id = device->product_id; + return identity; +} + +ControllerIdentity identity_for_device(const uni_hid_device_t* device) { + if (device == nullptr) { + return controller_identity_global(); + } + if (device->conn.protocol == UNI_BT_CONN_PROTOCOL_BR_EDR) { + ControllerIdentity identity{}; + identity.stable = true; + identity.transport = ControllerTransport::kClassic; + identity.address_type = BD_ADDR_TYPE_UNKNOWN; + memcpy(identity.address, device->conn.btaddr, + sizeof(identity.address)); + identity.vendor_id = device->vendor_id; + identity.product_id = device->product_id; + return identity; + } + if (device->conn.protocol == UNI_BT_CONN_PROTOCOL_BLE) { + const BleIdentityMapping* mapping = find_ble_identity_mapping( + device->conn.handle, device->conn.btaddr); + if (mapping != nullptr) { + return make_ble_identity(*mapping, device); + } + } + return controller_identity_global(); +} + +void publish_ble_identity(const BleIdentityMapping& mapping) { + ControllerIdentity observed_identity{}; + bool observe_identity = false; + critical_section_enter_blocking(&g_state_lock); + for (BackendSlot& slot : g_slots) { + if (slot.device != nullptr && + slot.device->conn.protocol == UNI_BT_CONN_PROTOCOL_BLE && + slot.device->conn.handle == mapping.connection_handle && + addresses_equal(slot.device->conn.btaddr, + mapping.connection_address)) { + slot.identity = make_ble_identity(mapping, slot.device); + if (slot.active) { + observed_identity = slot.identity; + observe_identity = true; + } + } + } + critical_section_exit(&g_state_lock); + if (observe_identity) { + profile_service_observe_identity_on_storage_core( + observed_identity); + } +} + +void record_ble_identity(hci_con_handle_t connection_handle, + const bd_addr_t connection_address, + uint8_t identity_address_type, + const bd_addr_t identity_address) { + BleIdentityMapping* mapping = + reserve_ble_identity_mapping(connection_handle); + if (mapping == nullptr) { + return; + } + *mapping = {}; + mapping->used = true; + mapping->connection_handle = connection_handle; + memcpy(mapping->connection_address, connection_address, + sizeof(mapping->connection_address)); + mapping->identity_address_type = identity_address_type; + memcpy(mapping->identity_address, identity_address, + sizeof(mapping->identity_address)); + publish_ble_identity(*mapping); +} + +void clear_ble_identity_for_handle(hci_con_handle_t connection_handle) { + for (BleIdentityMapping& mapping : g_ble_identity_mappings) { + if (mapping.used && + mapping.connection_handle == connection_handle) { + mapping = {}; + } + } + + critical_section_enter_blocking(&g_state_lock); + for (BackendSlot& slot : g_slots) { + if (slot.device != nullptr && + slot.device->conn.protocol == UNI_BT_CONN_PROTOCOL_BLE && + slot.device->conn.handle == connection_handle) { + slot.identity = controller_identity_global(); + } + } + critical_section_exit(&g_state_lock); +} + +void clear_ble_identity_for_device(const uni_hid_device_t* device) { + if (device == nullptr || + device->conn.protocol != UNI_BT_CONN_PROTOCOL_BLE) { + return; + } + for (BleIdentityMapping& mapping : g_ble_identity_mappings) { + if (mapping.used && + mapping.connection_handle == device->conn.handle && + addresses_equal(mapping.connection_address, + device->conn.btaddr)) { + mapping = {}; + } + } +} + +void connection_address_for_handle(hci_con_handle_t connection_handle, + const bd_addr_t fallback, + bd_addr_t output) { + const uni_hid_device_t* device = + uni_hid_device_get_instance_for_connection_handle( + connection_handle); + if (device != nullptr) { + memcpy(output, device->conn.btaddr, sizeof(bd_addr_t)); + return; + } + const BleIdentityMapping* mapping = + find_ble_identity_mapping_for_handle(connection_handle); + if (mapping != nullptr) { + memcpy(output, mapping->connection_address, sizeof(bd_addr_t)); + return; + } + memcpy(output, fallback, sizeof(bd_addr_t)); +} + void apply_slot_lighting(uint8_t slot_index, uni_hid_device_t* device) { const SwitchRgbColor color = switch_pro_get_slot_light_color(slot_index); @@ -249,6 +444,7 @@ void publish_all_neutral() { critical_section_enter_blocking(&g_state_lock); for (BackendSlot& slot : g_slots) { slot.state = make_neutral_state(); + slot.identity = controller_identity_global(); slot.device = nullptr; slot.active = false; slot.rumble_pending = false; @@ -256,6 +452,9 @@ void publish_all_neutral() { ++slot.connection_generation; } critical_section_exit(&g_state_lock); + for (BleIdentityMapping& mapping : g_ble_identity_mappings) { + mapping = {}; + } g_connection_status = ConnectionStatus::Initializing; g_connection_policy_state = ConnectionPolicyState::FailedClosed; g_pairing_window_open = false; @@ -498,18 +697,101 @@ bool pairing_window_active_at(uint32_t now_ms) { static_cast(now_ms - g_pairing_window_deadline_ms) < 0; } -void handle_pairing_hci_event(uint8_t packet_type, uint16_t channel, - uint8_t* packet, uint16_t size) { +void handle_btstack_event(uint8_t packet_type, uint16_t channel, + uint8_t* packet, uint16_t size) { (void)channel; - if (packet_type != HCI_EVENT_PACKET || packet == nullptr || size < 8) { + if (packet_type != HCI_EVENT_PACKET || packet == nullptr || size < 2) { return; } bd_addr_t address{}; + bd_addr_t identity_address{}; + bd_addr_t connection_address{}; + hci_con_handle_t connection_handle = 0; const bool pairing_open = pairing_window_active_at(btstack_run_loop_get_time_ms()); switch (hci_event_packet_get_type(packet)) { + case SM_EVENT_IDENTITY_RESOLVING_STARTED: + if (size >= 11) { + clear_ble_identity_for_handle( + sm_event_identity_resolving_started_get_handle(packet)); + } + break; + case SM_EVENT_IDENTITY_RESOLVING_FAILED: + if (size >= 11) { + clear_ble_identity_for_handle( + sm_event_identity_resolving_failed_get_handle(packet)); + } + break; + case SM_EVENT_IDENTITY_RESOLVING_SUCCEEDED: + if (size >= 20) { + connection_handle = + sm_event_identity_resolving_succeeded_get_handle(packet); + sm_event_identity_resolving_succeeded_get_address( + packet, connection_address); + sm_event_identity_resolving_succeeded_get_identity_address( + packet, identity_address); + record_ble_identity( + connection_handle, connection_address, + sm_event_identity_resolving_succeeded_get_identity_addr_type( + packet), + identity_address); + } + break; + case SM_EVENT_IDENTITY_CREATED: + if (size >= 20) { + connection_handle = + sm_event_identity_created_get_handle(packet); + sm_event_identity_created_get_address(packet, address); + sm_event_identity_created_get_identity_address( + packet, identity_address); + connection_address_for_handle( + connection_handle, address, connection_address); + record_ble_identity( + connection_handle, connection_address, + sm_event_identity_created_get_identity_addr_type(packet), + identity_address); + } + break; + case SM_EVENT_REENCRYPTION_STARTED: + if (size >= 11) { + connection_handle = + sm_event_reencryption_started_get_handle(packet); + sm_event_reencryption_started_get_address( + packet, identity_address); + connection_address_for_handle( + connection_handle, identity_address, + connection_address); + record_ble_identity( + connection_handle, connection_address, + sm_event_reencryption_started_get_addr_type(packet), + identity_address); + } + break; + case SM_EVENT_REENCRYPTION_COMPLETE: + if (size >= 12) { + connection_handle = + sm_event_reencryption_complete_get_handle(packet); + if (sm_event_reencryption_complete_get_status(packet) == + ERROR_CODE_SUCCESS) { + sm_event_reencryption_complete_get_address( + packet, identity_address); + connection_address_for_handle( + connection_handle, identity_address, + connection_address); + record_ble_identity( + connection_handle, connection_address, + sm_event_reencryption_complete_get_addr_type(packet), + identity_address); + } else { + clear_ble_identity_for_handle(connection_handle); + } + } + break; case HCI_EVENT_USER_CONFIRMATION_REQUEST: + if (size < 8) { + break; + } hci_event_user_confirmation_request_get_bd_addr(packet, address); if (pairing_open) { gap_ssp_confirmation_response(address); @@ -518,6 +800,9 @@ void handle_pairing_hci_event(uint8_t packet_type, uint16_t channel, } break; case HCI_EVENT_USER_PASSKEY_REQUEST: + if (size < 8) { + break; + } hci_event_user_passkey_request_get_bd_addr(packet, address); if (pairing_open) { gap_ssp_passkey_response(address, 0); @@ -634,6 +919,7 @@ void process_clear_pairings(uint32_t now_ms) { BackendSlot& slot = g_slots[slot_index]; devices[slot_index] = slot.device; slot.state = make_neutral_state(); + slot.identity = controller_identity_global(); slot.device = nullptr; slot.active = false; slot.rumble_pending = false; @@ -648,6 +934,9 @@ void process_clear_pairings(uint32_t now_ms) { if (!requested) { return; } + for (BleIdentityMapping& mapping : g_ble_identity_mappings) { + mapping = {}; + } g_pairing_window_open = false; gap_set_bondable_mode(false); @@ -733,8 +1022,9 @@ void update_status_led() { } void process_configuration_timer(btstack_timer_source_t* timer) { - configuration_service_task_on_storage_core( - btstack_run_loop_get_time_ms()); + const uint32_t now_ms = btstack_run_loop_get_time_ms(); + configuration_service_task_on_storage_core(now_ms); + profile_service_task_on_storage_core(now_ms); btstack_run_loop_set_timer(timer, kConfigurationPollIntervalMs); btstack_run_loop_add_timer(timer); } @@ -828,7 +1118,9 @@ void platform_on_init_complete() { gap_set_bondable_mode(false); sm_set_accepted_stk_generation_methods(0); gap_ssp_set_auto_accept(false); - g_pairing_event_callback.callback = handle_pairing_hci_event; + g_pairing_event_callback.callback = handle_btstack_event; + g_identity_event_callback.callback = handle_btstack_event; + sm_add_event_handler(&g_identity_event_callback); hci_add_event_handler(&g_pairing_event_callback); refresh_pairing_snapshot(); // Keep Bluepad32 autoconnect active whenever at least one slot is free. @@ -869,6 +1161,8 @@ void platform_on_device_connected(uni_hid_device_t* device) { if (slot_index < 0) { return; } + const ControllerIdentity connection_identity = + identity_for_device(device); bool tracked_connection = false; critical_section_enter_blocking(&g_state_lock); @@ -881,6 +1175,9 @@ void platform_on_device_connected(uni_hid_device_t* device) { } else { tracked_connection = slot.device == device; } + if (tracked_connection) { + slot.identity = connection_identity; + } critical_section_exit(&g_state_lock); if (tracked_connection) { @@ -898,18 +1195,18 @@ void platform_on_device_disconnected(uni_hid_device_t* device) { critical_section_enter_blocking(&g_state_lock); BackendSlot& slot = g_slots[slot_index]; if (slot.device == device) { - if (slot.active) { - slot.state = make_neutral_state(); - ++slot.state_generation; - } + slot.state = make_neutral_state(); + slot.identity = controller_identity_global(); slot.device = nullptr; slot.active = false; slot.rumble_pending = false; reset_slot_hotkeys(slot); + ++slot.state_generation; ++slot.connection_generation; disconnected_tracked_device = true; } critical_section_exit(&g_state_lock); + clear_ble_identity_for_device(device); if (disconnected_tracked_device) { // Re-evaluate from scratch: resume discovery only after the final @@ -929,6 +1226,8 @@ uni_error_t platform_on_device_ready(uni_hid_device_t* device) { if (slot_index < 0) { return UNI_ERROR_NO_SLOTS; } + const ControllerIdentity connection_identity = + identity_for_device(device); bool occupied_mismatch = false; bool became_active = false; @@ -936,6 +1235,7 @@ uni_error_t platform_on_device_ready(uni_hid_device_t* device) { BackendSlot& slot = g_slots[slot_index]; occupied_mismatch = slot.device != nullptr && slot.device != device; if (!occupied_mismatch) { + slot.identity = connection_identity; slot.device = device; if (!slot.active) { slot.state = make_neutral_state(); @@ -953,6 +1253,10 @@ uni_error_t platform_on_device_ready(uni_hid_device_t* device) { } if (became_active) { apply_slot_lighting(static_cast(slot_index), device); + if (connection_identity.stable) { + profile_service_observe_identity_on_storage_core( + connection_identity); + } } @@ -1021,6 +1325,7 @@ uni_platform* get_platform() { halt_wireless_backend(); } configuration_service_initialize_on_storage_core(); + profile_service_initialize_on_storage_core(); if (cyw43_arch_init() != 0) { halt_wireless_backend(); } @@ -1047,14 +1352,17 @@ void bluepad32_input_backend_init() { critical_section_init(&g_state_lock); configuration_service_prepare(); + profile_service_prepare(); for (uint8_t slot_index = 0; slot_index < kSlotCount; ++slot_index) { BackendSlot& slot = g_slots[slot_index]; slot = {}; slot.state = make_neutral_state(); + slot.identity = controller_identity_global(); slot.pending_rumble.slot = slot_index; reset_slot_hotkeys(slot); g_consumed_generation[slot_index] = 0; g_last_snapshot_generation[slot_index] = 0; + g_ble_identity_mappings[slot_index] = {}; } g_pairing_window_requested = false; g_pairing_snapshot_requested = false; @@ -1139,27 +1447,29 @@ void bluepad32_input_backend_pairing_snapshot( } -bool bluepad32_input_backend_snapshot(uint8_t slot_index, - ControllerState* out) { - if (out == nullptr || !valid_slot(slot_index)) { - return false; +void bluepad32_input_backend_snapshot(uint8_t slot_index, + Bluepad32SlotSnapshot* out) { + if (out == nullptr) { + return; } - if (!g_initialized) { - *out = make_neutral_state(); - return false; + *out = {}; + if (!valid_slot(slot_index) || !g_initialized) { + return; } critical_section_enter_blocking(&g_state_lock); - *out = g_slots[slot_index].state; - const bool controller_active = g_slots[slot_index].active; - const uint32_t generation = g_slots[slot_index].state_generation; + const BackendSlot& slot = g_slots[slot_index]; + out->active = slot.active; + out->connection_generation = slot.connection_generation; + out->identity = slot.identity; + out->state = slot.state; + const uint32_t state_generation = slot.state_generation; critical_section_exit(&g_state_lock); - if (generation == g_consumed_generation[slot_index]) { - out->motion_sample_count = 0; + if (state_generation == g_consumed_generation[slot_index]) { + out->state.motion_sample_count = 0; } - g_last_snapshot_generation[slot_index] = generation; - return controller_active; + g_last_snapshot_generation[slot_index] = state_generation; } void bluepad32_input_backend_report_sent(uint8_t slot_index) { diff --git a/bluepad32_input_backend.h b/bluepad32_input_backend.h index d7bdaa6..4d359b8 100644 --- a/bluepad32_input_backend.h +++ b/bluepad32_input_backend.h @@ -3,6 +3,7 @@ #include #include "controller_color.h" +#include "controller_identity.h" #include "controller_state.h" #include "switch_haptics.h" @@ -32,13 +33,21 @@ struct Bluepad32PairingSnapshot { bool overflow; Bluepad32PairingRecord records[BLUEPAD32_PAIRING_RECORD_CAPACITY]; }; +struct Bluepad32SlotSnapshot { + bool active; + uint32_t connection_generation; + ControllerIdentity identity; + ControllerState state; +}; + void bluepad32_input_backend_init(); void bluepad32_input_backend_start(); void bluepad32_input_backend_open_pairing_window(); void bluepad32_input_backend_clear_pairings(); -bool bluepad32_input_backend_snapshot(uint8_t slot, ControllerState* out); +void bluepad32_input_backend_snapshot(uint8_t slot, + Bluepad32SlotSnapshot* out); void bluepad32_input_backend_request_pairing_snapshot(); void bluepad32_input_backend_pairing_snapshot( Bluepad32PairingSnapshot* out); diff --git a/configuration_service.cpp b/configuration_service.cpp index e693947..d41199c 100644 --- a/configuration_service.cpp +++ b/configuration_service.cpp @@ -171,6 +171,10 @@ ConfigurationTransactionStatus configuration_service_reset( if (status == ConfigurationTransactionStatus::kReceiving) { status = g_transaction.finish(transaction_id); } + if (status == ConfigurationTransactionStatus::kPending && + g_snapshot.reset_generation != UINT32_MAX) { + ++g_snapshot.reset_generation; + } g_snapshot.transaction = g_transaction.snapshot(); critical_section_exit(&g_lock); return status; diff --git a/configuration_service.h b/configuration_service.h index 5a12709..9eb8ceb 100644 --- a/configuration_service.h +++ b/configuration_service.h @@ -17,6 +17,7 @@ struct ConfigurationServiceSnapshot { AdapterConfiguration configuration{}; uint32_t generation = 0; uint32_t payload_crc = 0; + uint32_t reset_generation = 0; ConfigurationTransactionSnapshot transaction{}; }; diff --git a/controller_identity.cpp b/controller_identity.cpp new file mode 100644 index 0000000..bfe32b3 --- /dev/null +++ b/controller_identity.cpp @@ -0,0 +1,82 @@ +#include "controller_identity.h" + +#include + +namespace { + +bool controller_identity_valid(const ControllerIdentity& identity) { + const uint8_t transport = static_cast(identity.transport); + if (transport > static_cast(ControllerTransport::kBle)) { + return false; + } + if (identity.stable) { + return identity.transport != ControllerTransport::kUnknown; + } + return controller_identity_is_global(identity); +} + +} // namespace + +ControllerIdentity controller_identity_global() { + return {}; +} + +bool controller_identity_is_global(const ControllerIdentity& identity) { + const ControllerIdentity global{}; + return controller_identity_equal(identity, global); +} + +bool controller_identity_equal(const ControllerIdentity& first, + const ControllerIdentity& second) { + return first.stable == second.stable && + first.transport == second.transport && + first.address_type == second.address_type && + memcmp(first.address, second.address, sizeof(first.address)) == 0 && + first.vendor_id == second.vendor_id && + first.product_id == second.product_id; +} + +bool controller_identity_encode(const ControllerIdentity& identity, + uint8_t* output, size_t output_size) { + if (output == nullptr || output_size < CONTROLLER_IDENTITY_ENCODED_SIZE || + !controller_identity_valid(identity)) { + return false; + } + + output[0] = identity.stable ? 1 : 0; + output[1] = static_cast(identity.transport); + output[2] = identity.address_type; + output[3] = 0; + memcpy(&output[4], identity.address, sizeof(identity.address)); + output[10] = static_cast(identity.vendor_id); + output[11] = static_cast(identity.vendor_id >> 8); + output[12] = static_cast(identity.product_id); + output[13] = static_cast(identity.product_id >> 8); + return true; +} + +bool controller_identity_decode(const uint8_t* input, size_t input_size, + ControllerIdentity* output) { + if (input == nullptr || output == nullptr || + input_size != CONTROLLER_IDENTITY_ENCODED_SIZE || input[0] > 1 || + input[1] > static_cast(ControllerTransport::kBle) || + input[3] != 0) { + return false; + } + + ControllerIdentity decoded{}; + decoded.stable = input[0] != 0; + decoded.transport = static_cast(input[1]); + decoded.address_type = input[2]; + memcpy(decoded.address, &input[4], sizeof(decoded.address)); + decoded.vendor_id = static_cast(input[10]) | + static_cast(input[11] << 8); + decoded.product_id = static_cast(input[12]) | + static_cast(input[13] << 8); + if (!controller_identity_valid(decoded)) { + return false; + } + + *output = decoded; + return true; +} diff --git a/controller_identity.h b/controller_identity.h new file mode 100644 index 0000000..c363991 --- /dev/null +++ b/controller_identity.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include + +constexpr size_t CONTROLLER_IDENTITY_ENCODED_SIZE = 14; + +enum class ControllerTransport : uint8_t { + kUnknown = 0, + kClassic = 1, + kBle = 2, +}; + +struct ControllerIdentity { + bool stable = false; + ControllerTransport transport = ControllerTransport::kUnknown; + uint8_t address_type = 0; + uint8_t address[6]{}; + uint16_t vendor_id = 0; + uint16_t product_id = 0; +}; + +ControllerIdentity controller_identity_global(); +bool controller_identity_is_global(const ControllerIdentity& identity); +bool controller_identity_equal(const ControllerIdentity& first, + const ControllerIdentity& second); +bool controller_identity_encode(const ControllerIdentity& identity, + uint8_t* output, size_t output_size); +bool controller_identity_decode(const uint8_t* input, size_t input_size, + ControllerIdentity* output); diff --git a/controller_profile.cpp b/controller_profile.cpp new file mode 100644 index 0000000..9b3c47f --- /dev/null +++ b/controller_profile.cpp @@ -0,0 +1,725 @@ +#include "controller_profile.h" + +#include + +namespace { + +constexpr uint8_t kDatabaseMagic[4] = {'S', 'P', 'D', 'B'}; +constexpr size_t kFallbackOffset = CONTROLLER_PROFILE_DATABASE_HEADER_SIZE; +constexpr size_t kEntriesOffset = + kFallbackOffset + CONTROLLER_PROFILE_COUNT * + CONTROLLER_PROFILE_ENCODED_SIZE; +constexpr uint8_t kStickInvertX = 1u << 0; +constexpr uint8_t kStickInvertY = 1u << 1; +constexpr uint8_t kMacroOverrideMask = + kControllerProfileOverrideButtons | + kControllerProfileOverrideLeftStick | + kControllerProfileOverrideRightStick | + kControllerProfileOverrideLeftTrigger | + kControllerProfileOverrideRightTrigger; + +uint16_t profile_read_u16(const uint8_t* input) { + return static_cast(input[0]) | + (static_cast(input[1]) << 8); +} + +int16_t profile_read_i16(const uint8_t* input) { + return static_cast(profile_read_u16(input)); +} + +void profile_write_u16(uint8_t* output, uint16_t value) { + output[0] = static_cast(value); + output[1] = static_cast(value >> 8); +} + +void profile_write_i16(uint8_t* output, int16_t value) { + profile_write_u16(output, static_cast(value)); +} + +bool profile_bytes_are_zero(const uint8_t* data, size_t size) { + for (size_t index = 0; index < size; ++index) { + if (data[index] != 0) { + return false; + } + } + return true; +} + +bool valid_button(uint8_t button) { + return button < CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT || + button == CONTROLLER_PROFILE_NO_BUTTON; +} + +bool valid_macro_step(const ControllerProfileMacroStep& step, + bool must_end) { + if (must_end) { + return step.type == ControllerProfileMacroStepType::kEnd && + step.override_flags == 0 && step.duration_ms == 0 && + step.output_button_mask == 0 && step.left_stick_x == 0 && + step.left_stick_y == 0 && step.right_stick_x == 0 && + step.right_stick_y == 0 && step.left_trigger == 0 && + step.right_trigger == 0; + } + if (step.type != ControllerProfileMacroStepType::kState || + (step.override_flags & ~kMacroOverrideMask) != 0 || + step.duration_ms > CONTROLLER_PROFILE_MAX_WAIT_MS) { + return false; + } + if ((step.override_flags & kControllerProfileOverrideButtons) == 0 && + step.output_button_mask != 0) { + return false; + } + if ((step.override_flags & kControllerProfileOverrideLeftStick) == 0 && + (step.left_stick_x != 0 || step.left_stick_y != 0)) { + return false; + } + if ((step.override_flags & kControllerProfileOverrideRightStick) == 0 && + (step.right_stick_x != 0 || step.right_stick_y != 0)) { + return false; + } + if ((step.override_flags & kControllerProfileOverrideLeftTrigger) == 0 && + step.left_trigger != 0) { + return false; + } + if ((step.override_flags & kControllerProfileOverrideRightTrigger) == 0 && + step.right_trigger != 0) { + return false; + } + return true; +} + +void copy_overlap(size_t range_offset, uint8_t* output, + size_t output_size, size_t field_offset, + const uint8_t* field, size_t field_size) { + const size_t range_end = range_offset + output_size; + const size_t field_end = field_offset + field_size; + if (range_offset >= field_end || field_offset >= range_end) { + return; + } + const size_t start = range_offset > field_offset + ? range_offset + : field_offset; + const size_t end = range_end < field_end ? range_end : field_end; + memcpy(&output[start - range_offset], &field[start - field_offset], + end - start); +} + +bool read_zero_region(ControllerProfileDatabaseRead read, void* context, + size_t offset, size_t size) { + uint8_t buffer[CONTROLLER_PROFILE_ENCODED_SIZE]{}; + while (size != 0) { + const size_t chunk = size < sizeof(buffer) ? size : sizeof(buffer); + if (!read(context, offset, buffer, chunk) || + !profile_bytes_are_zero(buffer, chunk)) { + return false; + } + offset += chunk; + size -= chunk; + } + return true; +} + +} // namespace + +ControllerProfile controller_profile_default(const ControllerIdentity& identity, + uint8_t profile_index) { + (void)identity; + (void)profile_index; + ControllerProfile profile{}; + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT; ++index) { + profile.button_map[index] = index; + profile.turbo_modes[index] = ControllerProfileTurboMode::kOff; + } + for (ControllerProfileStickConfiguration& stick : profile.sticks) { + stick.center_x = 0; + stick.center_y = 0; + stick.inner_deadzone = 0; + stick.outer_saturation = 32767; + stick.curve_q8_8 = 256; + stick.invert_x = false; + stick.invert_y = false; + } + for (ControllerProfileTriggerConfiguration& trigger : profile.triggers) { + trigger.lower_deadzone = 0; + trigger.upper_saturation = UINT16_MAX; + trigger.curve_q8_8 = 256; + trigger.digital_threshold = 0x8000; + } + profile.weak_rumble_scale = UINT8_MAX; + profile.strong_rumble_scale = UINT8_MAX; + profile.confirmation_policy = + ControllerProfileConfirmationPolicy::kRumbleAndLed; + profile.switching_chord = 0; + profile.macro_trigger = CONTROLLER_PROFILE_NO_BUTTON; + profile.macro_cancel = CONTROLLER_PROFILE_NO_BUTTON; + profile.macro_step_count = 1; + for (ControllerProfileMacroStep& step : profile.macro_steps) { + step = {}; + step.type = ControllerProfileMacroStepType::kEnd; + } + return profile; +} + +bool controller_profile_validate(const ControllerProfile& profile) { + for (uint8_t output : profile.button_map) { + if (!valid_button(output)) { + return false; + } + } + for (const ControllerProfileStickConfiguration& stick : profile.sticks) { + if (stick.inner_deadzone >= stick.outer_saturation || + stick.outer_saturation > 32767 || stick.curve_q8_8 == 0) { + return false; + } + } + for (const ControllerProfileTriggerConfiguration& trigger : + profile.triggers) { + if (trigger.lower_deadzone >= trigger.upper_saturation || + trigger.curve_q8_8 == 0 || + trigger.digital_threshold < trigger.lower_deadzone || + trigger.digital_threshold > trigger.upper_saturation) { + return false; + } + } + if (static_cast(profile.confirmation_policy) > + static_cast( + ControllerProfileConfirmationPolicy::kRumbleAndLed) || + !valid_button(profile.macro_trigger) || + !valid_button(profile.macro_cancel) || + profile.macro_step_count == 0 || + profile.macro_step_count > CONTROLLER_PROFILE_MACRO_STEP_CAPACITY) { + return false; + } + for (ControllerProfileTurboMode mode : profile.turbo_modes) { + if (static_cast(mode) > + static_cast(ControllerProfileTurboMode::kAutoBurst)) { + return false; + } + } + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_MACRO_STEP_CAPACITY; ++index) { + const bool must_end = index >= profile.macro_step_count - 1; + if (!valid_macro_step(profile.macro_steps[index], must_end)) { + return false; + } + } + return true; +} + +bool controller_profile_encode(const ControllerProfile& profile, + uint8_t* output, size_t output_size) { + if (output == nullptr || output_size != CONTROLLER_PROFILE_ENCODED_SIZE || + !controller_profile_validate(profile)) { + return false; + } + memset(output, 0, output_size); + profile_write_u16(&output[0], CONTROLLER_PROFILE_SCHEMA_VERSION); + profile_write_u16(&output[2], CONTROLLER_PROFILE_ENCODED_SIZE); + memcpy(&output[4], profile.button_map, + CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT); + for (uint8_t index = 0; index < 2; ++index) { + const ControllerProfileStickConfiguration& stick = + profile.sticks[index]; + uint8_t* encoded = &output[20 + index * 16]; + profile_write_i16(&encoded[0], stick.center_x); + profile_write_i16(&encoded[2], stick.center_y); + profile_write_u16(&encoded[4], stick.inner_deadzone); + profile_write_u16(&encoded[6], stick.outer_saturation); + profile_write_u16(&encoded[8], stick.curve_q8_8); + encoded[10] = (stick.invert_x ? kStickInvertX : 0) | + (stick.invert_y ? kStickInvertY : 0); + } + for (uint8_t index = 0; index < 2; ++index) { + const ControllerProfileTriggerConfiguration& trigger = + profile.triggers[index]; + uint8_t* encoded = &output[52 + index * 10]; + profile_write_u16(&encoded[0], trigger.lower_deadzone); + profile_write_u16(&encoded[2], trigger.upper_saturation); + profile_write_u16(&encoded[4], trigger.curve_q8_8); + profile_write_u16(&encoded[6], trigger.digital_threshold); + } + output[72] = profile.weak_rumble_scale; + output[73] = profile.strong_rumble_scale; + output[74] = static_cast(profile.confirmation_policy); + profile_write_u16(&output[76], profile.switching_chord); + output[78] = profile.macro_trigger; + output[79] = profile.macro_cancel; + output[80] = profile.macro_step_count; + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT; ++index) { + output[82 + index] = + static_cast(profile.turbo_modes[index]); + } + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_MACRO_STEP_CAPACITY; ++index) { + const ControllerProfileMacroStep& step = profile.macro_steps[index]; + uint8_t* encoded = &output[100 + index * 19]; + encoded[0] = static_cast(step.type); + encoded[1] = step.override_flags; + profile_write_u16(&encoded[2], step.duration_ms); + profile_write_u16(&encoded[4], step.output_button_mask); + profile_write_i16(&encoded[6], step.left_stick_x); + profile_write_i16(&encoded[8], step.left_stick_y); + profile_write_i16(&encoded[10], step.right_stick_x); + profile_write_i16(&encoded[12], step.right_stick_y); + profile_write_u16(&encoded[14], step.left_trigger); + profile_write_u16(&encoded[16], step.right_trigger); + } + return true; +} + +bool controller_profile_decode(const uint8_t* input, size_t input_size, + ControllerProfile* output) { + if (input == nullptr || output == nullptr || + input_size != CONTROLLER_PROFILE_ENCODED_SIZE || + profile_read_u16(&input[0]) != CONTROLLER_PROFILE_SCHEMA_VERSION || + profile_read_u16(&input[2]) != CONTROLLER_PROFILE_ENCODED_SIZE || + !profile_bytes_are_zero(&input[31], 5) || !profile_bytes_are_zero(&input[47], 5) || + !profile_bytes_are_zero(&input[60], 2) || !profile_bytes_are_zero(&input[70], 2) || + input[75] != 0 || input[81] != 0 || + !profile_bytes_are_zero(&input[98], 2) || !profile_bytes_are_zero(&input[252], 4)) { + return false; + } + + ControllerProfile profile{}; + memcpy(profile.button_map, &input[4], + CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT); + for (uint8_t index = 0; index < 2; ++index) { + ControllerProfileStickConfiguration& stick = profile.sticks[index]; + const uint8_t* encoded = &input[20 + index * 16]; + if ((encoded[10] & ~(kStickInvertX | kStickInvertY)) != 0) { + return false; + } + stick.center_x = profile_read_i16(&encoded[0]); + stick.center_y = profile_read_i16(&encoded[2]); + stick.inner_deadzone = profile_read_u16(&encoded[4]); + stick.outer_saturation = profile_read_u16(&encoded[6]); + stick.curve_q8_8 = profile_read_u16(&encoded[8]); + stick.invert_x = (encoded[10] & kStickInvertX) != 0; + stick.invert_y = (encoded[10] & kStickInvertY) != 0; + } + for (uint8_t index = 0; index < 2; ++index) { + ControllerProfileTriggerConfiguration& trigger = + profile.triggers[index]; + const uint8_t* encoded = &input[52 + index * 10]; + trigger.lower_deadzone = profile_read_u16(&encoded[0]); + trigger.upper_saturation = profile_read_u16(&encoded[2]); + trigger.curve_q8_8 = profile_read_u16(&encoded[4]); + trigger.digital_threshold = profile_read_u16(&encoded[6]); + } + profile.weak_rumble_scale = input[72]; + profile.strong_rumble_scale = input[73]; + profile.confirmation_policy = + static_cast(input[74]); + profile.switching_chord = profile_read_u16(&input[76]); + profile.macro_trigger = input[78]; + profile.macro_cancel = input[79]; + profile.macro_step_count = input[80]; + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT; ++index) { + profile.turbo_modes[index] = + static_cast(input[82 + index]); + } + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_MACRO_STEP_CAPACITY; ++index) { + ControllerProfileMacroStep& step = profile.macro_steps[index]; + const uint8_t* encoded = &input[100 + index * 19]; + if (encoded[18] != 0) { + return false; + } + step.type = static_cast(encoded[0]); + step.override_flags = encoded[1]; + step.duration_ms = profile_read_u16(&encoded[2]); + step.output_button_mask = profile_read_u16(&encoded[4]); + step.left_stick_x = profile_read_i16(&encoded[6]); + step.left_stick_y = profile_read_i16(&encoded[8]); + step.right_stick_x = profile_read_i16(&encoded[10]); + step.right_stick_y = profile_read_i16(&encoded[12]); + step.left_trigger = profile_read_u16(&encoded[14]); + step.right_trigger = profile_read_u16(&encoded[16]); + } + if (!controller_profile_validate(profile)) { + return false; + } + *output = profile; + return true; +} + +void controller_profile_database_default(ControllerProfileDatabase* database) { + if (database == nullptr) { + return; + } + database->fallback_active_profile = 0; + const ControllerIdentity global = controller_identity_global(); + for (ControllerProfileDatabaseEntry& entry : database->entries) { + entry.used = false; + entry.identity = global; + entry.active_profile = 0; + } + for (uint8_t profile_index = 0; + profile_index < CONTROLLER_PROFILE_COUNT; ++profile_index) { + database->fallback_profiles[profile_index] = + controller_profile_default(global, profile_index); + } +} + +bool controller_profile_database_validate( + const ControllerProfileDatabase& database) { + if (database.fallback_active_profile >= CONTROLLER_PROFILE_COUNT) { + return false; + } + for (const ControllerProfile& profile : database.fallback_profiles) { + if (!controller_profile_validate(profile)) { + return false; + } + } + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY; ++index) { + const ControllerProfileDatabaseEntry& entry = database.entries[index]; + if (!entry.used) { + continue; + } + if (!entry.identity.stable || + controller_identity_is_global(entry.identity) || + entry.active_profile >= CONTROLLER_PROFILE_COUNT) { + return false; + } + for (uint8_t prior = 0; prior < index; ++prior) { + if (database.entries[prior].used && + controller_identity_equal(database.entries[prior].identity, + entry.identity)) { + return false; + } + } + for (const ControllerProfile& profile : entry.profiles) { + if (!controller_profile_validate(profile)) { + return false; + } + } + } + return true; +} + +bool controller_profile_database_encode_range( + const ControllerProfileDatabase& database, size_t offset, + uint8_t* output, size_t size) { + if (output == nullptr || offset > CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE || + size > CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset || + !controller_profile_database_validate(database)) { + return false; + } + memset(output, 0, size); + uint8_t header[CONTROLLER_PROFILE_DATABASE_HEADER_SIZE]{}; + memcpy(header, kDatabaseMagic, sizeof(kDatabaseMagic)); + profile_write_u16(&header[4], + CONTROLLER_PROFILE_DATABASE_SCHEMA_VERSION); + profile_write_u16(&header[6], CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE); + header[8] = CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY; + header[9] = CONTROLLER_PROFILE_COUNT; + header[10] = database.fallback_active_profile; + uint8_t used_count = 0; + for (const ControllerProfileDatabaseEntry& entry : database.entries) { + used_count += entry.used ? 1 : 0; + } + header[11] = used_count; + copy_overlap(offset, output, size, 0, header, sizeof(header)); + + uint8_t encoded_profile[CONTROLLER_PROFILE_ENCODED_SIZE]{}; + for (uint8_t profile_index = 0; + profile_index < CONTROLLER_PROFILE_COUNT; ++profile_index) { + const size_t profile_offset = + kFallbackOffset + + profile_index * CONTROLLER_PROFILE_ENCODED_SIZE; + if (offset < profile_offset + CONTROLLER_PROFILE_ENCODED_SIZE && + offset + size > profile_offset) { + if (!controller_profile_encode( + database.fallback_profiles[profile_index], + encoded_profile, sizeof(encoded_profile))) { + return false; + } + copy_overlap(offset, output, size, profile_offset, + encoded_profile, sizeof(encoded_profile)); + } + } + + for (uint8_t entry_index = 0; + entry_index < CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY; + ++entry_index) { + const ControllerProfileDatabaseEntry& entry = + database.entries[entry_index]; + if (!entry.used) { + continue; + } + const size_t entry_offset = + kEntriesOffset + entry_index * CONTROLLER_PROFILE_DATABASE_ENTRY_SIZE; + uint8_t entry_header[CONTROLLER_PROFILE_DATABASE_ENTRY_HEADER_SIZE]{}; + if (!controller_identity_encode(entry.identity, entry_header, + CONTROLLER_IDENTITY_ENCODED_SIZE)) { + return false; + } + entry_header[14] = entry.active_profile; + entry_header[15] = 1; + copy_overlap(offset, output, size, entry_offset, entry_header, + sizeof(entry_header)); + for (uint8_t profile_index = 0; + profile_index < CONTROLLER_PROFILE_COUNT; ++profile_index) { + const size_t profile_offset = + entry_offset + CONTROLLER_PROFILE_DATABASE_ENTRY_HEADER_SIZE + + profile_index * CONTROLLER_PROFILE_ENCODED_SIZE; + if (offset < profile_offset + CONTROLLER_PROFILE_ENCODED_SIZE && + offset + size > profile_offset) { + if (!controller_profile_encode(entry.profiles[profile_index], + encoded_profile, + sizeof(encoded_profile))) { + return false; + } + copy_overlap(offset, output, size, profile_offset, + encoded_profile, sizeof(encoded_profile)); + } + } + } + return true; +} + +bool controller_profile_database_decode( + ControllerProfileDatabaseRead read, void* context, + ControllerProfileDatabase* output) { + if (read == nullptr || output == nullptr) { + return false; + } + uint8_t header[CONTROLLER_PROFILE_DATABASE_HEADER_SIZE]{}; + if (!read(context, 0, header, sizeof(header)) || + memcmp(header, kDatabaseMagic, sizeof(kDatabaseMagic)) != 0 || + profile_read_u16(&header[4]) != + CONTROLLER_PROFILE_DATABASE_SCHEMA_VERSION || + profile_read_u16(&header[6]) != CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE || + header[8] != CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY || + header[9] != CONTROLLER_PROFILE_COUNT || + header[10] >= CONTROLLER_PROFILE_COUNT || + header[11] > CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY || + !profile_bytes_are_zero(&header[12], sizeof(header) - 12)) { + return false; + } + + controller_profile_database_default(output); + output->fallback_active_profile = header[10]; + uint8_t encoded_profile[CONTROLLER_PROFILE_ENCODED_SIZE]{}; + for (uint8_t profile_index = 0; + profile_index < CONTROLLER_PROFILE_COUNT; ++profile_index) { + const size_t profile_offset = + kFallbackOffset + + profile_index * CONTROLLER_PROFILE_ENCODED_SIZE; + if (!read(context, profile_offset, encoded_profile, + sizeof(encoded_profile)) || + !controller_profile_decode( + encoded_profile, sizeof(encoded_profile), + &output->fallback_profiles[profile_index])) { + controller_profile_database_default(output); + return false; + } + } + + uint8_t decoded_used_count = 0; + for (uint8_t entry_index = 0; + entry_index < CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY; + ++entry_index) { + const size_t entry_offset = + kEntriesOffset + entry_index * CONTROLLER_PROFILE_DATABASE_ENTRY_SIZE; + uint8_t entry_header[CONTROLLER_PROFILE_DATABASE_ENTRY_HEADER_SIZE]{}; + if (!read(context, entry_offset, entry_header, + sizeof(entry_header))) { + controller_profile_database_default(output); + return false; + } + if (entry_header[15] == 0) { + if (!profile_bytes_are_zero(entry_header, sizeof(entry_header)) || + !read_zero_region( + read, context, + entry_offset + CONTROLLER_PROFILE_DATABASE_ENTRY_HEADER_SIZE, + CONTROLLER_PROFILE_COUNT * + CONTROLLER_PROFILE_ENCODED_SIZE)) { + controller_profile_database_default(output); + return false; + } + continue; + } + if (entry_header[15] != 1 || + entry_header[14] >= CONTROLLER_PROFILE_COUNT) { + controller_profile_database_default(output); + return false; + } + ControllerProfileDatabaseEntry& entry = output->entries[entry_index]; + if (!controller_identity_decode(entry_header, + CONTROLLER_IDENTITY_ENCODED_SIZE, + &entry.identity) || + !entry.identity.stable || + controller_identity_is_global(entry.identity)) { + controller_profile_database_default(output); + return false; + } + entry.used = true; + entry.active_profile = entry_header[14]; + ++decoded_used_count; + for (uint8_t profile_index = 0; + profile_index < CONTROLLER_PROFILE_COUNT; ++profile_index) { + const size_t profile_offset = + entry_offset + CONTROLLER_PROFILE_DATABASE_ENTRY_HEADER_SIZE + + profile_index * CONTROLLER_PROFILE_ENCODED_SIZE; + if (!read(context, profile_offset, encoded_profile, + sizeof(encoded_profile)) || + !controller_profile_decode(encoded_profile, + sizeof(encoded_profile), + &entry.profiles[profile_index])) { + controller_profile_database_default(output); + return false; + } + } + } + if (decoded_used_count != header[11] || + !controller_profile_database_validate(*output)) { + controller_profile_database_default(output); + return false; + } + return true; +} + +const ControllerProfileDatabaseEntry* controller_profile_database_find( + const ControllerProfileDatabase& database, + const ControllerIdentity& identity) { + if (!identity.stable || controller_identity_is_global(identity)) { + return nullptr; + } + for (const ControllerProfileDatabaseEntry& entry : database.entries) { + if (entry.used && + controller_identity_equal(entry.identity, identity)) { + return &entry; + } + } + return nullptr; +} + +ControllerProfileDatabaseEntry* controller_profile_database_find( + ControllerProfileDatabase* database, + const ControllerIdentity& identity) { + if (database == nullptr) { + return nullptr; + } + return const_cast( + controller_profile_database_find(*database, identity)); +} + +ControllerProfileDatabaseResult controller_profile_database_ensure( + ControllerProfileDatabase* database, const ControllerIdentity& identity, + ControllerProfileDatabaseEntry** output) { + if (database == nullptr || output == nullptr || !identity.stable || + controller_identity_is_global(identity)) { + return ControllerProfileDatabaseResult::kInvalidArgument; + } + if (ControllerProfileDatabaseEntry* found = + controller_profile_database_find(database, identity)) { + *output = found; + return ControllerProfileDatabaseResult::kOk; + } + for (ControllerProfileDatabaseEntry& entry : database->entries) { + if (!entry.used) { + entry.active_profile = 0; + entry.used = true; + entry.identity = identity; + for (uint8_t profile_index = 0; + profile_index < CONTROLLER_PROFILE_COUNT; ++profile_index) { + entry.profiles[profile_index] = + controller_profile_default(identity, profile_index); + } + *output = &entry; + return ControllerProfileDatabaseResult::kOk; + } + } + return ControllerProfileDatabaseResult::kFull; +} + +const ControllerProfile* controller_profile_database_get( + const ControllerProfileDatabase& database, + const ControllerIdentity& identity, uint8_t profile_index) { + if (profile_index >= CONTROLLER_PROFILE_COUNT) { + return nullptr; + } + if (controller_identity_is_global(identity)) { + return &database.fallback_profiles[profile_index]; + } + const ControllerProfileDatabaseEntry* entry = + controller_profile_database_find(database, identity); + return entry == nullptr ? nullptr : &entry->profiles[profile_index]; +} + +ControllerProfileDatabaseResult controller_profile_database_set( + ControllerProfileDatabase* database, const ControllerIdentity& identity, + uint8_t profile_index, const ControllerProfile& profile) { + if (database == nullptr || profile_index >= CONTROLLER_PROFILE_COUNT || + !controller_profile_validate(profile)) { + return ControllerProfileDatabaseResult::kInvalidArgument; + } + if (controller_identity_is_global(identity)) { + database->fallback_profiles[profile_index] = profile; + return ControllerProfileDatabaseResult::kOk; + } + ControllerProfileDatabaseEntry* entry = nullptr; + const ControllerProfileDatabaseResult result = + controller_profile_database_ensure(database, identity, &entry); + if (result == ControllerProfileDatabaseResult::kOk) { + entry->profiles[profile_index] = profile; + } + return result; +} + +ControllerProfileDatabaseResult controller_profile_database_reset( + ControllerProfileDatabase* database, const ControllerIdentity& identity, + uint8_t profile_index) { + if (database == nullptr || + (profile_index != CONTROLLER_PROFILE_ALL && + profile_index >= CONTROLLER_PROFILE_COUNT)) { + return ControllerProfileDatabaseResult::kInvalidArgument; + } + if (controller_identity_is_global(identity)) { + for (uint8_t index = 0; index < CONTROLLER_PROFILE_COUNT; ++index) { + if (profile_index == CONTROLLER_PROFILE_ALL || + profile_index == index) { + database->fallback_profiles[index] = + controller_profile_default(identity, index); + } + } + return ControllerProfileDatabaseResult::kOk; + } + ControllerProfileDatabaseEntry* entry = nullptr; + const ControllerProfileDatabaseResult result = + controller_profile_database_ensure(database, identity, &entry); + if (result != ControllerProfileDatabaseResult::kOk) { + return result; + } + for (uint8_t index = 0; index < CONTROLLER_PROFILE_COUNT; ++index) { + if (profile_index == CONTROLLER_PROFILE_ALL || + profile_index == index) { + entry->profiles[index] = controller_profile_default(identity, index); + } + } + return ControllerProfileDatabaseResult::kOk; +} + +ControllerProfileDatabaseResult controller_profile_database_activate( + ControllerProfileDatabase* database, const ControllerIdentity& identity, + uint8_t profile_index) { + if (database == nullptr || profile_index >= CONTROLLER_PROFILE_COUNT) { + return ControllerProfileDatabaseResult::kInvalidArgument; + } + if (controller_identity_is_global(identity)) { + database->fallback_active_profile = profile_index; + return ControllerProfileDatabaseResult::kOk; + } + ControllerProfileDatabaseEntry* entry = nullptr; + const ControllerProfileDatabaseResult result = + controller_profile_database_ensure(database, identity, &entry); + if (result == ControllerProfileDatabaseResult::kOk) { + entry->active_profile = profile_index; + } + return result; +} diff --git a/controller_profile.h b/controller_profile.h new file mode 100644 index 0000000..14b30db --- /dev/null +++ b/controller_profile.h @@ -0,0 +1,191 @@ +#pragma once + +#include +#include + +#include "controller_identity.h" + +constexpr uint16_t CONTROLLER_PROFILE_SCHEMA_VERSION = 1; +constexpr size_t CONTROLLER_PROFILE_ENCODED_SIZE = 256; +constexpr uint8_t CONTROLLER_PROFILE_COUNT = 4; +constexpr uint8_t CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT = 16; +constexpr uint8_t CONTROLLER_PROFILE_MACRO_STEP_CAPACITY = 8; +constexpr uint16_t CONTROLLER_PROFILE_MAX_WAIT_MS = 10000; +constexpr uint8_t CONTROLLER_PROFILE_NO_BUTTON = 0xff; +constexpr uint8_t CONTROLLER_PROFILE_ALL = 0xff; + +constexpr uint8_t CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY = 16; +constexpr uint16_t CONTROLLER_PROFILE_DATABASE_SCHEMA_VERSION = 1; +constexpr size_t CONTROLLER_PROFILE_DATABASE_HEADER_SIZE = 32; +constexpr size_t CONTROLLER_PROFILE_DATABASE_ENTRY_HEADER_SIZE = 16; +constexpr size_t CONTROLLER_PROFILE_DATABASE_ENTRY_SIZE = + CONTROLLER_PROFILE_DATABASE_ENTRY_HEADER_SIZE + + CONTROLLER_PROFILE_COUNT * CONTROLLER_PROFILE_ENCODED_SIZE; +constexpr size_t CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE = + CONTROLLER_PROFILE_DATABASE_HEADER_SIZE + + CONTROLLER_PROFILE_COUNT * CONTROLLER_PROFILE_ENCODED_SIZE + + CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY * + CONTROLLER_PROFILE_DATABASE_ENTRY_SIZE; + +static_assert(CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE == 17696, + "profile database wire size changed"); +enum class ControllerProfileLogicalButton : uint8_t { + kSouth = 0, + kEast = 1, + kWest = 2, + kNorth = 3, + kLeftShoulder = 4, + kRightShoulder = 5, + kSelect = 6, + kStart = 7, + kSystem = 8, + kCapture = 9, + kLeftStick = 10, + kRightStick = 11, + kDpadUp = 12, + kDpadDown = 13, + kDpadLeft = 14, + kDpadRight = 15, +}; +static_assert( + static_cast(ControllerProfileLogicalButton::kDpadRight) + 1 == + CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT); + + +enum class ControllerProfileConfirmationPolicy : uint8_t { + kNone = 0, + kRumble = 1, + kLed = 2, + kRumbleAndLed = 3, +}; + +enum class ControllerProfileTurboMode : uint8_t { + kOff = 0, + kTurbo = 1, + kAutoBurst = 2, +}; + +enum class ControllerProfileMacroStepType : uint8_t { + kState = 0, + kEnd = 1, +}; + +enum ControllerProfileMacroOverride : uint8_t { + kControllerProfileOverrideButtons = 1u << 0, + kControllerProfileOverrideLeftStick = 1u << 1, + kControllerProfileOverrideRightStick = 1u << 2, + kControllerProfileOverrideLeftTrigger = 1u << 3, + kControllerProfileOverrideRightTrigger = 1u << 4, +}; + +struct ControllerProfileStickConfiguration { + int16_t center_x = 0; + int16_t center_y = 0; + uint16_t inner_deadzone = 0; + uint16_t outer_saturation = 32767; + uint16_t curve_q8_8 = 256; + bool invert_x = false; + bool invert_y = false; +}; + +struct ControllerProfileTriggerConfiguration { + uint16_t lower_deadzone = 0; + uint16_t upper_saturation = UINT16_MAX; + uint16_t curve_q8_8 = 256; + uint16_t digital_threshold = 0x8000; +}; + +struct ControllerProfileMacroStep { + ControllerProfileMacroStepType type = + ControllerProfileMacroStepType::kEnd; + uint8_t override_flags = 0; + uint16_t duration_ms = 0; + uint16_t output_button_mask = 0; + int16_t left_stick_x = 0; + int16_t left_stick_y = 0; + int16_t right_stick_x = 0; + int16_t right_stick_y = 0; + uint16_t left_trigger = 0; + uint16_t right_trigger = 0; +}; + +struct ControllerProfile { + uint8_t button_map[CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT]{}; + ControllerProfileStickConfiguration sticks[2]{}; + ControllerProfileTriggerConfiguration triggers[2]{}; + uint8_t weak_rumble_scale = UINT8_MAX; + uint8_t strong_rumble_scale = UINT8_MAX; + ControllerProfileConfirmationPolicy confirmation_policy = + ControllerProfileConfirmationPolicy::kRumbleAndLed; + uint16_t switching_chord = 0; + uint8_t macro_trigger = CONTROLLER_PROFILE_NO_BUTTON; + uint8_t macro_cancel = CONTROLLER_PROFILE_NO_BUTTON; + uint8_t macro_step_count = 1; + ControllerProfileTurboMode + turbo_modes[CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT]{}; + ControllerProfileMacroStep + macro_steps[CONTROLLER_PROFILE_MACRO_STEP_CAPACITY]{}; +}; + +struct ControllerProfileDatabaseEntry { + bool used = false; + ControllerIdentity identity{}; + uint8_t active_profile = 0; + ControllerProfile profiles[CONTROLLER_PROFILE_COUNT]{}; +}; + +struct ControllerProfileDatabase { + uint8_t fallback_active_profile = 0; + ControllerProfile fallback_profiles[CONTROLLER_PROFILE_COUNT]{}; + ControllerProfileDatabaseEntry + entries[CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY]{}; +}; + +enum class ControllerProfileDatabaseResult : uint8_t { + kOk = 0, + kInvalidArgument = 1, + kFull = 2, +}; + +using ControllerProfileDatabaseRead = bool (*)( + void* context, size_t offset, uint8_t* output, size_t size); + +ControllerProfile controller_profile_default(const ControllerIdentity& identity, + uint8_t profile_index); +bool controller_profile_validate(const ControllerProfile& profile); +bool controller_profile_encode(const ControllerProfile& profile, + uint8_t* output, size_t output_size); +bool controller_profile_decode(const uint8_t* input, size_t input_size, + ControllerProfile* output); + +void controller_profile_database_default(ControllerProfileDatabase* database); +bool controller_profile_database_validate( + const ControllerProfileDatabase& database); +bool controller_profile_database_encode_range( + const ControllerProfileDatabase& database, size_t offset, + uint8_t* output, size_t size); +bool controller_profile_database_decode( + ControllerProfileDatabaseRead read, void* context, + ControllerProfileDatabase* output); + +const ControllerProfileDatabaseEntry* controller_profile_database_find( + const ControllerProfileDatabase& database, + const ControllerIdentity& identity); +ControllerProfileDatabaseEntry* controller_profile_database_find( + ControllerProfileDatabase* database, + const ControllerIdentity& identity); +ControllerProfileDatabaseResult controller_profile_database_ensure( + ControllerProfileDatabase* database, const ControllerIdentity& identity, + ControllerProfileDatabaseEntry** output); +const ControllerProfile* controller_profile_database_get( + const ControllerProfileDatabase& database, + const ControllerIdentity& identity, uint8_t profile_index); +ControllerProfileDatabaseResult controller_profile_database_set( + ControllerProfileDatabase* database, const ControllerIdentity& identity, + uint8_t profile_index, const ControllerProfile& profile); +ControllerProfileDatabaseResult controller_profile_database_reset( + ControllerProfileDatabase* database, const ControllerIdentity& identity, + uint8_t profile_index); +ControllerProfileDatabaseResult controller_profile_database_activate( + ControllerProfileDatabase* database, const ControllerIdentity& identity, + uint8_t profile_index); diff --git a/pico_profile_storage.cpp b/pico_profile_storage.cpp new file mode 100644 index 0000000..03bf425 --- /dev/null +++ b/pico_profile_storage.cpp @@ -0,0 +1,126 @@ +#include "pico_profile_storage.h" + +#include + +#include "configuration_storage.h" +#include "hardware/flash.h" +#include "pico/btstack_flash_bank.h" +#include "pico/flash.h" +#include "pico/platform.h" + +extern "C" char __flash_binary_end; + +namespace { + +constexpr size_t kConfigurationStorageSize = + CONFIGURATION_STORAGE_COPY_COUNT * FLASH_SECTOR_SIZE; +constexpr uint32_t kConfigurationStorageOffset = + PICO_FLASH_BANK_STORAGE_OFFSET - kConfigurationStorageSize; +constexpr uint32_t kProfileStorageOffset = + kConfigurationStorageOffset - PROFILE_STORAGE_TOTAL_SIZE; + +static_assert(FLASH_SECTOR_SIZE == PROFILE_STORAGE_SECTOR_SIZE, + "profile storage sector size does not match Pico flash"); +static_assert(FLASH_PAGE_SIZE == PROFILE_STORAGE_PAGE_SIZE, + "profile storage page size does not match Pico flash"); +static_assert(PICO_FLASH_BANK_STORAGE_OFFSET >= + kConfigurationStorageSize + PROFILE_STORAGE_TOTAL_SIZE, + "profile storage offset underflows flash"); +static_assert(kProfileStorageOffset + PROFILE_STORAGE_TOTAL_SIZE <= + kConfigurationStorageOffset, + "profile storage overlaps adapter configuration storage"); +static_assert(kConfigurationStorageOffset + kConfigurationStorageSize <= + PICO_FLASH_BANK_STORAGE_OFFSET, + "adapter configuration storage overlaps BTstack bonds"); +static_assert(PICO_FLASH_BANK_STORAGE_OFFSET + + PICO_FLASH_BANK_TOTAL_SIZE <= + PICO_FLASH_SIZE_BYTES, + "BTstack storage exceeds flash"); + +struct FlashMutation { + bool erase; + uint32_t offset; + const uint8_t* data; +}; + +void perform_flash_mutation(void* context) { + const auto* mutation = static_cast(context); + if (mutation->erase) { + flash_range_erase(mutation->offset, FLASH_SECTOR_SIZE); + } else { + flash_range_program(mutation->offset, mutation->data, + FLASH_PAGE_SIZE); + } +} + +bool storage_region_available() { + const uintptr_t binary_end = + reinterpret_cast(&__flash_binary_end) - XIP_BASE; + return binary_end <= kProfileStorageOffset; +} + +bool read_storage(void*, uint8_t bank, size_t offset, uint8_t* output, + size_t size) { + if (bank >= PROFILE_STORAGE_BANK_COUNT || output == nullptr || + offset > PROFILE_STORAGE_BANK_SIZE || + size > PROFILE_STORAGE_BANK_SIZE - offset || + !storage_region_available()) { + return false; + } + const uintptr_t address = + XIP_BASE + kProfileStorageOffset + + bank * PROFILE_STORAGE_BANK_SIZE + offset; + memcpy(output, reinterpret_cast(address), size); + return true; +} + +bool erase_storage_sector(void*, uint8_t bank, size_t offset) { + if (bank >= PROFILE_STORAGE_BANK_COUNT || + offset % FLASH_SECTOR_SIZE != 0 || + offset > PROFILE_STORAGE_BANK_SIZE || + FLASH_SECTOR_SIZE > PROFILE_STORAGE_BANK_SIZE - offset || + !storage_region_available()) { + return false; + } + FlashMutation mutation{ + true, + static_cast( + kProfileStorageOffset + bank * PROFILE_STORAGE_BANK_SIZE + offset), + nullptr, + }; + return flash_safe_execute(perform_flash_mutation, &mutation, + UINT32_MAX) == PICO_OK; +} + +bool program_storage(void*, uint8_t bank, size_t offset, + const uint8_t* data, size_t size) { + if (bank >= PROFILE_STORAGE_BANK_COUNT || data == nullptr || + size != FLASH_PAGE_SIZE || offset % FLASH_PAGE_SIZE != 0 || + offset > PROFILE_STORAGE_BANK_SIZE || + size > PROFILE_STORAGE_BANK_SIZE - offset || + !storage_region_available()) { + return false; + } + FlashMutation mutation{ + false, + static_cast( + kProfileStorageOffset + bank * PROFILE_STORAGE_BANK_SIZE + offset), + data, + }; + return flash_safe_execute(perform_flash_mutation, &mutation, + UINT32_MAX) == PICO_OK; +} + +} // namespace + +ProfileStorageIo pico_profile_storage_io() { + return { + nullptr, + PROFILE_STORAGE_BANK_SIZE, + FLASH_SECTOR_SIZE, + FLASH_PAGE_SIZE, + read_storage, + erase_storage_sector, + program_storage, + }; +} diff --git a/pico_profile_storage.h b/pico_profile_storage.h new file mode 100644 index 0000000..3fd8b42 --- /dev/null +++ b/pico_profile_storage.h @@ -0,0 +1,5 @@ +#pragma once + +#include "profile_storage.h" + +ProfileStorageIo pico_profile_storage_io(); diff --git a/profile_service.cpp b/profile_service.cpp new file mode 100644 index 0000000..f646b38 --- /dev/null +++ b/profile_service.cpp @@ -0,0 +1,548 @@ +#include "profile_service.h" + +#include + +#include "pico/critical_section.h" +#include "pico_profile_storage.h" +#include "profile_storage.h" + +namespace { + +constexpr uint32_t kMinimumCommitIntervalMs = 1000; + +enum class PendingCommandType : uint8_t { + kNone = 0, + kReset = 1, + kActivate = 2, +}; + +struct PendingCommand { + PendingCommandType type = PendingCommandType::kNone; + ControllerIdentity identity{}; + uint8_t profile_index = 0; +}; + +struct ProfileTransaction { + ControllerIdentity identity{}; + uint8_t profile_index = 0; + ConfigurationTransactionSnapshot snapshot{}; + uint8_t payload[CONTROLLER_PROFILE_ENCODED_SIZE]{}; +}; + +critical_section_t g_lock; +bool g_prepared = false; +ProfileStorage g_storage; +ControllerProfileDatabase g_database; +ProfileServiceMetadata g_metadata; +ProfileServiceListSnapshot g_list; +ProfileServiceSelectedSnapshot g_selected; +ProfileTransaction g_transaction; +PendingCommand g_command; +bool g_identity_dirty = false; +bool g_has_committed = false; +uint32_t g_last_commit_ms = 0; + +bool valid_identity(const ControllerIdentity& identity) { + uint8_t encoded[CONTROLLER_IDENTITY_ENCODED_SIZE]{}; + return (controller_identity_is_global(identity) || identity.stable) && + controller_identity_encode(identity, encoded, sizeof(encoded)); +} + +void refresh_list_locked() { + g_list = {}; + g_list.metadata = g_metadata; + g_list.count = 1; + g_list.rows[0].identity = controller_identity_global(); + g_list.rows[0].active_profile = + g_database.fallback_active_profile; + for (const ControllerProfileDatabaseEntry& entry : g_database.entries) { + if (!entry.used || g_list.count >= PROFILE_SERVICE_LIST_CAPACITY) { + continue; + } + ProfileServiceListRow& row = g_list.rows[g_list.count++]; + row.identity = entry.identity; + row.active_profile = entry.active_profile; + } +} + +void refresh_selected_locked() { + g_selected.metadata = g_metadata; + const ControllerProfile* profile = controller_profile_database_get( + g_database, g_selected.identity, g_selected.profile_index); + if (profile == nullptr) { + g_selected.valid = false; + g_selected.status = ConfigurationTransactionStatus::kMalformed; + return; + } + g_selected.profile = *profile; + g_selected.valid = true; + g_selected.status = ConfigurationTransactionStatus::kCommitted; +} + +void refresh_metadata_locked(ProfileServiceState state) { + const ProfileStorageSnapshot& stored = g_storage.snapshot(); + g_metadata.state = state; + g_metadata.generation = stored.valid ? stored.generation : 0; + g_metadata.payload_crc = stored.valid ? stored.payload_crc : 0; + refresh_list_locked(); + refresh_selected_locked(); +} + +ConfigurationTransactionStatus database_result_status( + ControllerProfileDatabaseResult result) { + switch (result) { + case ControllerProfileDatabaseResult::kOk: + return ConfigurationTransactionStatus::kPending; + case ControllerProfileDatabaseResult::kFull: + return ConfigurationTransactionStatus::kTooLarge; + case ControllerProfileDatabaseResult::kInvalidArgument: + return ConfigurationTransactionStatus::kMalformed; + } + return ConfigurationTransactionStatus::kStorageError; +} + +bool mutation_ready(uint32_t now_ms) { + return !g_has_committed || + static_cast(now_ms - g_last_commit_ms) >= + kMinimumCommitIntervalMs; +} + +void finish_mutation(ConfigurationTransactionStatus status, + bool clear_command) { + const ProfileStorageSnapshot& stored = g_storage.snapshot(); + critical_section_enter_blocking(&g_lock); + g_transaction.snapshot.status = status; + g_transaction.snapshot.stored_generation = + stored.valid ? stored.generation : 0; + g_transaction.snapshot.stored_crc = + stored.valid ? stored.payload_crc : 0; + if (clear_command) { + g_command = {}; + } + refresh_metadata_locked( + status == ConfigurationTransactionStatus::kStorageError + ? ProfileServiceState::kStorageError + : ProfileServiceState::kReady); + critical_section_exit(&g_lock); +} + +} // namespace + +void profile_service_prepare() { + if (g_prepared) { + return; + } + critical_section_init(&g_lock); + g_metadata = {}; + g_list = {}; + g_selected = {}; + g_selected.identity = controller_identity_global(); + g_selected.profile_index = 0; + g_transaction = {}; + g_command = {}; + g_identity_dirty = false; + g_has_committed = false; + g_last_commit_ms = 0; + g_prepared = true; +} + +void profile_service_initialize_on_storage_core() { + if (!g_prepared) { + profile_service_prepare(); + } + const bool initialized = + g_storage.initialize(pico_profile_storage_io(), &g_database); + critical_section_enter_blocking(&g_lock); + refresh_metadata_locked(initialized ? ProfileServiceState::kReady + : ProfileServiceState::kStorageError); + critical_section_exit(&g_lock); +} + +bool profile_service_observe_identity_on_storage_core( + const ControllerIdentity& identity) { + if (!g_prepared || !identity.stable || + controller_identity_is_global(identity) || + !valid_identity(identity)) { + return false; + } + critical_section_enter_blocking(&g_lock); + if (g_metadata.state != ProfileServiceState::kReady) { + critical_section_exit(&g_lock); + return false; + } + ControllerProfileDatabaseEntry* entry = + controller_profile_database_find(&g_database, identity); + if (entry != nullptr) { + critical_section_exit(&g_lock); + return true; + } + const ControllerProfileDatabaseResult result = + controller_profile_database_ensure(&g_database, identity, &entry); + if (result == ControllerProfileDatabaseResult::kOk) { + g_identity_dirty = true; + refresh_list_locked(); + } + critical_section_exit(&g_lock); + return result == ControllerProfileDatabaseResult::kOk; +} + + +void profile_service_task_on_storage_core(uint32_t now_ms) { + PendingCommand command{}; + bool process_write = false; + bool process_identity = false; + ControllerIdentity write_identity{}; + uint8_t write_profile_index = 0; + uint8_t write_payload[CONTROLLER_PROFILE_ENCODED_SIZE]{}; + + critical_section_enter_blocking(&g_lock); + if (mutation_ready(now_ms)) { + if (g_command.type != PendingCommandType::kNone) { + command = g_command; + } else if (g_transaction.snapshot.status == + ConfigurationTransactionStatus::kPending) { + process_write = true; + write_identity = g_transaction.identity; + write_profile_index = g_transaction.profile_index; + memcpy(write_payload, g_transaction.payload, + sizeof(write_payload)); + } else if (g_identity_dirty) { + process_identity = true; + } + } + critical_section_exit(&g_lock); + + if (!process_write && !process_identity && + command.type == PendingCommandType::kNone) { + return; + } + + ControllerProfileDatabaseResult database_result = + ControllerProfileDatabaseResult::kInvalidArgument; + if (process_identity) { + database_result = ControllerProfileDatabaseResult::kOk; + } else if (process_write) { + ControllerProfile profile{}; + if (controller_profile_decode(write_payload, sizeof(write_payload), + &profile)) { + critical_section_enter_blocking(&g_lock); + database_result = controller_profile_database_set( + &g_database, write_identity, write_profile_index, profile); + critical_section_exit(&g_lock); + } + } else if (command.type == PendingCommandType::kReset) { + critical_section_enter_blocking(&g_lock); + database_result = controller_profile_database_reset( + &g_database, command.identity, command.profile_index); + critical_section_exit(&g_lock); + } else if (command.type == PendingCommandType::kActivate) { + critical_section_enter_blocking(&g_lock); + database_result = controller_profile_database_activate( + &g_database, command.identity, command.profile_index); + critical_section_exit(&g_lock); + } + + if (database_result != ControllerProfileDatabaseResult::kOk) { + finish_mutation(database_result_status(database_result), + !process_write); + return; + } + + const ProfileStorageResult storage_result = g_storage.commit(g_database); + ConfigurationTransactionStatus status = + ConfigurationTransactionStatus::kStorageError; + if (storage_result == ProfileStorageResult::kOk) { + status = ConfigurationTransactionStatus::kCommitted; + g_has_committed = true; + g_last_commit_ms = now_ms; + } else if (storage_result == ProfileStorageResult::kUnchanged) { + status = ConfigurationTransactionStatus::kUnchanged; + } else { + critical_section_enter_blocking(&g_lock); + const bool restored = + g_storage.initialize(pico_profile_storage_io(), &g_database); + critical_section_exit(&g_lock); + if (!restored) { + status = ConfigurationTransactionStatus::kStorageError; + } + } + if (process_identity) { + critical_section_enter_blocking(&g_lock); + g_identity_dirty = false; + refresh_metadata_locked( + status == ConfigurationTransactionStatus::kStorageError + ? ProfileServiceState::kStorageError + : ProfileServiceState::kReady); + critical_section_exit(&g_lock); + return; + } + critical_section_enter_blocking(&g_lock); + g_identity_dirty = false; + critical_section_exit(&g_lock); + finish_mutation(status, !process_write); +} + +ConfigurationTransactionStatus profile_service_select( + const ControllerIdentity& identity, uint8_t profile_index) { + if (!g_prepared) { + profile_service_prepare(); + } + critical_section_enter_blocking(&g_lock); + ConfigurationTransactionStatus status = + ConfigurationTransactionStatus::kCommitted; + if (!valid_identity(identity) || + profile_index >= CONTROLLER_PROFILE_COUNT) { + status = ConfigurationTransactionStatus::kMalformed; + g_selected.metadata = g_metadata; + g_selected.identity = identity; + g_selected.profile_index = profile_index; + g_selected.valid = false; + g_selected.status = status; + } else if (g_metadata.state != ProfileServiceState::kReady) { + status = g_metadata.state == ProfileServiceState::kLoading + ? ConfigurationTransactionStatus::kPending + : ConfigurationTransactionStatus::kStorageError; + g_selected.metadata = g_metadata; + g_selected.identity = identity; + g_selected.profile_index = profile_index; + g_selected.valid = false; + g_selected.status = status; + } else { + g_selected.identity = identity; + g_selected.profile_index = profile_index; + refresh_selected_locked(); + status = g_selected.status; + } + critical_section_exit(&g_lock); + return status; +} + +ConfigurationTransactionStatus profile_service_begin( + uint32_t transaction_id, const ControllerIdentity& identity, + uint8_t profile_index, uint16_t schema_version, size_t payload_size, + uint32_t payload_crc) { + if (!g_prepared) { + profile_service_prepare(); + } + critical_section_enter_blocking(&g_lock); + if (g_command.type != PendingCommandType::kNone || + g_transaction.snapshot.status == + ConfigurationTransactionStatus::kReceiving || + g_transaction.snapshot.status == + ConfigurationTransactionStatus::kPending) { + critical_section_exit(&g_lock); + return ConfigurationTransactionStatus::kBusy; + } + g_transaction = {}; + g_transaction.snapshot.transaction_id = transaction_id; + g_transaction.identity = identity; + g_transaction.profile_index = profile_index; + if (transaction_id == 0 || !valid_identity(identity) || + profile_index >= CONTROLLER_PROFILE_COUNT || payload_size == 0) { + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kMalformed; + } else if (schema_version != CONTROLLER_PROFILE_SCHEMA_VERSION) { + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kUnsupportedSchema; + } else if (payload_size > CONTROLLER_PROFILE_ENCODED_SIZE) { + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kTooLarge; + } else if (payload_size != CONTROLLER_PROFILE_ENCODED_SIZE) { + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kMalformed; + } else { + g_transaction.snapshot.expected_size = + static_cast(payload_size); + g_transaction.snapshot.expected_crc = payload_crc; + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kReceiving; + } + const ConfigurationTransactionStatus status = + g_transaction.snapshot.status; + critical_section_exit(&g_lock); + return status; +} + +ConfigurationTransactionStatus profile_service_append( + uint32_t transaction_id, size_t offset, const uint8_t* data, + size_t size) { + critical_section_enter_blocking(&g_lock); + if (g_transaction.snapshot.status != + ConfigurationTransactionStatus::kReceiving) { + critical_section_exit(&g_lock); + return ConfigurationTransactionStatus::kBusy; + } + if (transaction_id != g_transaction.snapshot.transaction_id || + data == nullptr || size == 0 || + offset != g_transaction.snapshot.received_size || + offset > g_transaction.snapshot.expected_size || + size > g_transaction.snapshot.expected_size - offset) { + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kOutOfOrder; + } else { + memcpy(&g_transaction.payload[offset], data, size); + g_transaction.snapshot.received_size = + static_cast(offset + size); + } + const ConfigurationTransactionStatus status = + g_transaction.snapshot.status; + critical_section_exit(&g_lock); + return status; +} + +ConfigurationTransactionStatus profile_service_commit( + uint32_t transaction_id) { + critical_section_enter_blocking(&g_lock); + if (g_transaction.snapshot.status != + ConfigurationTransactionStatus::kReceiving || + transaction_id != g_transaction.snapshot.transaction_id || + g_transaction.snapshot.received_size != + g_transaction.snapshot.expected_size) { + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kOutOfOrder; + } else if (profile_storage_crc32( + g_transaction.payload, + g_transaction.snapshot.expected_size) != + g_transaction.snapshot.expected_crc) { + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kBadCrc; + } else { + ControllerProfile profile{}; + g_transaction.snapshot.status = + controller_profile_decode( + g_transaction.payload, + g_transaction.snapshot.expected_size, &profile) + ? ConfigurationTransactionStatus::kPending + : ConfigurationTransactionStatus::kMalformed; + } + const ConfigurationTransactionStatus status = + g_transaction.snapshot.status; + critical_section_exit(&g_lock); + return status; +} + +ConfigurationTransactionStatus profile_service_reset( + uint32_t transaction_id, const ControllerIdentity& identity, + uint8_t profile_index) { + if (!g_prepared) { + profile_service_prepare(); + } + critical_section_enter_blocking(&g_lock); + if (g_command.type != PendingCommandType::kNone || + g_transaction.snapshot.status == + ConfigurationTransactionStatus::kReceiving || + g_transaction.snapshot.status == + ConfigurationTransactionStatus::kPending) { + critical_section_exit(&g_lock); + return ConfigurationTransactionStatus::kBusy; + } + g_transaction = {}; + g_transaction.snapshot.transaction_id = transaction_id; + g_transaction.identity = identity; + g_transaction.profile_index = profile_index; + if (transaction_id == 0 || !valid_identity(identity) || + (profile_index != CONTROLLER_PROFILE_ALL && + profile_index >= CONTROLLER_PROFILE_COUNT)) { + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kMalformed; + critical_section_exit(&g_lock); + return ConfigurationTransactionStatus::kMalformed; + } + g_transaction.snapshot.status = ConfigurationTransactionStatus::kPending; + g_command.type = PendingCommandType::kReset; + g_command.identity = identity; + g_command.profile_index = profile_index; + critical_section_exit(&g_lock); + return ConfigurationTransactionStatus::kPending; +} + +ConfigurationTransactionStatus profile_service_activate( + uint32_t transaction_id, const ControllerIdentity& identity, + uint8_t profile_index) { + if (!g_prepared) { + profile_service_prepare(); + } + critical_section_enter_blocking(&g_lock); + if (g_command.type != PendingCommandType::kNone || + g_transaction.snapshot.status == + ConfigurationTransactionStatus::kReceiving || + g_transaction.snapshot.status == + ConfigurationTransactionStatus::kPending) { + critical_section_exit(&g_lock); + return ConfigurationTransactionStatus::kBusy; + } + g_transaction = {}; + g_transaction.snapshot.transaction_id = transaction_id; + g_transaction.identity = identity; + g_transaction.profile_index = profile_index; + if (transaction_id == 0 || !valid_identity(identity) || + profile_index >= CONTROLLER_PROFILE_COUNT) { + g_transaction.snapshot.status = + ConfigurationTransactionStatus::kMalformed; + critical_section_exit(&g_lock); + return ConfigurationTransactionStatus::kMalformed; + } + g_transaction.snapshot.status = ConfigurationTransactionStatus::kPending; + g_command.type = PendingCommandType::kActivate; + g_command.identity = identity; + g_command.profile_index = profile_index; + critical_section_exit(&g_lock); + return ConfigurationTransactionStatus::kPending; +} + +void profile_service_list_snapshot(ProfileServiceListSnapshot* output) { + if (output == nullptr) { + return; + } + critical_section_enter_blocking(&g_lock); + *output = g_list; + critical_section_exit(&g_lock); +} + +void profile_service_selected_snapshot( + ProfileServiceSelectedSnapshot* output) { + if (output == nullptr) { + return; + } + critical_section_enter_blocking(&g_lock); + *output = g_selected; + critical_section_exit(&g_lock); +} + +void profile_service_transaction_snapshot( + ProfileServiceTransactionSnapshot* output) { + if (output == nullptr) { + return; + } + critical_section_enter_blocking(&g_lock); + output->metadata = g_metadata; + output->identity = g_transaction.identity; + output->profile_index = g_transaction.profile_index; + output->transaction = g_transaction.snapshot; + critical_section_exit(&g_lock); +} + +bool profile_service_active_profile(const ControllerIdentity& identity, + ControllerProfile* output, + uint8_t* profile_index) { + if (output == nullptr || profile_index == nullptr || + !valid_identity(identity)) { + return false; + } + critical_section_enter_blocking(&g_lock); + if (g_metadata.state != ProfileServiceState::kReady) { + critical_section_exit(&g_lock); + return false; + } + const ControllerProfileDatabaseEntry* entry = + controller_profile_database_find(g_database, identity); + if (entry != nullptr) { + *profile_index = entry->active_profile; + *output = entry->profiles[entry->active_profile]; + } else { + *profile_index = g_database.fallback_active_profile; + *output = g_database.fallback_profiles[*profile_index]; + } + critical_section_exit(&g_lock); + return true; +} diff --git a/profile_service.h b/profile_service.h new file mode 100644 index 0000000..b96b04a --- /dev/null +++ b/profile_service.h @@ -0,0 +1,83 @@ +#pragma once + +#include +#include + +#include "configuration_transaction.h" +#include "controller_profile.h" + +constexpr uint8_t PROFILE_SERVICE_LIST_CAPACITY = + CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY + 1; + +enum class ProfileServiceState : uint8_t { + kLoading = 0, + kReady = 1, + kStorageError = 2, +}; + +struct ProfileServiceMetadata { + ProfileServiceState state = ProfileServiceState::kLoading; + uint32_t generation = 0; + uint32_t payload_crc = 0; +}; + +struct ProfileServiceListRow { + ControllerIdentity identity{}; + uint8_t active_profile = 0; +}; + +struct ProfileServiceListSnapshot { + ProfileServiceMetadata metadata{}; + uint8_t count = 0; + ProfileServiceListRow rows[PROFILE_SERVICE_LIST_CAPACITY]{}; +}; + +struct ProfileServiceSelectedSnapshot { + ProfileServiceMetadata metadata{}; + bool valid = false; + ConfigurationTransactionStatus status = + ConfigurationTransactionStatus::kIdle; + ControllerIdentity identity{}; + uint8_t profile_index = 0; + ControllerProfile profile{}; +}; + +struct ProfileServiceTransactionSnapshot { + ProfileServiceMetadata metadata{}; + ControllerIdentity identity{}; + uint8_t profile_index = 0; + ConfigurationTransactionSnapshot transaction{}; +}; + +void profile_service_prepare(); +void profile_service_initialize_on_storage_core(); +void profile_service_task_on_storage_core(uint32_t now_ms); +bool profile_service_observe_identity_on_storage_core( + const ControllerIdentity& identity); + +ConfigurationTransactionStatus profile_service_select( + const ControllerIdentity& identity, uint8_t profile_index); +ConfigurationTransactionStatus profile_service_begin( + uint32_t transaction_id, const ControllerIdentity& identity, + uint8_t profile_index, uint16_t schema_version, size_t payload_size, + uint32_t payload_crc); +ConfigurationTransactionStatus profile_service_append( + uint32_t transaction_id, size_t offset, const uint8_t* data, + size_t size); +ConfigurationTransactionStatus profile_service_commit( + uint32_t transaction_id); +ConfigurationTransactionStatus profile_service_reset( + uint32_t transaction_id, const ControllerIdentity& identity, + uint8_t profile_index); +ConfigurationTransactionStatus profile_service_activate( + uint32_t transaction_id, const ControllerIdentity& identity, + uint8_t profile_index); + +void profile_service_list_snapshot(ProfileServiceListSnapshot* output); +void profile_service_selected_snapshot( + ProfileServiceSelectedSnapshot* output); +void profile_service_transaction_snapshot( + ProfileServiceTransactionSnapshot* output); +bool profile_service_active_profile(const ControllerIdentity& identity, + ControllerProfile* output, + uint8_t* profile_index); diff --git a/profile_storage.cpp b/profile_storage.cpp new file mode 100644 index 0000000..14af5ab --- /dev/null +++ b/profile_storage.cpp @@ -0,0 +1,301 @@ +#include "profile_storage.h" + +#include + +namespace { + +constexpr uint8_t kRecordMagic[4] = {'S', 'P', 'P', 'F'}; +constexpr uint16_t kRecordFormatVersion = 1; +constexpr size_t kHeaderFieldsSize = 24; +constexpr size_t kHeaderCrcOffset = 20; + +uint16_t storage_read_u16(const uint8_t* input) { + return static_cast(input[0]) | + (static_cast(input[1]) << 8); +} + +uint32_t storage_read_u32(const uint8_t* input) { + return static_cast(input[0]) | + (static_cast(input[1]) << 8) | + (static_cast(input[2]) << 16) | + (static_cast(input[3]) << 24); +} + +void storage_write_u16(uint8_t* output, uint16_t value) { + output[0] = static_cast(value); + output[1] = static_cast(value >> 8); +} + +void storage_write_u32(uint8_t* output, uint32_t value) { + output[0] = static_cast(value); + output[1] = static_cast(value >> 8); + output[2] = static_cast(value >> 16); + output[3] = static_cast(value >> 24); +} + +uint32_t crc32_update(uint32_t crc, const uint8_t* data, size_t size) { + for (size_t index = 0; index < size; ++index) { + crc ^= data[index]; + for (uint8_t bit = 0; bit < 8; ++bit) { + crc = (crc >> 1) ^ + (0xedb88320u & + static_cast( + -static_cast(crc & 1u))); + } + } + return crc; +} + +bool generation_is_newer(uint32_t candidate, uint32_t current) { + return static_cast(candidate - current) > 0; +} + +struct DatabaseReadContext { + const ProfileStorageIo* io; + uint8_t bank; +}; + +bool read_database(void* context, size_t offset, uint8_t* output, + size_t size) { + const auto* read_context = + static_cast(context); + return read_context->io->read( + read_context->io->context, read_context->bank, + PROFILE_STORAGE_RECORD_HEADER_SIZE + offset, output, size); +} + +} // namespace + +uint32_t profile_storage_crc32(const uint8_t* data, size_t size) { + if (data == nullptr && size != 0) { + return 0; + } + return ~crc32_update(0xffffffffu, data, size); +} + +bool ProfileStorage::initialize(const ProfileStorageIo& io, + ControllerProfileDatabase* database) { + io_ = io; + snapshot_ = {}; + initialized_ = database != nullptr && io_.read != nullptr && + io_.erase_sector != nullptr && io_.program != nullptr && + io_.bank_size >= PROFILE_STORAGE_BANK_SIZE && + io_.sector_size == PROFILE_STORAGE_SECTOR_SIZE && + io_.page_size == PROFILE_STORAGE_PAGE_SIZE && + io_.bank_size % io_.sector_size == 0 && + io_.sector_size % io_.page_size == 0; + if (!initialized_) { + return false; + } + + controller_profile_database_default(database); + BankHeader headers[PROFILE_STORAGE_BANK_COUNT]{}; + bool valid[PROFILE_STORAGE_BANK_COUNT]{}; + for (uint8_t bank = 0; bank < PROFILE_STORAGE_BANK_COUNT; ++bank) { + valid[bank] = read_header(bank, &headers[bank]) && + validate_payload(bank, headers[bank].payload_crc); + } + + uint8_t first = 0; + uint8_t second = 1; + if (valid[1] && + (!valid[0] || generation_is_newer(headers[1].generation, + headers[0].generation))) { + first = 1; + second = 0; + } + const uint8_t order[PROFILE_STORAGE_BANK_COUNT] = {first, second}; + for (uint8_t candidate : order) { + if (!valid[candidate] || !decode_bank(candidate, database)) { + continue; + } + snapshot_.valid = true; + snapshot_.generation = headers[candidate].generation; + snapshot_.payload_crc = headers[candidate].payload_crc; + snapshot_.active_bank = candidate; + return true; + } + controller_profile_database_default(database); + return true; +} + +ProfileStorageResult ProfileStorage::commit( + const ControllerProfileDatabase& database) { + if (!initialized_ || !controller_profile_database_validate(database)) { + return ProfileStorageResult::kInvalidArgument; + } + if (snapshot_.valid && payload_matches(snapshot_.active_bank, database)) { + return ProfileStorageResult::kUnchanged; + } + + uint8_t page[PROFILE_STORAGE_PAGE_SIZE]{}; + uint32_t crc = 0xffffffffu; + for (size_t offset = 0; + offset < CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE; + offset += sizeof(page)) { + const size_t size = + CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset < sizeof(page) + ? CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset + : sizeof(page); + if (!controller_profile_database_encode_range(database, offset, + page, size)) { + return ProfileStorageResult::kInvalidArgument; + } + crc = crc32_update(crc, page, size); + } + crc = ~crc; + + const uint8_t target_bank = snapshot_.valid + ? snapshot_.active_bank ^ 1u + : 0; + for (size_t offset = 0; offset < PROFILE_STORAGE_BANK_SIZE; + offset += PROFILE_STORAGE_SECTOR_SIZE) { + if (!io_.erase_sector(io_.context, target_bank, offset)) { + return ProfileStorageResult::kIoError; + } + } + + for (size_t offset = 0; + offset < CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE; + offset += sizeof(page)) { + memset(page, 0, sizeof(page)); + const size_t size = + CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset < sizeof(page) + ? CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset + : sizeof(page); + if (!controller_profile_database_encode_range(database, offset, + page, size) || + !io_.program(io_.context, target_bank, + PROFILE_STORAGE_RECORD_HEADER_SIZE + offset, + page, sizeof(page))) { + return ProfileStorageResult::kIoError; + } + } + + uint8_t stored_page[PROFILE_STORAGE_PAGE_SIZE]{}; + uint32_t stored_crc = 0xffffffffu; + bool stored_payload_matches = true; + for (size_t offset = 0; + offset < CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE; + offset += sizeof(stored_page)) { + const size_t size = + CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset < + sizeof(stored_page) + ? CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset + : sizeof(stored_page); + if (!io_.read(io_.context, target_bank, + PROFILE_STORAGE_RECORD_HEADER_SIZE + offset, + stored_page, size)) { + return ProfileStorageResult::kIoError; + } + if (!controller_profile_database_encode_range(database, offset, + page, size)) { + return ProfileStorageResult::kInvalidArgument; + } + for (size_t index = 0; index < size; ++index) { + stored_payload_matches &= + stored_page[index] == page[index]; + } + stored_crc = crc32_update(stored_crc, stored_page, size); + } + if (!stored_payload_matches || ~stored_crc != crc) { + return ProfileStorageResult::kIoError; + } + + memset(page, 0, sizeof(page)); + memcpy(page, kRecordMagic, sizeof(kRecordMagic)); + storage_write_u16(&page[4], kRecordFormatVersion); + storage_write_u16(&page[6], + CONTROLLER_PROFILE_DATABASE_SCHEMA_VERSION); + const uint32_t generation = + snapshot_.valid ? snapshot_.generation + 1u : 1u; + storage_write_u32(&page[8], generation); + storage_write_u32(&page[12], + CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE); + storage_write_u32(&page[16], crc); + storage_write_u32(&page[kHeaderCrcOffset], + profile_storage_crc32(page, kHeaderCrcOffset)); + if (!io_.program(io_.context, target_bank, 0, page, sizeof(page))) { + return ProfileStorageResult::kIoError; + } + + snapshot_.valid = true; + snapshot_.generation = generation; + snapshot_.payload_crc = crc; + snapshot_.active_bank = target_bank; + return ProfileStorageResult::kOk; +} + +const ProfileStorageSnapshot& ProfileStorage::snapshot() const { + return snapshot_; +} + +bool ProfileStorage::read_header(uint8_t bank, BankHeader* output) const { + uint8_t header[kHeaderFieldsSize]{}; + if (bank >= PROFILE_STORAGE_BANK_COUNT || output == nullptr || + !io_.read(io_.context, bank, 0, header, sizeof(header)) || + memcmp(header, kRecordMagic, sizeof(kRecordMagic)) != 0 || + storage_read_u16(&header[4]) != kRecordFormatVersion || + storage_read_u16(&header[6]) != + CONTROLLER_PROFILE_DATABASE_SCHEMA_VERSION || + storage_read_u32(&header[12]) != + CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE || + profile_storage_crc32(header, kHeaderCrcOffset) != + storage_read_u32(&header[kHeaderCrcOffset])) { + return false; + } + output->generation = storage_read_u32(&header[8]); + output->payload_crc = storage_read_u32(&header[16]); + return true; +} + +bool ProfileStorage::validate_payload(uint8_t bank, + uint32_t expected_crc) const { + uint8_t page[PROFILE_STORAGE_PAGE_SIZE]{}; + uint32_t crc = 0xffffffffu; + for (size_t offset = 0; + offset < CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE; + offset += sizeof(page)) { + const size_t size = + CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset < sizeof(page) + ? CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset + : sizeof(page); + if (!io_.read(io_.context, bank, + PROFILE_STORAGE_RECORD_HEADER_SIZE + offset, + page, size)) { + return false; + } + crc = crc32_update(crc, page, size); + } + return ~crc == expected_crc; +} + +bool ProfileStorage::decode_bank( + uint8_t bank, ControllerProfileDatabase* database) const { + DatabaseReadContext context{&io_, bank}; + return controller_profile_database_decode(read_database, &context, + database); +} + +bool ProfileStorage::payload_matches( + uint8_t bank, const ControllerProfileDatabase& database) const { + uint8_t stored[PROFILE_STORAGE_PAGE_SIZE]{}; + uint8_t encoded[PROFILE_STORAGE_PAGE_SIZE]{}; + for (size_t offset = 0; + offset < CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE; + offset += sizeof(stored)) { + const size_t size = + CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset < sizeof(stored) + ? CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE - offset + : sizeof(stored); + if (!io_.read(io_.context, bank, + PROFILE_STORAGE_RECORD_HEADER_SIZE + offset, + stored, size) || + !controller_profile_database_encode_range(database, offset, + encoded, size) || + memcmp(stored, encoded, size) != 0) { + return false; + } + } + return true; +} diff --git a/profile_storage.h b/profile_storage.h new file mode 100644 index 0000000..ec6d8fd --- /dev/null +++ b/profile_storage.h @@ -0,0 +1,78 @@ +#pragma once + +#include +#include + +#include "controller_profile.h" + +constexpr uint8_t PROFILE_STORAGE_BANK_COUNT = 2; +constexpr size_t PROFILE_STORAGE_SECTOR_SIZE = 4096; +constexpr size_t PROFILE_STORAGE_SECTORS_PER_BANK = 5; +constexpr size_t PROFILE_STORAGE_BANK_SIZE = + PROFILE_STORAGE_SECTOR_SIZE * PROFILE_STORAGE_SECTORS_PER_BANK; +constexpr size_t PROFILE_STORAGE_TOTAL_SIZE = + PROFILE_STORAGE_BANK_COUNT * PROFILE_STORAGE_BANK_SIZE; +constexpr size_t PROFILE_STORAGE_PAGE_SIZE = 256; +constexpr size_t PROFILE_STORAGE_RECORD_HEADER_SIZE = + PROFILE_STORAGE_PAGE_SIZE; + +static_assert(PROFILE_STORAGE_BANK_SIZE == 20 * 1024); +static_assert(PROFILE_STORAGE_TOTAL_SIZE == 40 * 1024); +static_assert(PROFILE_STORAGE_RECORD_HEADER_SIZE + + CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE <= + PROFILE_STORAGE_BANK_SIZE, + "profile database does not fit a storage bank"); + +enum class ProfileStorageResult : uint8_t { + kOk = 0, + kUnchanged = 1, + kInvalidArgument = 2, + kIoError = 3, +}; + +struct ProfileStorageIo { + void* context = nullptr; + size_t bank_size = 0; + size_t sector_size = 0; + size_t page_size = 0; + bool (*read)(void* context, uint8_t bank, size_t offset, + uint8_t* output, size_t size) = nullptr; + bool (*erase_sector)(void* context, uint8_t bank, + size_t offset) = nullptr; + bool (*program)(void* context, uint8_t bank, size_t offset, + const uint8_t* data, size_t size) = nullptr; +}; + +struct ProfileStorageSnapshot { + bool valid = false; + uint32_t generation = 0; + uint32_t payload_crc = 0; + uint8_t active_bank = 0; +}; + +uint32_t profile_storage_crc32(const uint8_t* data, size_t size); + +class ProfileStorage { +public: + bool initialize(const ProfileStorageIo& io, + ControllerProfileDatabase* database); + ProfileStorageResult commit(const ControllerProfileDatabase& database); + const ProfileStorageSnapshot& snapshot() const; + +private: + struct BankHeader { + uint32_t generation = 0; + uint32_t payload_crc = 0; + }; + + bool read_header(uint8_t bank, BankHeader* output) const; + bool validate_payload(uint8_t bank, uint32_t expected_crc) const; + bool decode_bank(uint8_t bank, + ControllerProfileDatabase* database) const; + bool payload_matches(uint8_t bank, + const ControllerProfileDatabase& database) const; + + ProfileStorageIo io_{}; + ProfileStorageSnapshot snapshot_{}; + bool initialized_ = false; +}; diff --git a/src/switch_pico_bridge/config_manager.py b/src/switch_pico_bridge/config_manager.py index 3d7169d..8d5fcc3 100755 --- a/src/switch_pico_bridge/config_manager.py +++ b/src/switch_pico_bridge/config_manager.py @@ -1,9 +1,10 @@ #!/usr/bin/env python3 -"""Manage switch-pico persistent configuration and pairings over USB EP0.""" +"""Manage switch-pico persistent configuration, profiles, and pairings.""" from __future__ import annotations import argparse +import json import secrets import struct import sys @@ -11,6 +12,7 @@ import time import zlib from collections.abc import Iterable, Sequence from dataclasses import dataclass +from pathlib import Path from typing import Any, Protocol, cast import usb.core @@ -22,7 +24,7 @@ PROTOCOL_VERSION = 1 REQUEST_HEADER_SIZE = 16 RESPONSE_HEADER_SIZE = 20 MAXIMUM_REQUEST_SIZE = 64 -MAXIMUM_RESPONSE_SIZE = 152 +MAXIMUM_RESPONSE_SIZE = 293 MAXIMUM_CHUNK_SIZE = 40 USB_TIMEOUT_MS = 1000 @@ -36,6 +38,15 @@ OP_TRANSACTION_STATUS = 0x15 OP_PAIRING_READ = 0x20 OP_PAIRING_REFRESH = 0x21 OP_PAIRING_CLEAR = 0x22 +OP_PROFILE_LIST = 0x30 +OP_PROFILE_SELECT = 0x31 +OP_PROFILE_READ = 0x32 +OP_PROFILE_BEGIN = 0x33 +OP_PROFILE_CHUNK = 0x34 +OP_PROFILE_COMMIT = 0x35 +OP_PROFILE_RESET = 0x36 +OP_PROFILE_ACTIVATE = 0x37 +OP_PROFILE_TRANSACTION_STATUS = 0x38 STATUS_OK = 0 STATUS_PENDING = 1 @@ -55,8 +66,50 @@ PAIRING_WINDOW_SECONDS_MIN = 10 PAIRING_WINDOW_SECONDS_MAX = 300 PAIRING_RECORD_SIZE = 8 PAIRING_RECORD_CAPACITY = 16 +TRANSPORT_UNKNOWN = 0 TRANSPORT_CLASSIC = 1 TRANSPORT_BLE = 2 +PROFILE_SCHEMA_VERSION = 1 +PROFILE_SIZE = 256 +PROFILE_CAPACITY = 4 +PROFILE_IDENTITY_CAPACITY = 16 +PROFILE_LIST_CAPACITY = PROFILE_IDENTITY_CAPACITY + 1 +CONTROLLER_IDENTITY_SIZE = 14 +PROFILE_LIST_ROW_SIZE = 16 +PROFILE_NONE_BUTTON = 0xFF +PROFILE_MACRO_STEP_CAPACITY = 8 +PROFILE_MACRO_STEP_SIZE = 19 +PROFILE_MAXIMUM_WAIT_MS = 10000 + +LOGICAL_BUTTONS = ( + "south", + "east", + "west", + "north", + "left_shoulder", + "right_shoulder", + "select", + "start", + "system", + "capture", + "left_stick", + "right_stick", + "dpad_up", + "dpad_down", + "dpad_left", + "dpad_right", +) +RUMBLE_POLICIES = ("none", "rumble", "led", "rumble_and_led") +TURBO_MODES = ("off", "turbo", "auto_burst") +MACRO_STEP_TYPES = ("state", "end") +MACRO_OVERRIDE_NAMES = ( + "buttons", + "left_stick", + "right_stick", + "left_trigger", + "right_trigger", +) +MACRO_OVERRIDE_MASK = (1 << len(MACRO_OVERRIDE_NAMES)) - 1 class ConfigManagerError(RuntimeError): @@ -152,6 +205,934 @@ class PairingSnapshot: records: tuple[PairingRecord, ...] +def _require_int(value: Any, name: str, minimum: int, maximum: int) -> int: + if type(value) is not int or not minimum <= value <= maximum: + raise ConfigManagerError( + f"{name} must be an integer from {minimum} to {maximum}" + ) + return value + + +def _require_bool(value: Any, name: str) -> bool: + if type(value) is not bool: + raise ConfigManagerError(f"{name} must be true or false") + return value + + +def _require_object( + value: Any, fields: Sequence[str], name: str +) -> dict[str, Any]: + if type(value) is not dict: + raise ConfigManagerError(f"{name} must be a JSON object") + expected = set(fields) + actual = set(value) + missing = sorted(expected - actual) + unknown = sorted(actual - expected) + if missing or unknown: + details: list[str] = [] + if missing: + details.append("missing " + ", ".join(missing)) + if unknown: + details.append("unknown " + ", ".join(unknown)) + raise ConfigManagerError(f"invalid {name}: {'; '.join(details)}") + return value + + +def _require_enum(value: Any, choices: Sequence[str], name: str) -> int: + if type(value) is not str or value not in choices: + raise ConfigManagerError( + f"{name} must be one of {', '.join(choices)}" + ) + return choices.index(value) + + +def _button_index(value: Any, name: str) -> int: + if value is None: + return PROFILE_NONE_BUTTON + if type(value) is not str or value not in LOGICAL_BUTTONS: + raise ConfigManagerError( + f"{name} must be a logical button name or null" + ) + return LOGICAL_BUTTONS.index(value) + + +def _button_name(value: int) -> str | None: + if value == PROFILE_NONE_BUTTON: + return None + return LOGICAL_BUTTONS[value] + + +def _button_mask_from_json(value: Any, name: str) -> int: + if type(value) is not list: + raise ConfigManagerError(f"{name} must be a JSON array") + mask = 0 + for entry in value: + index = _button_index(entry, name) + if index == PROFILE_NONE_BUTTON: + raise ConfigManagerError(f"{name} cannot contain null") + bit = 1 << index + if mask & bit: + raise ConfigManagerError(f"{name} contains a duplicate button") + mask |= bit + return mask + + +def _button_mask_to_json(mask: int) -> list[str]: + return [ + name + for index, name in enumerate(LOGICAL_BUTTONS) + if mask & (1 << index) + ] + + +@dataclass(frozen=True) +class ControllerIdentity: + stable: bool + transport: int + address_type: int + address: bytes + vendor_id: int + product_id: int + + def __post_init__(self) -> None: + _require_bool(self.stable, "identity stable") + _require_int(self.transport, "identity transport", 0, 2) + _require_int(self.address_type, "identity address_type", 0, 0xFF) + if type(self.address) is not bytes or len(self.address) != 6: + raise ConfigManagerError("identity address must contain six bytes") + _require_int(self.vendor_id, "identity vendor_id", 0, 0xFFFF) + _require_int(self.product_id, "identity product_id", 0, 0xFFFF) + if self.stable: + if self.transport not in (TRANSPORT_CLASSIC, TRANSPORT_BLE): + raise ConfigManagerError( + "stable identity transport must be Classic or BLE" + ) + elif ( + self.transport != TRANSPORT_UNKNOWN + or self.address_type != 0 + or self.address != bytes(6) + or self.vendor_id != 0 + or self.product_id != 0 + ): + raise ConfigManagerError( + "unstable identity must be the all-zero global fallback" + ) + + @classmethod + def global_fallback(cls) -> ControllerIdentity: + return cls(False, TRANSPORT_UNKNOWN, 0, bytes(6), 0, 0) + + @classmethod + def from_bytes(cls, payload: bytes) -> ControllerIdentity: + payload = bytes(payload) + if len(payload) != CONTROLLER_IDENTITY_SIZE: + raise ConfigManagerError("invalid controller identity size") + stable, transport, address_type, reserved = payload[:4] + if stable not in (0, 1) or reserved != 0: + raise ConfigManagerError("invalid controller identity encoding") + vendor_id, product_id = struct.unpack_from(" bytes: + return ( + bytes( + [ + int(self.stable), + self.transport, + self.address_type, + 0, + ] + ) + + self.address + + struct.pack(" bool: + return not self.stable + + @property + def address_text(self) -> str: + return ":".join(f"{octet:02X}" for octet in self.address) + + @property + def transport_text(self) -> str: + if self.transport == TRANSPORT_CLASSIC: + return "Classic" + if self.transport == TRANSPORT_BLE: + return "BLE" + return "Unknown" + + +@dataclass(frozen=True) +class ProfileListEntry: + identity: ControllerIdentity + active_profile_index: int + + def __post_init__(self) -> None: + if not isinstance(self.identity, ControllerIdentity): + raise ConfigManagerError( + "profile-list identity must be a ControllerIdentity" + ) + _require_int( + self.active_profile_index, + "active profile index", + 0, + PROFILE_CAPACITY - 1, + ) + + +@dataclass(frozen=True) +class StickConfig: + center_x: int + center_y: int + inner_deadzone: int + outer_saturation: int + curve_q8_8: int + invert_x: bool + invert_y: bool + + def __post_init__(self) -> None: + _require_int(self.center_x, "stick center_x", -0x8000, 0x7FFF) + _require_int(self.center_y, "stick center_y", -0x8000, 0x7FFF) + _require_int( + self.inner_deadzone, "stick inner_deadzone", 0, 0x7FFF + ) + _require_int( + self.outer_saturation, "stick outer_saturation", 1, 0x7FFF + ) + if self.inner_deadzone >= self.outer_saturation: + raise ConfigManagerError( + "stick inner_deadzone must be below outer_saturation" + ) + _require_int(self.curve_q8_8, "stick curve_q8_8", 1, 0xFFFF) + _require_bool(self.invert_x, "stick invert_x") + _require_bool(self.invert_y, "stick invert_y") + + @classmethod + def from_bytes(cls, payload: bytes) -> StickConfig: + if len(payload) != 16 or payload[11:] != bytes(5): + raise ConfigManagerError("invalid stick configuration encoding") + center_x, center_y, inner, outer, curve_q8_8, flags = struct.unpack( + " bytes: + flags = int(self.invert_x) | (int(self.invert_y) << 1) + return struct.pack( + " dict[str, Any]: + return { + "center_x": self.center_x, + "center_y": self.center_y, + "inner_deadzone": self.inner_deadzone, + "outer_saturation": self.outer_saturation, + "curve_q8_8": self.curve_q8_8, + "invert_x": self.invert_x, + "invert_y": self.invert_y, + } + + @classmethod + def from_json_object(cls, value: Any, name: str) -> StickConfig: + fields = ( + "center_x", + "center_y", + "inner_deadzone", + "outer_saturation", + "curve_q8_8", + "invert_x", + "invert_y", + ) + obj = _require_object(value, fields, name) + return cls( + _require_int(obj["center_x"], f"{name}.center_x", -0x8000, 0x7FFF), + _require_int(obj["center_y"], f"{name}.center_y", -0x8000, 0x7FFF), + _require_int( + obj["inner_deadzone"], + f"{name}.inner_deadzone", + 0, + 0x7FFF, + ), + _require_int( + obj["outer_saturation"], + f"{name}.outer_saturation", + 1, + 0x7FFF, + ), + _require_int( + obj["curve_q8_8"], f"{name}.curve_q8_8", 1, 0xFFFF + ), + _require_bool(obj["invert_x"], f"{name}.invert_x"), + _require_bool(obj["invert_y"], f"{name}.invert_y"), + ) + + +@dataclass(frozen=True) +class TriggerConfig: + lower_deadzone: int + upper_saturation: int + curve_q8_8: int + digital_threshold: int + + def __post_init__(self) -> None: + _require_int( + self.lower_deadzone, "trigger lower_deadzone", 0, 0xFFFF + ) + _require_int( + self.upper_saturation, "trigger upper_saturation", 1, 0xFFFF + ) + if self.lower_deadzone >= self.upper_saturation: + raise ConfigManagerError( + "trigger lower_deadzone must be below upper_saturation" + ) + _require_int(self.curve_q8_8, "trigger curve_q8_8", 1, 0xFFFF) + _require_int( + self.digital_threshold, + "trigger digital_threshold", + self.lower_deadzone, + self.upper_saturation, + ) + + @classmethod + def from_bytes(cls, payload: bytes) -> TriggerConfig: + if len(payload) != 10 or payload[8:] != b"\x00\x00": + raise ConfigManagerError("invalid trigger configuration encoding") + return cls(*struct.unpack(" bytes: + return struct.pack( + " dict[str, int]: + return { + "lower_deadzone": self.lower_deadzone, + "upper_saturation": self.upper_saturation, + "curve_q8_8": self.curve_q8_8, + "digital_threshold": self.digital_threshold, + } + + @classmethod + def from_json_object(cls, value: Any, name: str) -> TriggerConfig: + fields = ( + "lower_deadzone", + "upper_saturation", + "curve_q8_8", + "digital_threshold", + ) + obj = _require_object(value, fields, name) + return cls( + _require_int( + obj["lower_deadzone"], + f"{name}.lower_deadzone", + 0, + 0xFFFF, + ), + _require_int( + obj["upper_saturation"], + f"{name}.upper_saturation", + 1, + 0xFFFF, + ), + _require_int( + obj["curve_q8_8"], f"{name}.curve_q8_8", 1, 0xFFFF + ), + _require_int( + obj["digital_threshold"], + f"{name}.digital_threshold", + 0, + 0xFFFF, + ), + ) + + +@dataclass(frozen=True) +class MacroStep: + step_type: int + override_flags: int + duration_ms: int + output_button_mask: int + left_stick_x: int + left_stick_y: int + right_stick_x: int + right_stick_y: int + left_trigger: int + right_trigger: int + + def __post_init__(self) -> None: + _require_int(self.step_type, "macro step type", 0, 1) + _require_int( + self.override_flags, "macro override flags", 0, MACRO_OVERRIDE_MASK + ) + _require_int( + self.duration_ms, + "macro duration_ms", + 0, + PROFILE_MAXIMUM_WAIT_MS, + ) + _require_int( + self.output_button_mask, "macro output button mask", 0, 0xFFFF + ) + for name in ( + "left_stick_x", + "left_stick_y", + "right_stick_x", + "right_stick_y", + ): + _require_int( + getattr(self, name), f"macro {name}", -0x8000, 0x7FFF + ) + _require_int(self.left_trigger, "macro left_trigger", 0, 0xFFFF) + _require_int(self.right_trigger, "macro right_trigger", 0, 0xFFFF) + if self.step_type == 1 and any( + ( + self.override_flags, + self.duration_ms, + self.output_button_mask, + self.left_stick_x, + self.left_stick_y, + self.right_stick_x, + self.right_stick_y, + self.left_trigger, + self.right_trigger, + ) + ): + raise ConfigManagerError("end macro step must otherwise be zero") + if self.step_type == 0: + if ( + not self.override_flags & 1 + and self.output_button_mask != 0 + ): + raise ConfigManagerError( + "macro buttons require the buttons override" + ) + if ( + not self.override_flags & 2 + and (self.left_stick_x != 0 or self.left_stick_y != 0) + ): + raise ConfigManagerError( + "macro left stick values require the left_stick override" + ) + if ( + not self.override_flags & 4 + and (self.right_stick_x != 0 or self.right_stick_y != 0) + ): + raise ConfigManagerError( + "macro right stick values require the right_stick override" + ) + if not self.override_flags & 8 and self.left_trigger != 0: + raise ConfigManagerError( + "macro left trigger requires the left_trigger override" + ) + if not self.override_flags & 16 and self.right_trigger != 0: + raise ConfigManagerError( + "macro right trigger requires the right_trigger override" + ) + + @classmethod + def end(cls) -> MacroStep: + return cls(1, 0, 0, 0, 0, 0, 0, 0, 0, 0) + + @classmethod + def from_bytes(cls, payload: bytes) -> MacroStep: + if len(payload) != PROFILE_MACRO_STEP_SIZE or payload[18] != 0: + raise ConfigManagerError("invalid macro step encoding") + return cls(*struct.unpack(" bytes: + return struct.pack( + " dict[str, Any]: + return { + "type": MACRO_STEP_TYPES[self.step_type], + "overrides": [ + name + for index, name in enumerate(MACRO_OVERRIDE_NAMES) + if self.override_flags & (1 << index) + ], + "duration_ms": self.duration_ms, + "output_buttons": _button_mask_to_json( + self.output_button_mask + ), + "left_stick": { + "x": self.left_stick_x, + "y": self.left_stick_y, + }, + "right_stick": { + "x": self.right_stick_x, + "y": self.right_stick_y, + }, + "triggers": { + "left": self.left_trigger, + "right": self.right_trigger, + }, + } + + @classmethod + def from_json_object(cls, value: Any, name: str) -> MacroStep: + fields = ( + "type", + "overrides", + "duration_ms", + "output_buttons", + "left_stick", + "right_stick", + "triggers", + ) + obj = _require_object(value, fields, name) + overrides = obj["overrides"] + if type(overrides) is not list: + raise ConfigManagerError(f"{name}.overrides must be a JSON array") + override_flags = 0 + for override in overrides: + index = _require_enum( + override, MACRO_OVERRIDE_NAMES, f"{name}.overrides" + ) + bit = 1 << index + if override_flags & bit: + raise ConfigManagerError( + f"{name}.overrides contains a duplicate" + ) + override_flags |= bit + left = _require_object( + obj["left_stick"], ("x", "y"), f"{name}.left_stick" + ) + right = _require_object( + obj["right_stick"], ("x", "y"), f"{name}.right_stick" + ) + triggers = _require_object( + obj["triggers"], ("left", "right"), f"{name}.triggers" + ) + return cls( + _require_enum(obj["type"], MACRO_STEP_TYPES, f"{name}.type"), + override_flags, + _require_int( + obj["duration_ms"], + f"{name}.duration_ms", + 0, + PROFILE_MAXIMUM_WAIT_MS, + ), + _button_mask_from_json( + obj["output_buttons"], f"{name}.output_buttons" + ), + _require_int( + left["x"], f"{name}.left_stick.x", -0x8000, 0x7FFF + ), + _require_int( + left["y"], f"{name}.left_stick.y", -0x8000, 0x7FFF + ), + _require_int( + right["x"], f"{name}.right_stick.x", -0x8000, 0x7FFF + ), + _require_int( + right["y"], f"{name}.right_stick.y", -0x8000, 0x7FFF + ), + _require_int( + triggers["left"], + f"{name}.triggers.left", + 0, + 0xFFFF, + ), + _require_int( + triggers["right"], + f"{name}.triggers.right", + 0, + 0xFFFF, + ), + ) + + +@dataclass(frozen=True) +class ControllerProfile: + button_map: tuple[int, ...] + left_stick: StickConfig + right_stick: StickConfig + left_trigger: TriggerConfig + right_trigger: TriggerConfig + weak_rumble_scale: int + strong_rumble_scale: int + confirmation_policy: int + switching_chord: int + macro_trigger: int + macro_cancel: int + macro_steps: tuple[MacroStep, ...] + turbo_modes: tuple[int, ...] + + def __post_init__(self) -> None: + if type(self.button_map) is not tuple or len(self.button_map) != len( + LOGICAL_BUTTONS + ): + raise ConfigManagerError( + "button map must contain 16 logical mappings" + ) + for mapping in self.button_map: + if type(mapping) is not int or ( + mapping != PROFILE_NONE_BUTTON + and not 0 <= mapping < len(LOGICAL_BUTTONS) + ): + raise ConfigManagerError("invalid logical button mapping") + if not isinstance(self.left_stick, StickConfig) or not isinstance( + self.right_stick, StickConfig + ): + raise ConfigManagerError("profile sticks must be StickConfig values") + if not isinstance(self.left_trigger, TriggerConfig) or not isinstance( + self.right_trigger, TriggerConfig + ): + raise ConfigManagerError( + "profile triggers must be TriggerConfig values" + ) + _require_int( + self.weak_rumble_scale, "weak rumble scale", 0, 0xFF + ) + _require_int( + self.strong_rumble_scale, "strong rumble scale", 0, 0xFF + ) + _require_int( + self.confirmation_policy, + "confirmation policy", + 0, + len(RUMBLE_POLICIES) - 1, + ) + _require_int(self.switching_chord, "switching chord", 0, 0xFFFF) + for value, name in ( + (self.macro_trigger, "macro trigger"), + (self.macro_cancel, "macro cancel"), + ): + if type(value) is not int or ( + value != PROFILE_NONE_BUTTON + and not 0 <= value < len(LOGICAL_BUTTONS) + ): + raise ConfigManagerError(f"invalid {name}") + if ( + type(self.macro_steps) is not tuple + or not 1 <= len(self.macro_steps) <= PROFILE_MACRO_STEP_CAPACITY + or not all(isinstance(step, MacroStep) for step in self.macro_steps) + ): + raise ConfigManagerError("macro must contain one to eight steps") + if any(step.step_type != 0 for step in self.macro_steps[:-1]): + raise ConfigManagerError("only the final macro step may be end") + if self.macro_steps[-1] != MacroStep.end(): + raise ConfigManagerError("final macro step must be canonical end") + if type(self.turbo_modes) is not tuple or len( + self.turbo_modes + ) != len(LOGICAL_BUTTONS): + raise ConfigManagerError("Turbo modes must contain 16 entries") + for mode in self.turbo_modes: + _require_int(mode, "Turbo mode", 0, len(TURBO_MODES) - 1) + + @classmethod + def default(cls) -> ControllerProfile: + stick = StickConfig(0, 0, 0, 0x7FFF, 256, False, False) + trigger = TriggerConfig(0, 0xFFFF, 256, 0x8000) + return cls( + button_map=tuple(range(len(LOGICAL_BUTTONS))), + left_stick=stick, + right_stick=stick, + left_trigger=trigger, + right_trigger=trigger, + weak_rumble_scale=0xFF, + strong_rumble_scale=0xFF, + confirmation_policy=3, + switching_chord=0, + macro_trigger=PROFILE_NONE_BUTTON, + macro_cancel=PROFILE_NONE_BUTTON, + macro_steps=(MacroStep.end(),), + turbo_modes=(0,) * len(LOGICAL_BUTTONS), + ) + + @classmethod + def from_bytes(cls, payload: bytes) -> ControllerProfile: + payload = bytes(payload) + if len(payload) != PROFILE_SIZE: + raise ConfigManagerError("invalid profile size") + version, size = struct.unpack_from(" bytes: + payload = bytearray(PROFILE_SIZE) + struct.pack_into( + " dict[str, Any]: + return { + "schema_version": PROFILE_SCHEMA_VERSION, + "size": PROFILE_SIZE, + "button_map": { + name: _button_name(self.button_map[index]) + for index, name in enumerate(LOGICAL_BUTTONS) + }, + "sticks": { + "left": self.left_stick.to_json_object(), + "right": self.right_stick.to_json_object(), + }, + "triggers": { + "left": self.left_trigger.to_json_object(), + "right": self.right_trigger.to_json_object(), + }, + "rumble": { + "weak_scale": self.weak_rumble_scale, + "strong_scale": self.strong_rumble_scale, + "confirmation_policy": RUMBLE_POLICIES[ + self.confirmation_policy + ], + }, + "switching_chord": _button_mask_to_json(self.switching_chord), + "macro": { + "trigger": _button_name(self.macro_trigger), + "cancel": _button_name(self.macro_cancel), + "steps": [ + step.to_json_object() for step in self.macro_steps + ], + }, + "turbo": { + name: TURBO_MODES[self.turbo_modes[index]] + for index, name in enumerate(LOGICAL_BUTTONS) + }, + } + + def to_json(self) -> str: + return json.dumps(self.to_json_object(), indent=2) + "\n" + + @classmethod + def from_json_object(cls, value: Any) -> ControllerProfile: + fields = ( + "schema_version", + "size", + "button_map", + "sticks", + "triggers", + "rumble", + "switching_chord", + "macro", + "turbo", + ) + obj = _require_object(value, fields, "profile") + if ( + _require_int( + obj["schema_version"], "profile.schema_version", 0, 0xFFFF + ) + != PROFILE_SCHEMA_VERSION + or _require_int(obj["size"], "profile.size", 0, 0xFFFF) + != PROFILE_SIZE + ): + raise ConfigManagerError("unsupported profile schema") + button_map = _require_object( + obj["button_map"], LOGICAL_BUTTONS, "profile.button_map" + ) + sticks = _require_object( + obj["sticks"], ("left", "right"), "profile.sticks" + ) + triggers = _require_object( + obj["triggers"], ("left", "right"), "profile.triggers" + ) + rumble = _require_object( + obj["rumble"], + ("weak_scale", "strong_scale", "confirmation_policy"), + "profile.rumble", + ) + macro = _require_object( + obj["macro"], ("trigger", "cancel", "steps"), "profile.macro" + ) + turbo = _require_object( + obj["turbo"], LOGICAL_BUTTONS, "profile.turbo" + ) + steps = macro["steps"] + if type(steps) is not list or not ( + 1 <= len(steps) <= PROFILE_MACRO_STEP_CAPACITY + ): + raise ConfigManagerError( + "profile.macro.steps must contain one to eight steps" + ) + return cls( + button_map=tuple( + _button_index( + button_map[name], f"profile.button_map.{name}" + ) + for name in LOGICAL_BUTTONS + ), + left_stick=StickConfig.from_json_object( + sticks["left"], "profile.sticks.left" + ), + right_stick=StickConfig.from_json_object( + sticks["right"], "profile.sticks.right" + ), + left_trigger=TriggerConfig.from_json_object( + triggers["left"], "profile.triggers.left" + ), + right_trigger=TriggerConfig.from_json_object( + triggers["right"], "profile.triggers.right" + ), + weak_rumble_scale=_require_int( + rumble["weak_scale"], + "profile.rumble.weak_scale", + 0, + 0xFF, + ), + strong_rumble_scale=_require_int( + rumble["strong_scale"], + "profile.rumble.strong_scale", + 0, + 0xFF, + ), + confirmation_policy=_require_enum( + rumble["confirmation_policy"], + RUMBLE_POLICIES, + "profile.rumble.confirmation_policy", + ), + switching_chord=_button_mask_from_json( + obj["switching_chord"], "profile.switching_chord" + ), + macro_trigger=_button_index( + macro["trigger"], "profile.macro.trigger" + ), + macro_cancel=_button_index( + macro["cancel"], "profile.macro.cancel" + ), + macro_steps=tuple( + MacroStep.from_json_object( + step, f"profile.macro.steps[{index}]" + ) + for index, step in enumerate(steps) + ), + turbo_modes=tuple( + _require_enum( + turbo[name], TURBO_MODES, f"profile.turbo.{name}" + ) + for name in LOGICAL_BUTTONS + ), + ) + + @classmethod + def from_json(cls, payload: str) -> ControllerProfile: + try: + value = json.loads(payload, object_pairs_hook=_unique_json_object) + except json.JSONDecodeError as exc: + raise ConfigManagerError(f"invalid profile JSON: {exc.msg}") from exc + return cls.from_json_object(value) + + +def _unique_json_object(pairs: list[tuple[str, Any]]) -> dict[str, Any]: + result: dict[str, Any] = {} + for key, value in pairs: + if key in result: + raise ConfigManagerError(f"duplicate JSON field {key}") + result[key] = value + return result + + def _crc32(payload: bytes) -> int: return zlib.crc32(payload) & 0xFFFFFFFF @@ -362,6 +1343,197 @@ def reset_configuration(device: UsbDevice, timeout: float) -> TransactionStatus: return _wait_for_transaction(device, transaction_id, timeout) +def parse_profile_list(envelope: Envelope) -> tuple[ProfileListEntry, ...]: + _raise_status(envelope) + if envelope.schema_version != PROFILE_SCHEMA_VERSION: + raise ConfigManagerError("unsupported profile-list schema") + if not envelope.payload: + raise ConfigManagerError("short profile-list payload") + count = envelope.payload[0] + if ( + not 1 <= count <= PROFILE_LIST_CAPACITY + or len(envelope.payload) != 1 + count * PROFILE_LIST_ROW_SIZE + ): + raise ConfigManagerError("invalid profile-list count") + entries: list[ProfileListEntry] = [] + identities: set[ControllerIdentity] = set() + for index in range(count): + offset = 1 + index * PROFILE_LIST_ROW_SIZE + identity = ControllerIdentity.from_bytes( + envelope.payload[offset : offset + CONTROLLER_IDENTITY_SIZE] + ) + active_profile_index = envelope.payload[offset + 14] + if envelope.payload[offset + 15] != 0: + raise ConfigManagerError("profile-list reserved field is nonzero") + if index == 0 and not identity.is_global_fallback: + raise ConfigManagerError( + "profile list does not begin with global fallback" + ) + if index != 0 and identity.is_global_fallback: + raise ConfigManagerError("duplicate global fallback profile entry") + if identity in identities: + raise ConfigManagerError("duplicate identity in profile list") + identities.add(identity) + entries.append(ProfileListEntry(identity, active_profile_index)) + return tuple(entries) + + +def list_profiles(device: UsbDevice) -> tuple[ProfileListEntry, ...]: + return parse_profile_list(_control_in(device, OP_PROFILE_LIST)) + + +def _validate_profile_index(profile_index: int) -> None: + _require_int( + profile_index, + "profile index", + 0, + PROFILE_CAPACITY - 1, + ) + + +def select_profile( + device: UsbDevice, + identity: ControllerIdentity, + profile_index: int, +) -> None: + _validate_profile_index(profile_index) + _control_out( + device, + OP_PROFILE_SELECT, + identity.to_bytes() + bytes((profile_index,)), + ) + + +def read_selected_profile(device: UsbDevice) -> ControllerProfile: + envelope = _control_in(device, OP_PROFILE_READ) + _raise_status(envelope) + if envelope.schema_version != PROFILE_SCHEMA_VERSION: + raise ConfigManagerError("unsupported profile schema") + return ControllerProfile.from_bytes(envelope.payload) + + +def read_profile( + device: UsbDevice, + identity: ControllerIdentity, + profile_index: int, +) -> ControllerProfile: + select_profile(device, identity, profile_index) + return read_selected_profile(device) + + +def read_profile_transaction_status(device: UsbDevice) -> TransactionStatus: + envelope = _control_in(device, OP_PROFILE_TRANSACTION_STATUS) + _raise_status(envelope, pending_ok=True) + if ( + envelope.schema_version != PROFILE_SCHEMA_VERSION + or len(envelope.payload) != 20 + ): + raise ConfigManagerError("invalid profile transaction-status payload") + values = struct.unpack(" TransactionStatus: + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + envelope = _control_in(device, OP_PROFILE_TRANSACTION_STATUS) + if ( + envelope.schema_version != PROFILE_SCHEMA_VERSION + or len(envelope.payload) != 20 + ): + raise ConfigManagerError( + "invalid profile transaction-status payload" + ) + values = struct.unpack(" TransactionStatus: + _validate_profile_index(profile_index) + if not isinstance(profile, ControllerProfile): + raise ConfigManagerError("profile must be a ControllerProfile") + payload = profile.to_bytes() + transaction_id = secrets.randbits(32) or 1 + begin = ( + struct.pack(" TransactionStatus: + wire_index = PROFILE_NONE_BUTTON + if profile_index is not None: + _validate_profile_index(profile_index) + wire_index = profile_index + transaction_id = secrets.randbits(32) or 1 + _control_out( + device, + OP_PROFILE_RESET, + struct.pack(" TransactionStatus: + _validate_profile_index(profile_index) + transaction_id = secrets.randbits(32) or 1 + _control_out( + device, + OP_PROFILE_ACTIVATE, + struct.pack(" PairingSnapshot: _raise_status(envelope, pending_ok=True) if len(envelope.payload) < 4: @@ -484,10 +1656,96 @@ def _print_pairings(snapshot: PairingSnapshot) -> None: print("Warning: additional pairings did not fit in the response.") +def _print_profiles(entries: Sequence[ProfileListEntry]) -> None: + for index, entry in enumerate(entries): + identity = entry.identity + if identity.is_global_fallback: + description = "global fallback" + else: + description = ( + f"{identity.transport_text} {identity.address_text} " + f"address-type {identity.address_type} " + f"VID:PID {identity.vendor_id:04X}:{identity.product_id:04X}" + ) + print( + f"{index}: {description} " + f"(active profile {entry.active_profile_index + 1})" + ) + + +def _resolve_profile_identity( + entries: Sequence[ProfileListEntry], identity_index: int +) -> ControllerIdentity: + if not 0 <= identity_index < len(entries): + raise ConfigManagerError( + f"identity index {identity_index} is out of range; " + f"use profiles list to see indices 0 through {len(entries) - 1}" + ) + return entries[identity_index].identity + + +def _load_profile(path: Path) -> ControllerProfile: + try: + payload = path.read_text(encoding="utf-8") + except OSError as exc: + raise ConfigManagerError( + f"could not read profile JSON {path}: {exc}" + ) from exc + return ControllerProfile.from_json(payload) + + +def _save_profile(path: Path, profile: ControllerProfile) -> None: + try: + path.write_text(profile.to_json(), encoding="utf-8") + except OSError as exc: + raise ConfigManagerError( + f"could not write profile JSON {path}: {exc}" + ) from exc + + +def _profile_number(value: str) -> int: + try: + number = int(value) + except ValueError as exc: + raise argparse.ArgumentTypeError( + "profile must be a number from 1 to 4" + ) from exc + if not 1 <= number <= PROFILE_CAPACITY: + raise argparse.ArgumentTypeError("profile must be a number from 1 to 4") + return number - 1 + + +def _identity_index(value: str) -> int: + try: + index = int(value) + except ValueError as exc: + raise argparse.ArgumentTypeError( + "identity must be a non-negative list index" + ) from exc + if index < 0: + raise argparse.ArgumentTypeError( + "identity must be a non-negative list index" + ) + return index + + +def _add_identity_argument(parser: argparse.ArgumentParser) -> None: + parser.add_argument( + "--identity", + type=_identity_index, + default=0, + metavar="N", + help="identity index from profiles list (default: 0)", + ) + + def build_parser() -> argparse.ArgumentParser: parser = argparse.ArgumentParser( prog="switch-pico-config", - description="Manage switch-pico persistent configuration and pairings.", + description=( + "Manage switch-pico persistent configuration, profiles, " + "and pairings." + ), ) parser.add_argument("--bus", type=int, help="USB bus number") parser.add_argument("--address", type=int, help="USB device address") @@ -513,6 +1771,49 @@ def build_parser() -> argparse.ArgumentParser: config_reset = config_commands.add_parser("reset", help="restore defaults") config_reset.add_argument("--yes", action="store_true") + profiles = commands.add_parser( + "profiles", help="list, import, export, or select controller profiles" + ) + profile_commands = profiles.add_subparsers( + dest="profile_command", required=True + ) + profile_commands.add_parser( + "list", help="list profile identities and active profiles" + ) + profile_export = profile_commands.add_parser( + "export", help="export a profile as JSON" + ) + profile_export.add_argument( + "profile_index", type=_profile_number, metavar="PROFILE" + ) + profile_export.add_argument("path", type=Path, metavar="PATH") + _add_identity_argument(profile_export) + profile_import = profile_commands.add_parser( + "import", help="import a profile from JSON" + ) + profile_import.add_argument( + "profile_index", type=_profile_number, metavar="PROFILE" + ) + profile_import.add_argument("path", type=Path, metavar="PATH") + _add_identity_argument(profile_import) + profile_reset = profile_commands.add_parser( + "reset", help="reset one or all profiles" + ) + profile_reset.add_argument( + "profile_index", + type=lambda value: None if value == "all" else _profile_number(value), + metavar="PROFILE|all", + ) + _add_identity_argument(profile_reset) + profile_reset.add_argument("--yes", action="store_true") + profile_activate = profile_commands.add_parser( + "activate", help="activate a profile" + ) + profile_activate.add_argument( + "profile_index", type=_profile_number, metavar="PROFILE" + ) + _add_identity_argument(profile_activate) + pairings = commands.add_parser("pairings", help="list or clear pairings") pairing_commands = pairings.add_subparsers( dest="pairing_command", required=True @@ -528,14 +1829,32 @@ def main(argv: Sequence[str] | None = None) -> int: if args.timeout <= 0: print("error: --timeout must be positive", file=sys.stderr) return 2 - if args.command == "config" and args.config_command == "reset" and not args.yes: + if ( + args.command == "config" + and args.config_command == "reset" + and not args.yes + ): print("error: config reset requires --yes", file=sys.stderr) return 2 - if args.command == "pairings" and args.pairing_command == "clear" and not args.yes: + if ( + args.command == "pairings" + and args.pairing_command == "clear" + and not args.yes + ): print("error: pairings clear requires --yes", file=sys.stderr) return 2 + if ( + args.command == "profiles" + and args.profile_command == "reset" + and not args.yes + ): + print("error: profiles reset requires --yes", file=sys.stderr) + return 2 + imported_profile: ControllerProfile | None = None try: + if args.command == "profiles" and args.profile_command == "import": + imported_profile = _load_profile(args.path) device = find_pico(args.bus, args.address, args.timeout) if args.command == "status": info = read_info(device) @@ -581,6 +1900,53 @@ def main(argv: Sequence[str] | None = None) -> int: "Reset configuration at generation " f"{status.stored_generation}." ) + elif args.command == "profiles": + entries = list_profiles(device) + if args.profile_command == "list": + _print_profiles(entries) + else: + identity = _resolve_profile_identity(entries, args.identity) + if args.profile_command == "export": + profile = read_profile( + device, identity, args.profile_index + ) + _save_profile(args.path, profile) + print( + f"Exported profile {args.profile_index + 1} " + f"for identity {args.identity} to {args.path}." + ) + elif args.profile_command == "import": + status = write_profile( + device, + identity, + args.profile_index, + cast(ControllerProfile, imported_profile), + args.timeout, + ) + print( + f"Stored profile {args.profile_index + 1} " + f"for identity {args.identity} at generation " + f"{status.stored_generation} " + f"(CRC {status.stored_crc:08x})." + ) + elif args.profile_command == "reset": + reset_profile( + device, identity, args.profile_index, args.timeout + ) + target = ( + "all profiles" + if args.profile_index is None + else f"profile {args.profile_index + 1}" + ) + print(f"Reset {target} for identity {args.identity}.") + else: + activate_profile( + device, identity, args.profile_index, args.timeout + ) + print( + f"Activated profile {args.profile_index + 1} " + f"for identity {args.identity}." + ) elif args.pairing_command == "list": _print_pairings(refresh_pairings(device, args.timeout)) else: diff --git a/switch-pico.cpp b/switch-pico.cpp index 361021f..90aac7b 100644 --- a/switch-pico.cpp +++ b/switch-pico.cpp @@ -290,8 +290,10 @@ int main() { } for (uint8_t instance = 0; instance < BLUEPAD32_INPUT_BACKEND_SLOT_COUNT; ++instance) { - bluepad32_input_backend_snapshot(instance, - &g_user_states[instance]); + Bluepad32SlotSnapshot snapshot{}; + bluepad32_input_backend_snapshot(instance, &snapshot); + g_user_states[instance] = + snapshot.active ? snapshot.state : controller_neutral_state(); #ifdef SWITCH_PICO_ADAPTER_FEASIBILITY bool sent = false; if (adapter_host_probe_mode() == AdapterUsbMode::kXInput) { diff --git a/tests/bluepad32_backend_lifecycle_test.cpp b/tests/bluepad32_backend_lifecycle_test.cpp index 4f0d280..d498f7c 100644 --- a/tests/bluepad32_backend_lifecycle_test.cpp +++ b/tests/bluepad32_backend_lifecycle_test.cpp @@ -25,6 +25,7 @@ bool ssp_auto_accept = true; uint8_t accepted_stk_methods = 0xff; uint16_t link_supervision_timeout = 0; btstack_packet_handler_t pairing_event_handler = nullptr; +btstack_packet_handler_t identity_event_handler = nullptr; int confirmation_accepts = 0; int confirmation_rejections = 0; int passkey_accepts = 0; @@ -43,6 +44,8 @@ int cyw43_init_calls = 0; int uni_init_calls = 0; int device_disconnect_calls = 0; uni_hid_device_t* last_disconnected_device = nullptr; +uni_hid_device_t* lookup_devices[8]{}; +size_t lookup_device_count = 0; struct CoreStopped {}; @@ -82,11 +85,21 @@ uni_hid_device_t device( result.idx = idx; result.gamepad = gamepad; result.conn.protocol = protocol; + result.conn.handle = static_cast(0x40 + idx); result.conn.btaddr[5] = static_cast(idx + 1); + result.vendor_id = static_cast(0x1000 + idx); + result.product_id = static_cast(0x2000 + idx); result.report_parser.play_dual_rumble = play_rumble; return result; } +void register_lookup_device(uni_hid_device_t* candidate) { + require(lookup_device_count < + sizeof(lookup_devices) / sizeof(lookup_devices[0]), + "test BLE lookup registry overflow"); + lookup_devices[lookup_device_count++] = candidate; +} + } // namespace @@ -98,6 +111,16 @@ int uni_hid_device_get_idx_for_instance(const uni_hid_device_t* device) { return device == nullptr ? -1 : device->idx; } +uni_hid_device_t* uni_hid_device_get_instance_for_connection_handle( + hci_con_handle_t handle) { + for (size_t index = 0; index < lookup_device_count; ++index) { + if (lookup_devices[index]->conn.handle == handle) { + return lookup_devices[index]; + } + } + return nullptr; +} + void uni_hid_device_disconnect(uni_hid_device_t* device) { ++device_disconnect_calls; last_disconnected_device = device; @@ -226,6 +249,11 @@ void hci_add_event_handler( pairing_event_handler = callback_handler->callback; } +void sm_add_event_handler( + btstack_packet_callback_registration_t* callback_handler) { + identity_event_handler = callback_handler->callback; +} + uint8_t hci_event_packet_get_type(const uint8_t* packet) { return packet[0]; } @@ -246,6 +274,108 @@ void hci_event_user_passkey_request_get_bd_addr( copy_event_address(packet, address); } +hci_con_handle_t sm_event_handle(const uint8_t* packet) { + return static_cast(packet[2]) | + static_cast(packet[3] << 8); +} + +void copy_sm_event_address(const uint8_t* packet, size_t offset, + bd_addr_t address) { + for (size_t index = 0; index < sizeof(bd_addr_t); ++index) { + address[index] = packet[offset + sizeof(bd_addr_t) - 1 - index]; + } +} + +hci_con_handle_t sm_event_identity_resolving_started_get_handle( + const uint8_t* packet) { + return sm_event_handle(packet); +} + +hci_con_handle_t sm_event_identity_resolving_failed_get_handle( + const uint8_t* packet) { + return sm_event_handle(packet); +} + +hci_con_handle_t sm_event_identity_resolving_succeeded_get_handle( + const uint8_t* packet) { + return sm_event_handle(packet); +} + +uint8_t sm_event_identity_resolving_succeeded_get_addr_type( + const uint8_t* packet) { + return packet[4]; +} + +void sm_event_identity_resolving_succeeded_get_address( + const uint8_t* packet, bd_addr_t address) { + copy_sm_event_address(packet, 5, address); +} + +uint8_t sm_event_identity_resolving_succeeded_get_identity_addr_type( + const uint8_t* packet) { + return packet[11]; +} + +void sm_event_identity_resolving_succeeded_get_identity_address( + const uint8_t* packet, bd_addr_t address) { + copy_sm_event_address(packet, 12, address); +} + +hci_con_handle_t sm_event_identity_created_get_handle( + const uint8_t* packet) { + return sm_event_handle(packet); +} + +void sm_event_identity_created_get_address( + const uint8_t* packet, bd_addr_t address) { + copy_sm_event_address(packet, 5, address); +} + +uint8_t sm_event_identity_created_get_identity_addr_type( + const uint8_t* packet) { + return packet[11]; +} + +void sm_event_identity_created_get_identity_address( + const uint8_t* packet, bd_addr_t address) { + copy_sm_event_address(packet, 12, address); +} + +hci_con_handle_t sm_event_reencryption_started_get_handle( + const uint8_t* packet) { + return sm_event_handle(packet); +} + +uint8_t sm_event_reencryption_started_get_addr_type( + const uint8_t* packet) { + return packet[4]; +} + +void sm_event_reencryption_started_get_address( + const uint8_t* packet, bd_addr_t address) { + copy_sm_event_address(packet, 5, address); +} + +hci_con_handle_t sm_event_reencryption_complete_get_handle( + const uint8_t* packet) { + return sm_event_handle(packet); +} + +uint8_t sm_event_reencryption_complete_get_addr_type( + const uint8_t* packet) { + return packet[4]; +} + +void sm_event_reencryption_complete_get_address( + const uint8_t* packet, bd_addr_t address) { + copy_sm_event_address(packet, 5, address); +} + +uint8_t sm_event_reencryption_complete_get_status( + const uint8_t* packet) { + return packet[11]; +} + void uni_platform_set_custom(uni_platform* platform) { installed_platform = platform; @@ -284,10 +414,26 @@ uint32_t btstack_run_loop_get_time_ms() { } +#include "../controller_identity.cpp" #include "../bluepad32_input_backend.cpp" +ControllerIdentity observed_profile_identities[8]{}; +size_t observed_profile_identity_count = 0; void configuration_service_prepare() {} void configuration_service_initialize_on_storage_core() {} void configuration_service_task_on_storage_core(uint32_t) {} +void profile_service_prepare() {} +void profile_service_initialize_on_storage_core() {} +void profile_service_task_on_storage_core(uint32_t) {} +bool profile_service_observe_identity_on_storage_core( + const ControllerIdentity& identity) { + require(observed_profile_identity_count < + sizeof(observed_profile_identities) / + sizeof(observed_profile_identities[0]), + "profile identity observation fixture overflow"); + observed_profile_identities[observed_profile_identity_count++] = + identity; + return true; +} void configuration_service_snapshot(ConfigurationServiceSnapshot* output) { *output = {}; output->state = ConfigurationServiceState::kReady; @@ -319,6 +465,15 @@ SwitchRgbColor switch_pro_get_slot_light_color(uint8_t instance) { namespace { +bool read_controller_state(uint8_t slot, ControllerState* output) { + Bluepad32SlotSnapshot snapshot{}; + bluepad32_input_backend_snapshot(slot, &snapshot); + if (output != nullptr) { + *output = snapshot.state; + } + return snapshot.active; +} + void start_backend() { bluepad32_input_backend_init(); platform_on_init_complete(); @@ -327,8 +482,9 @@ void start_backend() { link_supervision_timeout == kClassicLinkSupervisionTimeout && !bondable && accepted_stk_methods == 0 && - !ssp_auto_accept && pairing_event_handler != nullptr, - "initialization must configure liveness and pairing policy"); + !ssp_auto_accept && pairing_event_handler != nullptr && + identity_event_handler != nullptr, + "initialization must register Classic and BLE identity policy"); } void start_pairing_backend() { start_backend(); @@ -344,6 +500,114 @@ void dispatch_pairing_event(uint8_t event_type) { pairing_event_handler(HCI_EVENT_PACKET, 0, packet, sizeof(packet)); } +void write_event_address(uint8_t* packet, size_t offset, + const bd_addr_t address) { + for (size_t index = 0; index < sizeof(bd_addr_t); ++index) { + packet[offset + index] = + address[sizeof(bd_addr_t) - 1 - index]; + } +} + +void dispatch_identity_event(uint8_t event_type, + const uni_hid_device_t& controller, + uint8_t identity_address_type, + const bd_addr_t identity_address, + uint8_t status = ERROR_CODE_SUCCESS) { + uint8_t packet[20]{}; + size_t packet_size = 0; + packet[0] = event_type; + packet[2] = static_cast(controller.conn.handle); + packet[3] = static_cast(controller.conn.handle >> 8); + switch (event_type) { + case SM_EVENT_IDENTITY_RESOLVING_SUCCEEDED: + packet_size = sizeof(packet); + packet[4] = BD_ADDR_TYPE_LE_RANDOM; + write_event_address(packet, 5, controller.conn.btaddr); + packet[11] = identity_address_type; + write_event_address(packet, 12, identity_address); + break; + case SM_EVENT_IDENTITY_CREATED: + packet_size = sizeof(packet); + packet[4] = identity_address_type; + write_event_address(packet, 5, identity_address); + packet[11] = identity_address_type; + write_event_address(packet, 12, identity_address); + break; + case SM_EVENT_REENCRYPTION_STARTED: + packet_size = 11; + packet[4] = identity_address_type; + write_event_address(packet, 5, identity_address); + break; + case SM_EVENT_REENCRYPTION_COMPLETE: + packet_size = 12; + packet[4] = identity_address_type; + write_event_address(packet, 5, identity_address); + packet[11] = status; + break; + default: + require(false, "unsupported identity event fixture"); + } + packet[1] = static_cast(packet_size - 2); + identity_event_handler( + HCI_EVENT_PACKET, 0, packet, + static_cast(packet_size)); +} + +void require_identity(const ControllerIdentity& actual, bool stable, + ControllerTransport transport, uint8_t address_type, + const bd_addr_t address, uint16_t vendor_id, + uint16_t product_id, const char* message) { + ControllerIdentity expected{}; + expected.stable = stable; + expected.transport = transport; + expected.address_type = address_type; + memcpy(expected.address, address, sizeof(expected.address)); + expected.vendor_id = vendor_id; + expected.product_id = product_id; + require(controller_identity_equal(actual, expected), message); +} + +void test_identity_encoding_contract() { + ControllerIdentity identity{}; + identity.stable = true; + identity.transport = ControllerTransport::kBle; + identity.address_type = BD_ADDR_TYPE_LE_RANDOM_IDENTITY; + const bd_addr_t address = {0x10, 0x11, 0x12, 0x13, 0x14, 0x15}; + memcpy(identity.address, address, sizeof(address)); + identity.vendor_id = 0x1234; + identity.product_id = 0xabcd; + + uint8_t encoded[CONTROLLER_IDENTITY_ENCODED_SIZE]{}; + const uint8_t expected[CONTROLLER_IDENTITY_ENCODED_SIZE] = { + 1, 2, 3, 0, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, + 0x34, 0x12, 0xcd, 0xab}; + require(controller_identity_encode(identity, encoded, sizeof(encoded)) && + memcmp(encoded, expected, sizeof(expected)) == 0, + "controller identity wire encoding changed"); + + ControllerIdentity decoded{}; + require(controller_identity_decode(encoded, sizeof(encoded), &decoded) && + controller_identity_equal(identity, decoded), + "controller identity wire round trip failed"); + decoded.product_id ^= 1; + require(!controller_identity_equal(identity, decoded), + "controller identity equality must include every field"); + encoded[3] = 1; + require(!controller_identity_decode(encoded, sizeof(encoded), &decoded), + "controller identity decoder must reject a nonzero reserved byte"); + + const ControllerIdentity global = controller_identity_global(); + memset(encoded, 0xff, sizeof(encoded)); + require(controller_identity_is_global(global) && + controller_identity_encode(global, encoded, + sizeof(encoded)), + "global identity helper must produce an encodable fallback"); + for (uint8_t byte : encoded) { + require(byte == 0, + "global fallback identity must encode as all zeroes"); + } +} + @@ -376,7 +640,7 @@ void test_ready_order(bool reverse) { for (int ready = 0; ready <= position; ++ready) { expected_active = expected_active || order[ready] == candidate; } - require(bluepad32_input_backend_snapshot(candidate, &snapshot) == + require(read_controller_state(candidate, &snapshot) == expected_active, "only ready indexed slots may become active"); } @@ -403,7 +667,7 @@ void test_ready_order(bool reverse) { for (int candidate = 0; candidate < kSlotCount; ++candidate) { ControllerState snapshot{}; - require(bluepad32_input_backend_snapshot(candidate, &snapshot) == + require(read_controller_state(candidate, &snapshot) == (candidate != slot), "disconnect must preserve every surviving slot"); } @@ -444,7 +708,7 @@ void test_rejections() { collision_data.gamepad.buttons = BUTTON_B; platform_on_controller_data(&collision, &collision_data); ControllerState snapshot{}; - require(bluepad32_input_backend_snapshot(0, &snapshot), + require(read_controller_state(0, &snapshot), "occupied slot must stay active"); require(!snapshot.button_east, "mismatched device input must not enter the occupied slot"); @@ -453,13 +717,13 @@ void test_rejections() { slot_zero_data.klass = UNI_CONTROLLER_CLASS_GAMEPAD; slot_zero_data.gamepad.accel[0] = 8192; platform_on_controller_data(&slot_zero, &slot_zero_data); - require(bluepad32_input_backend_snapshot(0, &snapshot) && + require(read_controller_state(0, &snapshot) && snapshot.motion_sample_count == 3, "valid slot input must remain observable"); - require(!bluepad32_input_backend_snapshot(4, &snapshot), + require(!read_controller_state(4, &snapshot), "public snapshot must reject slot 4"); bluepad32_input_backend_report_sent(4); - require(bluepad32_input_backend_snapshot(0, &snapshot) && + require(read_controller_state(0, &snapshot) && snapshot.motion_sample_count == 3, "slot 4 acknowledgement must not consume slot 0 IMU"); bluepad32_input_backend_queue_rumble(4, ControllerRumbleOutput{1, 2}); @@ -469,6 +733,7 @@ void test_rejections() { } void test_independent_lifecycle() { + test_identity_encoding_contract(); start_pairing_backend(); uni_hid_device_t aborted = device(0); @@ -500,7 +765,40 @@ void test_independent_lifecycle() { "pre-ready disconnect must restart Classic and BLE scans"); uni_hid_device_t devices[kSlotCount] = { - device(0), device(1), device(2), device(3)}; + device(0, true, UNI_BT_CONN_PROTOCOL_BR_EDR), + device(1, true, UNI_BT_CONN_PROTOCOL_BLE), + device(2, true, UNI_BT_CONN_PROTOCOL_BLE), + device(3, true, UNI_BT_CONN_PROTOCOL_BLE)}; + const bd_addr_t classic_address = + {0x10, 0x11, 0x12, 0x13, 0x14, 0x15}; + const bd_addr_t resolved_connection_address = + {0x41, 0x21, 0x22, 0x23, 0x24, 0x25}; + const bd_addr_t created_connection_address = + {0x42, 0x31, 0x32, 0x33, 0x34, 0x35}; + const bd_addr_t reencrypted_connection_address = + {0x43, 0x41, 0x42, 0x43, 0x44, 0x45}; + memcpy(devices[0].conn.btaddr, classic_address, + sizeof(classic_address)); + memcpy(devices[1].conn.btaddr, resolved_connection_address, + sizeof(resolved_connection_address)); + memcpy(devices[2].conn.btaddr, created_connection_address, + sizeof(created_connection_address)); + memcpy(devices[3].conn.btaddr, reencrypted_connection_address, + sizeof(reencrypted_connection_address)); + const bd_addr_t resolved_address = + {0x20, 0x21, 0x22, 0x23, 0x24, 0x25}; + const bd_addr_t reencrypted_address = + {0x30, 0x31, 0x32, 0x33, 0x34, 0x35}; + dispatch_identity_event( + SM_EVENT_IDENTITY_RESOLVING_SUCCEEDED, devices[1], + BD_ADDR_TYPE_LE_PUBLIC, resolved_address); + register_lookup_device(&devices[3]); + dispatch_identity_event( + SM_EVENT_REENCRYPTION_STARTED, devices[3], + BD_ADDR_TYPE_LE_RANDOM, reencrypted_address); + dispatch_identity_event( + SM_EVENT_REENCRYPTION_COMPLETE, devices[3], + BD_ADDR_TYPE_LE_RANDOM, reencrypted_address); for (int slot = 0; slot < kSlotCount; ++slot) { platform_on_device_connected(&devices[slot]); require(g_slots[slot].device == &devices[slot] && @@ -539,6 +837,73 @@ void test_independent_lifecycle() { !scanning_enabled && !incoming_connections, "four ready lifecycle devices must stop connection policy"); + Bluepad32SlotSnapshot lifecycle_snapshots[kSlotCount]{}; + uint32_t baseline_connection_generations[kSlotCount]{}; + for (int slot = 0; slot < kSlotCount; ++slot) { + bluepad32_input_backend_snapshot( + static_cast(slot), &lifecycle_snapshots[slot]); + require(lifecycle_snapshots[slot].active, + "ready slot snapshot must publish active state"); + baseline_connection_generations[slot] = + lifecycle_snapshots[slot].connection_generation; + } + require_identity( + lifecycle_snapshots[0].identity, true, + ControllerTransport::kClassic, BD_ADDR_TYPE_UNKNOWN, + devices[0].conn.btaddr, devices[0].vendor_id, + devices[0].product_id, + "Classic snapshot identity must use the connected device address"); + require_identity( + lifecycle_snapshots[1].identity, true, + ControllerTransport::kBle, BD_ADDR_TYPE_LE_PUBLIC, + resolved_address, devices[1].vendor_id, devices[1].product_id, + "resolved BLE snapshot must use the stable identity address"); + require(controller_identity_is_global( + lifecycle_snapshots[2].identity), + "unresolved BLE snapshot must use the global unstable identity"); + require_identity( + lifecycle_snapshots[3].identity, true, + ControllerTransport::kBle, BD_ADDR_TYPE_LE_RANDOM, + reencrypted_address, devices[3].vendor_id, + devices[3].product_id, + "reencrypted BLE snapshot must use the bonded identity address"); + require(observed_profile_identity_count == 3 && + controller_identity_equal( + observed_profile_identities[0], + lifecycle_snapshots[1].identity) && + controller_identity_equal( + observed_profile_identities[1], + lifecycle_snapshots[3].identity) && + controller_identity_equal( + observed_profile_identities[2], + lifecycle_snapshots[0].identity), + "only stable ready identities must be enrolled for profiles"); + require(baseline_connection_generations[0] == + first_pending_generation + 1 && + baseline_connection_generations[1] == + baseline_connection_generations[2] && + baseline_connection_generations[2] == + baseline_connection_generations[3], + "connection generations must isolate each slot lifecycle"); + + const bd_addr_t created_address = + {0x40, 0x41, 0x42, 0x43, 0x44, 0x45}; + register_lookup_device(&devices[2]); + dispatch_identity_event( + SM_EVENT_IDENTITY_CREATED, devices[2], + BD_ADDR_TYPE_LE_RANDOM, created_address); + bluepad32_input_backend_snapshot(2, &lifecycle_snapshots[2]); + require_identity( + lifecycle_snapshots[2].identity, true, + ControllerTransport::kBle, BD_ADDR_TYPE_LE_RANDOM, + created_address, devices[2].vendor_id, devices[2].product_id, + "new BLE identity event must update an active slot snapshot"); + require(observed_profile_identity_count == 4 && + controller_identity_equal( + observed_profile_identities[3], + lifecycle_snapshots[2].identity), + "late BLE identity creation must enroll the stable identity"); + const uint32_t buttons[kSlotCount] = { BUTTON_B, BUTTON_A, BUTTON_X, BUTTON_Y}; uni_controller_t data[kSlotCount]{}; @@ -552,7 +917,7 @@ void test_independent_lifecycle() { ControllerState states[kSlotCount]{}; for (int slot = 0; slot < kSlotCount; ++slot) { - require(bluepad32_input_backend_snapshot(slot, &states[slot]) && + require(read_controller_state(slot, &states[slot]) && states[slot].motion_sample_count == 3, "every slot must expose independent input and IMU"); } @@ -571,13 +936,13 @@ void test_independent_lifecycle() { bluepad32_input_backend_report_sent(3); for (int slot = 0; slot < kSlotCount; ++slot) { - require(bluepad32_input_backend_snapshot(slot, &states[slot]) && + require(read_controller_state(slot, &states[slot]) && states[slot].motion_sample_count == (slot == 3 ? 0 : 3), "slot 3 acknowledgement must not consume slots 0-2 IMU"); } for (int slot = 0; slot < 3; ++slot) { bluepad32_input_backend_report_sent(slot); - require(bluepad32_input_backend_snapshot(slot, &states[slot]) && + require(read_controller_state(slot, &states[slot]) && states[slot].motion_sample_count == 0, "each slot acknowledgement must consume only its own IMU"); } @@ -605,24 +970,43 @@ void test_independent_lifecycle() { bluepad32_input_backend_queue_rumble(3, ControllerRumbleOutput{55, 66}); const uint32_t disconnected_generation = - g_slots[3].connection_generation; + baseline_connection_generations[3]; const int starts_before_slot_three_disconnect = scan_starts; platform_on_device_disconnected(&devices[3]); require(scan_starts == starts_before_slot_three_disconnect + 1 && scanning_enabled && incoming_connections, "slot 3 disconnect must resume scanning and incoming connections"); - require(!bluepad32_input_backend_snapshot(3, &states[3]) && + require(!read_controller_state(3, &states[3]) && !states[3].button_north && states[3].left_stick_x == 0, "slot 3 disconnect must publish protocol-neutral state"); - require(bluepad32_input_backend_snapshot(0, &states[0]) && + bluepad32_input_backend_snapshot(3, &lifecycle_snapshots[3]); + require(!lifecycle_snapshots[3].active && + lifecycle_snapshots[3].connection_generation == + disconnected_generation + 1 && + controller_identity_is_global( + lifecycle_snapshots[3].identity) && + !lifecycle_snapshots[3].state.button_north && + lifecycle_snapshots[3].state.left_stick_x == 0, + "disconnect snapshot must atomically publish neutral state, " + "cleared identity, and a new connection generation"); + for (int survivor = 0; survivor < 3; ++survivor) { + bluepad32_input_backend_snapshot( + static_cast(survivor), + &lifecycle_snapshots[survivor]); + require(lifecycle_snapshots[survivor].active && + lifecycle_snapshots[survivor].connection_generation == + baseline_connection_generations[survivor], + "disconnect generation must not leak into surviving slots"); + } + require(read_controller_state(0, &states[0]) && states[0].button_east && - bluepad32_input_backend_snapshot(1, &states[1]) && + read_controller_state(1, &states[1]) && states[1].button_south && - bluepad32_input_backend_snapshot(2, &states[2]) && + read_controller_state(2, &states[2]) && states[2].button_west, "slot 3 disconnect must preserve slots 0-2"); platform_on_controller_data(&devices[0], &data[0]); - require(bluepad32_input_backend_snapshot(0, &states[0]) && + require(read_controller_state(0, &states[0]) && states[0].button_east, "slot 0 input must continue while slot 3 is disconnected"); const int slot_zero_calls_while_scanning = devices[0].rumble_calls; @@ -633,13 +1017,26 @@ void test_independent_lifecycle() { devices[0].last_high == 116, "slot 0 rumble must continue while slot 3 is disconnected"); - uni_hid_device_t slot_three_replacement = device(3); + uni_hid_device_t slot_three_replacement = + device(3, true, UNI_BT_CONN_PROTOCOL_BLE); + memcpy(slot_three_replacement.conn.btaddr, + devices[3].conn.btaddr, sizeof(devices[3].conn.btaddr)); require(platform_on_device_ready(&slot_three_replacement) == UNI_ERROR_SUCCESS, "slot 3 replacement must bind to the freed indexed slot"); process_rumble_timer(&g_rumble_timer); require(slot_three_replacement.rumble_calls == 0, "slot 3 replacement must not receive disconnected device rumble"); + bluepad32_input_backend_snapshot(3, &lifecycle_snapshots[3]); + require(lifecycle_snapshots[3].active && + lifecycle_snapshots[3].connection_generation == + disconnected_generation + 1 && + controller_identity_is_global( + lifecycle_snapshots[3].identity), + "replacement must keep the new generation and cannot inherit " + "the disconnected BLE identity"); + require(observed_profile_identity_count == 4, + "unstable replacement must not be enrolled for profiles"); g_slots[3].pending_rumble = { 3, disconnected_generation, ControllerRumbleOutput{77, 88}}; @@ -653,14 +1050,14 @@ void test_independent_lifecycle() { replacement_data.gamepad.buttons = BUTTON_Y; replacement_data.gamepad.accel[0] = 9000; platform_on_controller_data(&slot_three_replacement, &replacement_data); - require(bluepad32_input_backend_snapshot(3, &states[3]) && + require(read_controller_state(3, &states[3]) && states[3].button_north && states[3].motion_sample_count == 3, "replacement input and IMU must populate only slot 3"); - require(bluepad32_input_backend_snapshot(0, &states[0]) && + require(read_controller_state(0, &states[0]) && states[0].button_east && - bluepad32_input_backend_snapshot(1, &states[1]) && + read_controller_state(1, &states[1]) && states[1].button_south && - bluepad32_input_backend_snapshot(2, &states[2]) && + read_controller_state(2, &states[2]) && states[2].button_west, "slot 3 replacement must not disturb slots 0-2"); @@ -706,15 +1103,14 @@ void test_independent_lifecycle() { require(scan_starts == starts_before_disconnect + 1 && scanning_enabled && incoming_connections, "disconnecting slots 0-2 must resume connection policy"); - require(!bluepad32_input_backend_snapshot(slot, &states[slot]) && + require(!read_controller_state(slot, &states[slot]) && states[slot].left_stick_x == 0, "disconnect must publish protocol-neutral state"); for (int survivor = 0; survivor < kSlotCount; ++survivor) { if (survivor == slot) { continue; } - require(bluepad32_input_backend_snapshot(survivor, - &states[survivor]), + require(read_controller_state(survivor, &states[survivor]), "disconnect must preserve all three survivors"); } require(platform_on_device_ready(&replacements[slot]) == @@ -898,7 +1294,7 @@ void test_abxy_hotkey() { input.gamepad.buttons = BUTTON_A; platform_on_controller_data(&slot_zero, &input); ControllerState snapshot{}; - require(bluepad32_input_backend_snapshot(0, &snapshot), + require(read_controller_state(0, &snapshot), "slot 0 ABXY state was not published"); require_south_button_mapping( snapshot, kDefaultSwapAbxy, @@ -908,7 +1304,7 @@ void test_abxy_hotkey() { kAbxyHotkeyButtonMask | BUTTON_A; input.gamepad.misc_buttons = kAbxyHotkeyMiscMask; platform_on_controller_data(&slot_zero, &input); - require(bluepad32_input_backend_snapshot(0, &snapshot), + require(read_controller_state(0, &snapshot), "toggled slot 0 state was not published"); require_south_button_mapping( snapshot, !kDefaultSwapAbxy, @@ -960,7 +1356,7 @@ void test_abxy_hotkey() { peer_input.klass = UNI_CONTROLLER_CLASS_GAMEPAD; peer_input.gamepad.buttons = BUTTON_A; platform_on_controller_data(&slot_one, &peer_input); - require(bluepad32_input_backend_snapshot(1, &snapshot), + require(read_controller_state(1, &snapshot), "slot 1 ABXY state was not published"); require_south_button_mapping( snapshot, kDefaultSwapAbxy, @@ -988,7 +1384,7 @@ void test_motion_hotkey() { input.gamepad.accel[0] = 8192; platform_on_controller_data(&slot_zero, &input); ControllerState snapshot{}; - require(bluepad32_input_backend_snapshot(0, &snapshot) && + require(read_controller_state(0, &snapshot) && snapshot.motion_sample_count == (kDefaultMotionEnabled ? 3 : 0), "slot 0 did not start with configured motion state"); @@ -997,7 +1393,7 @@ void test_motion_hotkey() { input.gamepad.buttons = kMotionHotkeyButtonMask; input.gamepad.misc_buttons = kMotionHotkeyMiscMask; platform_on_controller_data(&slot_zero, &input); - require(bluepad32_input_backend_snapshot(0, &snapshot) && + require(read_controller_state(0, &snapshot) && snapshot.motion_sample_count == (kDefaultMotionEnabled ? 0 : 3) && !snapshot.dpad_up && !snapshot.button_right_shoulder && @@ -1030,7 +1426,7 @@ void test_motion_hotkey() { peer_input.klass = UNI_CONTROLLER_CLASS_GAMEPAD; peer_input.gamepad.accel[0] = 8192; platform_on_controller_data(&slot_one, &peer_input); - require(bluepad32_input_backend_snapshot(1, &snapshot) && + require(read_controller_state(1, &snapshot) && snapshot.motion_sample_count == (kDefaultMotionEnabled ? 3 : 0), "slot 0 motion chord changed slot 1 motion state"); @@ -1042,7 +1438,7 @@ void test_motion_hotkey() { input.gamepad.buttons = kMotionHotkeyButtonMask; input.gamepad.misc_buttons = kMotionHotkeyMiscMask; platform_on_controller_data(&slot_zero, &input); - require(bluepad32_input_backend_snapshot(0, &snapshot) && + require(read_controller_state(0, &snapshot) && snapshot.motion_sample_count == (kDefaultMotionEnabled ? 3 : 0), "released motion chord did not re-arm or restore motion"); @@ -1078,7 +1474,7 @@ void test_protocol_neutral_analog_state() { platform_on_controller_data(&controller, &input); ControllerState state{}; - require(bluepad32_input_backend_snapshot(0, &state), + require(read_controller_state(0, &state), "analog state was not published"); require(state.left_stick_x == INT16_MIN && state.left_stick_y == 0 && @@ -1096,7 +1492,7 @@ void test_protocol_neutral_analog_state() { input.gamepad.buttons = BUTTON_TRIGGER_L | BUTTON_TRIGGER_R; platform_on_controller_data(&controller, &input); - require(bluepad32_input_backend_snapshot(0, &state) && + require(read_controller_state(0, &state) && state.left_trigger == UINT16_MAX && state.right_trigger == UINT16_MAX, "digital trigger buttons did not map to full analog range"); diff --git a/tests/bluepad32_native_stubs/uni.h b/tests/bluepad32_native_stubs/uni.h index 7ee86a8..5fdc72e 100644 --- a/tests/bluepad32_native_stubs/uni.h +++ b/tests/bluepad32_native_stubs/uni.h @@ -6,6 +6,7 @@ typedef uint8_t bd_addr_t[6]; typedef uint8_t link_key_t[16]; typedef uint8_t sm_key_t[16]; typedef int link_key_type_t; +typedef uint16_t hci_con_handle_t; enum bd_addr_type_t { BD_ADDR_TYPE_LE_PUBLIC = 0, @@ -29,6 +30,12 @@ enum { HCI_EVENT_PACKET = 4, HCI_EVENT_USER_CONFIRMATION_REQUEST = 0x33, HCI_EVENT_USER_PASSKEY_REQUEST = 0x34, + SM_EVENT_IDENTITY_RESOLVING_STARTED = 0xcd, + SM_EVENT_IDENTITY_RESOLVING_FAILED = 0xce, + SM_EVENT_IDENTITY_RESOLVING_SUCCEEDED = 0xcf, + SM_EVENT_IDENTITY_CREATED = 0xd3, + SM_EVENT_REENCRYPTION_STARTED = 0xd6, + SM_EVENT_REENCRYPTION_COMPLETE = 0xd7, SM_STK_GENERATION_METHOD_JUST_WORKS = 0x01, SM_STK_GENERATION_METHOD_OOB = 0x02, SM_STK_GENERATION_METHOD_PASSKEY = 0x04, @@ -114,10 +121,13 @@ enum uni_bt_conn_protocol_t { struct uni_bt_conn_t { bd_addr_t btaddr; + hci_con_handle_t handle; uni_bt_conn_protocol_t protocol; }; struct uni_hid_device_t { + uint16_t vendor_id; + uint16_t product_id; uni_bt_conn_t conn; int idx; bool gamepad; @@ -154,6 +164,8 @@ struct uni_platform { bool uni_hid_device_is_gamepad(const uni_hid_device_t* device); int uni_hid_device_get_idx_for_instance(const uni_hid_device_t* device); void uni_hid_device_disconnect(uni_hid_device_t* device); +uni_hid_device_t* uni_hid_device_get_instance_for_connection_handle( + hci_con_handle_t handle); void uni_bt_allow_incoming_connections(bool enabled); void uni_bt_start_scanning_and_autoconnect_unsafe(); void uni_bt_stop_scanning_unsafe(); @@ -181,10 +193,48 @@ int gap_ssp_passkey_response(const bd_addr_t address, uint32_t passkey); int gap_ssp_passkey_negative(const bd_addr_t address); void hci_add_event_handler( btstack_packet_callback_registration_t* callback_handler); +void sm_add_event_handler( + btstack_packet_callback_registration_t* callback_handler); uint8_t hci_event_packet_get_type(const uint8_t* packet); void hci_event_user_confirmation_request_get_bd_addr( const uint8_t* packet, bd_addr_t address); void hci_event_user_passkey_request_get_bd_addr( const uint8_t* packet, bd_addr_t address); +hci_con_handle_t sm_event_identity_resolving_started_get_handle( + const uint8_t* packet); +hci_con_handle_t sm_event_identity_resolving_failed_get_handle( + const uint8_t* packet); +hci_con_handle_t sm_event_identity_resolving_succeeded_get_handle( + const uint8_t* packet); +uint8_t sm_event_identity_resolving_succeeded_get_addr_type( + const uint8_t* packet); +void sm_event_identity_resolving_succeeded_get_address( + const uint8_t* packet, bd_addr_t address); +uint8_t sm_event_identity_resolving_succeeded_get_identity_addr_type( + const uint8_t* packet); +void sm_event_identity_resolving_succeeded_get_identity_address( + const uint8_t* packet, bd_addr_t address); +hci_con_handle_t sm_event_identity_created_get_handle( + const uint8_t* packet); +void sm_event_identity_created_get_address( + const uint8_t* packet, bd_addr_t address); +uint8_t sm_event_identity_created_get_identity_addr_type( + const uint8_t* packet); +void sm_event_identity_created_get_identity_address( + const uint8_t* packet, bd_addr_t address); +hci_con_handle_t sm_event_reencryption_started_get_handle( + const uint8_t* packet); +uint8_t sm_event_reencryption_started_get_addr_type( + const uint8_t* packet); +void sm_event_reencryption_started_get_address( + const uint8_t* packet, bd_addr_t address); +hci_con_handle_t sm_event_reencryption_complete_get_handle( + const uint8_t* packet); +uint8_t sm_event_reencryption_complete_get_addr_type( + const uint8_t* packet); +void sm_event_reencryption_complete_get_address( + const uint8_t* packet, bd_addr_t address); +uint8_t sm_event_reencryption_complete_get_status( + const uint8_t* packet); void uni_platform_set_custom(uni_platform* platform); int uni_init(int argc, const char** argv); diff --git a/tests/controller_profile_test.cpp b/tests/controller_profile_test.cpp new file mode 100644 index 0000000..a235a26 --- /dev/null +++ b/tests/controller_profile_test.cpp @@ -0,0 +1,162 @@ +#include "controller_identity.h" +#include "controller_profile.h" + +#include +#include +#include + +namespace { + +uint8_t encoded_database[CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE]{}; +ControllerProfileDatabase database{}; +ControllerProfileDatabase decoded_database{}; + +void require(bool condition, const char* message) { + if (!condition) { + std::cerr << message << '\n'; + std::exit(1); + } +} + +ControllerIdentity identity(uint8_t suffix) { + ControllerIdentity value{}; + value.stable = true; + value.transport = ControllerTransport::kClassic; + value.address[5] = suffix; + value.vendor_id = 0x057e; + value.product_id = static_cast(0x2000u + suffix); + return value; +} + +bool read_encoded_database(void*, size_t offset, uint8_t* output, + size_t size) { + if (offset > sizeof(encoded_database) || + size > sizeof(encoded_database) - offset) { + return false; + } + memcpy(output, &encoded_database[offset], size); + return true; +} + +void test_profile_wire_schema() { + const ControllerProfile profile = + controller_profile_default(controller_identity_global(), 0); + uint8_t encoded[CONTROLLER_PROFILE_ENCODED_SIZE]{}; + require(controller_profile_encode(profile, encoded, sizeof(encoded)), + "default profile did not encode"); + require(encoded[0] == 1 && encoded[1] == 0 && + encoded[2] == 0 && encoded[3] == 1, + "profile header is not little-endian v1/256"); + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_LOGICAL_BUTTON_COUNT; ++index) { + require(encoded[4 + index] == index, + "default direct mapping is not identity"); + } + require(encoded[26] == 0xff && encoded[27] == 0x7f && + encoded[30] == 0, + "default stick encoding changed"); + require(encoded[54] == 0xff && encoded[55] == 0xff && + encoded[58] == 0x00 && encoded[59] == 0x80, + "default trigger encoding changed"); + require(encoded[72] == 0xff && encoded[73] == 0xff && + encoded[74] == 3 && encoded[78] == 0xff && + encoded[79] == 0xff && encoded[80] == 1, + "default rumble or macro encoding changed"); + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_MACRO_STEP_CAPACITY; ++index) { + require(encoded[100 + index * 19] == 1, + "unused macro step is not canonical end"); + } + + ControllerProfile decoded{}; + require(controller_profile_decode(encoded, sizeof(encoded), &decoded), + "default profile did not decode"); + encoded[252] = 1; + require(!controller_profile_decode(encoded, sizeof(encoded), &decoded), + "nonzero reserved profile byte was accepted"); + + ControllerProfile invalid = profile; + invalid.button_map[0] = 16; + require(!controller_profile_validate(invalid), + "invalid direct output was accepted"); + invalid = profile; + invalid.sticks[0].inner_deadzone = + invalid.sticks[0].outer_saturation; + require(!controller_profile_validate(invalid), + "empty stick range was accepted"); + invalid = profile; + invalid.triggers[0].digital_threshold = 0; + invalid.triggers[0].lower_deadzone = 1; + require(!controller_profile_validate(invalid), + "trigger threshold outside its range was accepted"); + invalid = profile; + invalid.turbo_modes[0] = + static_cast(3); + require(!controller_profile_validate(invalid), + "invalid Turbo mode was accepted"); + invalid = profile; + invalid.macro_step_count = 2; + invalid.macro_steps[0].type = + ControllerProfileMacroStepType::kState; + invalid.macro_steps[0].duration_ms = + CONTROLLER_PROFILE_MAX_WAIT_MS + 1; + require(!controller_profile_validate(invalid), + "unbounded macro wait was accepted"); + invalid.macro_steps[0].duration_ms = + CONTROLLER_PROFILE_MAX_WAIT_MS; + invalid.macro_steps[1].type = + ControllerProfileMacroStepType::kState; + require(!controller_profile_validate(invalid), + "macro without a final end was accepted"); +} + +void test_database_round_trip_and_capacity() { + controller_profile_database_default(&database); + for (uint8_t index = 0; + index < CONTROLLER_PROFILE_STABLE_IDENTITY_CAPACITY; ++index) { + ControllerProfileDatabaseEntry* entry = nullptr; + require(controller_profile_database_ensure( + &database, identity(static_cast(index + 1)), + &entry) == ControllerProfileDatabaseResult::kOk && + entry != nullptr, + "stable identity was not added"); + entry->active_profile = index % CONTROLLER_PROFILE_COUNT; + } + ControllerProfileDatabaseEntry* rejected = nullptr; + require(controller_profile_database_ensure( + &database, identity(99), &rejected) == + ControllerProfileDatabaseResult::kFull, + "seventeenth stable identity was not rejected"); + require(controller_profile_database_find(database, identity(1)) != + nullptr, + "full-table rejection evicted an existing identity"); + + for (size_t offset = 0; offset < sizeof(encoded_database); + offset += CONTROLLER_PROFILE_ENCODED_SIZE) { + const size_t size = sizeof(encoded_database) - offset < + CONTROLLER_PROFILE_ENCODED_SIZE + ? sizeof(encoded_database) - offset + : CONTROLLER_PROFILE_ENCODED_SIZE; + require(controller_profile_database_encode_range( + database, offset, &encoded_database[offset], size), + "database range did not encode"); + } + require(controller_profile_database_decode( + read_encoded_database, nullptr, &decoded_database), + "database did not decode"); + require(controller_profile_database_find( + decoded_database, identity(16)) != nullptr, + "last database identity did not round trip"); + + encoded_database[12] = 1; + require(!controller_profile_database_decode( + read_encoded_database, nullptr, &decoded_database), + "nonzero database header reservation was accepted"); +} + +} // namespace +int main() { + test_profile_wire_schema(); + test_database_round_trip_and_capacity(); + return 0; +} diff --git a/tests/profile_service_test.cpp b/tests/profile_service_test.cpp new file mode 100644 index 0000000..0239a39 --- /dev/null +++ b/tests/profile_service_test.cpp @@ -0,0 +1,186 @@ +#include "controller_identity.h" +#include "controller_profile.h" +#include "pico_profile_storage.h" +#include "profile_service.h" +#include "profile_storage.h" + +#include +#include +#include + +namespace { + +struct FakeFlash { + uint8_t bytes[PROFILE_STORAGE_BANK_COUNT][PROFILE_STORAGE_BANK_SIZE]; +}; + +FakeFlash flash{}; + +void require(bool condition, const char* message) { + if (!condition) { + std::cerr << message << '\n'; + std::exit(1); + } +} + +bool fake_read(void* context, uint8_t bank, size_t offset, + uint8_t* output, size_t size) { + auto* storage = static_cast(context); + if (bank >= PROFILE_STORAGE_BANK_COUNT || output == nullptr || + offset > PROFILE_STORAGE_BANK_SIZE || + size > PROFILE_STORAGE_BANK_SIZE - offset) { + return false; + } + memcpy(output, &storage->bytes[bank][offset], size); + return true; +} + +bool fake_erase_sector(void* context, uint8_t bank, size_t offset) { + auto* storage = static_cast(context); + if (bank >= PROFILE_STORAGE_BANK_COUNT || + offset % PROFILE_STORAGE_SECTOR_SIZE != 0 || + offset > PROFILE_STORAGE_BANK_SIZE || + PROFILE_STORAGE_SECTOR_SIZE > PROFILE_STORAGE_BANK_SIZE - offset) { + return false; + } + memset(&storage->bytes[bank][offset], 0xff, + PROFILE_STORAGE_SECTOR_SIZE); + return true; +} + +bool fake_program(void* context, uint8_t bank, size_t offset, + const uint8_t* data, size_t size) { + auto* storage = static_cast(context); + if (bank >= PROFILE_STORAGE_BANK_COUNT || data == nullptr || + size != PROFILE_STORAGE_PAGE_SIZE || + offset % PROFILE_STORAGE_PAGE_SIZE != 0 || + offset > PROFILE_STORAGE_BANK_SIZE || + size > PROFILE_STORAGE_BANK_SIZE - offset) { + return false; + } + for (size_t index = 0; index < size; ++index) { + storage->bytes[bank][offset + index] &= data[index]; + } + return true; +} + +ProfileStorageIo fake_io() { + return { + &flash, + PROFILE_STORAGE_BANK_SIZE, + PROFILE_STORAGE_SECTOR_SIZE, + PROFILE_STORAGE_PAGE_SIZE, + fake_read, + fake_erase_sector, + fake_program, + }; +} + +ProfileServiceTransactionSnapshot transaction_snapshot() { + ProfileServiceTransactionSnapshot snapshot{}; + profile_service_transaction_snapshot(&snapshot); + return snapshot; +} + +ControllerProfileDatabase reload_database( + const ProfileServiceTransactionSnapshot& transaction, + uint32_t expected_generation) { + ControllerProfileDatabase recovered{}; + ProfileStorage storage; + require(storage.initialize(fake_io(), &recovered) && + storage.snapshot().valid && + storage.snapshot().generation == expected_generation && + storage.snapshot().generation == + transaction.transaction.stored_generation && + storage.snapshot().payload_crc == + transaction.transaction.stored_crc, + "terminal transaction status did not identify persisted storage"); + return recovered; +} + +void test_pending_commands_are_not_decoded_as_profile_writes() { + memset(flash.bytes, 0xff, sizeof(flash.bytes)); + profile_service_prepare(); + profile_service_initialize_on_storage_core(); + + const ControllerIdentity identity = controller_identity_global(); + constexpr uint8_t kProfileIndex = 2; + ControllerProfile customized = + controller_profile_default(identity, kProfileIndex); + customized.strong_rumble_scale = 17; + uint8_t encoded[CONTROLLER_PROFILE_ENCODED_SIZE]{}; + require(controller_profile_encode(customized, encoded, sizeof(encoded)), + "customized profile did not encode"); + + constexpr uint32_t kWriteTransactionId = 0x10203040; + require(profile_service_begin( + kWriteTransactionId, identity, kProfileIndex, + CONTROLLER_PROFILE_SCHEMA_VERSION, sizeof(encoded), + profile_storage_crc32(encoded, sizeof(encoded))) == + ConfigurationTransactionStatus::kReceiving && + profile_service_append(kWriteTransactionId, 0, encoded, + sizeof(encoded)) == + ConfigurationTransactionStatus::kReceiving && + profile_service_commit(kWriteTransactionId) == + ConfigurationTransactionStatus::kPending, + "profile write did not reach pending"); + profile_service_task_on_storage_core(0); + require(transaction_snapshot().transaction.status == + ConfigurationTransactionStatus::kCommitted, + "profile write baseline did not commit"); + + constexpr uint32_t kResetTransactionId = 0xa5a55a5a; + require(profile_service_reset(kResetTransactionId, identity, + kProfileIndex) == + ConfigurationTransactionStatus::kPending, + "profile reset did not reach pending"); + ProfileServiceTransactionSnapshot reset = transaction_snapshot(); + require(reset.transaction.transaction_id == kResetTransactionId && + reset.transaction.status == + ConfigurationTransactionStatus::kPending, + "pending reset lost its transaction identity"); + + profile_service_task_on_storage_core(1000); + reset = transaction_snapshot(); + require(reset.transaction.transaction_id == kResetTransactionId && + reset.transaction.status == + ConfigurationTransactionStatus::kCommitted, + "one reset tick decoded profile payload or published a malformed result"); + ControllerProfileDatabase recovered = reload_database(reset, 2); + require(recovered.fallback_profiles[kProfileIndex].strong_rumble_scale == + UINT8_MAX, + "terminal reset status was published before reset persisted"); + + constexpr uint32_t kActivateTransactionId = 0x50607080; + constexpr uint8_t kActivatedProfile = 3; + require(profile_service_activate(kActivateTransactionId, identity, + kActivatedProfile) == + ConfigurationTransactionStatus::kPending, + "profile activation did not reach pending"); + ProfileServiceTransactionSnapshot activate = transaction_snapshot(); + require(activate.transaction.transaction_id == kActivateTransactionId && + activate.transaction.status == + ConfigurationTransactionStatus::kPending, + "pending activation lost its transaction identity"); + + profile_service_task_on_storage_core(2000); + activate = transaction_snapshot(); + require(activate.transaction.transaction_id == kActivateTransactionId && + activate.transaction.status == + ConfigurationTransactionStatus::kCommitted, + "one activation tick decoded profile payload or published a malformed result"); + recovered = reload_database(activate, 3); + require(recovered.fallback_active_profile == kActivatedProfile, + "terminal activation status was published before activation persisted"); +} + +} // namespace + +ProfileStorageIo pico_profile_storage_io() { + return fake_io(); +} + +int main() { + test_pending_commands_are_not_decoded_as_profile_writes(); + return 0; +} diff --git a/tests/profile_storage_test.cpp b/tests/profile_storage_test.cpp new file mode 100644 index 0000000..6301bb0 --- /dev/null +++ b/tests/profile_storage_test.cpp @@ -0,0 +1,235 @@ +#include "controller_identity.h" +#include "controller_profile.h" +#include "profile_storage.h" + +#include +#include +#include + +namespace { + +struct FakeFlash { + uint8_t bytes[PROFILE_STORAGE_BANK_COUNT][PROFILE_STORAGE_BANK_SIZE]; + int successful_programs = 0; + int fail_after_programs = -1; + bool corrupt_next_program = false; + bool fail_reads_after_header_program = false; + bool header_programmed = false; +}; + +FakeFlash flash{}; +ControllerProfileDatabase database{}; +ControllerProfileDatabase recovered_database{}; + +void require(bool condition, const char* message) { + if (!condition) { + std::cerr << message << '\n'; + std::exit(1); + } +} + +void erase_all() { + memset(flash.bytes, 0xff, sizeof(flash.bytes)); + flash.successful_programs = 0; + flash.fail_after_programs = -1; + flash.corrupt_next_program = false; + flash.fail_reads_after_header_program = false; + flash.header_programmed = false; +} + +bool fake_read(void* context, uint8_t bank, size_t offset, + uint8_t* output, size_t size) { + auto* storage = static_cast(context); + if (storage->fail_reads_after_header_program && + storage->header_programmed) { + return false; + } + if (bank >= PROFILE_STORAGE_BANK_COUNT || + offset > PROFILE_STORAGE_BANK_SIZE || + size > PROFILE_STORAGE_BANK_SIZE - offset) { + return false; + } + memcpy(output, &storage->bytes[bank][offset], size); + return true; +} + +bool fake_erase_sector(void* context, uint8_t bank, size_t offset) { + auto* storage = static_cast(context); + if (bank >= PROFILE_STORAGE_BANK_COUNT || + offset % PROFILE_STORAGE_SECTOR_SIZE != 0 || + offset > PROFILE_STORAGE_BANK_SIZE || + PROFILE_STORAGE_SECTOR_SIZE > + PROFILE_STORAGE_BANK_SIZE - offset) { + return false; + } + memset(&storage->bytes[bank][offset], 0xff, + PROFILE_STORAGE_SECTOR_SIZE); + return true; +} + +bool fake_program(void* context, uint8_t bank, size_t offset, + const uint8_t* data, size_t size) { + auto* storage = static_cast(context); + if (bank >= PROFILE_STORAGE_BANK_COUNT || data == nullptr || + size != PROFILE_STORAGE_PAGE_SIZE || + offset % PROFILE_STORAGE_PAGE_SIZE != 0 || + offset > PROFILE_STORAGE_BANK_SIZE || + size > PROFILE_STORAGE_BANK_SIZE - offset) { + return false; + } + if (storage->fail_after_programs >= 0 && + storage->successful_programs >= storage->fail_after_programs) { + return false; + } + for (size_t index = 0; index < size; ++index) { + storage->bytes[bank][offset + index] &= data[index]; + } + if (storage->corrupt_next_program) { + storage->bytes[bank][offset] ^= 1; + storage->corrupt_next_program = false; + } + if (offset == 0) { + storage->header_programmed = true; + } + ++storage->successful_programs; + return true; +} + +ProfileStorageIo fake_io() { + return { + &flash, + PROFILE_STORAGE_BANK_SIZE, + PROFILE_STORAGE_SECTOR_SIZE, + PROFILE_STORAGE_PAGE_SIZE, + fake_read, + fake_erase_sector, + fake_program, + }; +} + +void test_two_bank_recovery() { + erase_all(); + controller_profile_database_default(&database); + ProfileStorage storage; + require(storage.initialize(fake_io(), &database) && + !storage.snapshot().valid, + "erased profile storage did not initialize empty"); + require(storage.commit(database) == ProfileStorageResult::kOk && + storage.snapshot().generation == 1, + "first profile database did not commit"); + const int programs_after_first = flash.successful_programs; + require(storage.commit(database) == ProfileStorageResult::kUnchanged && + flash.successful_programs == programs_after_first, + "unchanged profile database consumed flash writes"); + + database.fallback_profiles[0].button_map[0] = 1; + require(storage.commit(database) == ProfileStorageResult::kOk && + storage.snapshot().generation == 2, + "second profile database generation did not commit"); + ProfileStorage reloaded; + require(reloaded.initialize(fake_io(), &recovered_database) && + reloaded.snapshot().generation == 2 && + recovered_database.fallback_profiles[0].button_map[0] == 1, + "latest profile database did not survive reload"); + + const uint8_t newest_bank = reloaded.snapshot().active_bank; + flash.bytes[newest_bank][PROFILE_STORAGE_RECORD_HEADER_SIZE + 4] ^= 1; + ProfileStorage after_corruption; + require(after_corruption.initialize(fake_io(), &recovered_database) && + after_corruption.snapshot().generation == 1 && + recovered_database.fallback_profiles[0].button_map[0] == 0, + "corrupt newest profile bank did not roll back"); +} + +void test_interrupted_commit_retains_previous_bank() { + erase_all(); + controller_profile_database_default(&database); + ProfileStorage storage; + require(storage.initialize(fake_io(), &database) && + storage.commit(database) == ProfileStorageResult::kOk, + "interruption baseline did not commit"); + database.fallback_profiles[1].button_map[2] = 3; + flash.fail_after_programs = flash.successful_programs + 1; + require(storage.commit(database) == ProfileStorageResult::kIoError, + "interrupted profile write reported success"); + + flash.fail_after_programs = -1; + ProfileStorage recovered; + require(recovered.initialize(fake_io(), &recovered_database) && + recovered.snapshot().generation == 1 && + recovered_database.fallback_profiles[1].button_map[2] == 2, + "interrupted profile write replaced previous bank"); +} + +void test_successful_header_program_is_commit_point() { + erase_all(); + controller_profile_database_default(&database); + ProfileStorage storage; + require(storage.initialize(fake_io(), &database) && + storage.commit(database) == ProfileStorageResult::kOk, + "commit-point baseline did not commit"); + + database.fallback_profiles[1].strong_rumble_scale = 17; + flash.header_programmed = false; + flash.fail_reads_after_header_program = true; + require(storage.commit(database) == ProfileStorageResult::kOk && + storage.snapshot().generation == 2, + "successful header program was rolled back by a later read"); + + flash.fail_reads_after_header_program = false; + ProfileStorage recovered; + require(recovered.initialize(fake_io(), &recovered_database) && + recovered.snapshot().generation == 2 && + recovered_database.fallback_profiles[1] + .strong_rumble_scale == 17, + "committed header did not recover after transient read failure"); +} + +void test_payload_corruption_prevents_header_publication() { + erase_all(); + controller_profile_database_default(&database); + ProfileStorage storage; + require(storage.initialize(fake_io(), &database) && + storage.commit(database) == ProfileStorageResult::kOk, + "corruption baseline did not commit"); + const ProfileStorageSnapshot previous = storage.snapshot(); + const uint8_t target_bank = previous.active_bank ^ 1u; + const int programs_before_corruption = flash.successful_programs; + constexpr int kPayloadProgramCount = + (CONTROLLER_PROFILE_DATABASE_ENCODED_SIZE + + PROFILE_STORAGE_PAGE_SIZE - 1) / + PROFILE_STORAGE_PAGE_SIZE; + + database.fallback_profiles[1].button_map[2] = 3; + flash.corrupt_next_program = true; + require(storage.commit(database) == ProfileStorageResult::kIoError && + flash.successful_programs == + programs_before_corruption + kPayloadProgramCount, + "corrupt payload programming reached the header program"); + for (size_t index = 0; index < PROFILE_STORAGE_RECORD_HEADER_SIZE; + ++index) { + require(flash.bytes[target_bank][index] == 0xff, + "rejected corrupt payload published a discoverable header"); + } + require(storage.snapshot().valid == previous.valid && + storage.snapshot().generation == previous.generation && + storage.snapshot().payload_crc == previous.payload_crc && + storage.snapshot().active_bank == previous.active_bank, + "rejected corrupt programming changed the storage snapshot"); + + ProfileStorage recovered; + require(recovered.initialize(fake_io(), &recovered_database) && + recovered.snapshot().generation == previous.generation && + recovered.snapshot().active_bank == previous.active_bank && + recovered_database.fallback_profiles[1].button_map[2] == 2, + "headerless corrupt payload was recovered"); +} + +} // namespace +int main() { + test_two_bank_recovery(); + test_interrupted_commit_retains_previous_bank(); + test_successful_header_program_is_commit_point(); + test_payload_corruption_prevents_header_publication(); + return 0; +} diff --git a/tests/test_config_manager.py b/tests/test_config_manager.py index b002165..5819074 100644 --- a/tests/test_config_manager.py +++ b/tests/test_config_manager.py @@ -1,7 +1,9 @@ from __future__ import annotations +import json import struct import zlib +from pathlib import Path import pytest @@ -56,7 +58,45 @@ class FakeDevice: ), ] self.pairing_generation = 4 + self.global_identity = config_manager.ControllerIdentity.global_fallback() + self.stable_identity = config_manager.ControllerIdentity( + True, + config_manager.TRANSPORT_CLASSIC, + 0, + bytes.fromhex("102030405060"), + 0x045E, + 0x02FD, + ) + self.profile_identities = [ + self.global_identity, + self.stable_identity, + ] + self.active_profiles = { + identity.to_bytes(): index + for identity, index in zip(self.profile_identities, (0, 1)) + } + default_profile = config_manager.ControllerProfile.default().to_bytes() + self.profiles = { + (identity.to_bytes(), index): default_profile + for identity in self.profile_identities + for index in range(config_manager.PROFILE_CAPACITY) + } + self.selected_profile = (self.global_identity.to_bytes(), 0) + self.profile_generation = 7 + self.profile_transaction_id = 0 + self.profile_transaction_identity = self.global_identity.to_bytes() + self.profile_transaction_index = 0 + self.profile_transaction_payload = bytearray() + self.profile_transaction_expected_size = 0 + self.profile_transaction_expected_crc = 0 + self.profile_transaction_status = config_manager.STATUS_OK + self.fail_profile_commit_status: int | None = None + self.bad_profile_response_crc = False self.requests: list[int] = [] + self.profile_chunk_sizes: list[int] = [] + self.pending_profile_mutation: tuple[int, bytes, int] | None = None + self.profile_transaction_pending_reads = 0 + self.profile_status_responses: list[tuple[int, int]] = [] def _pairing_payload(self) -> bytes: payload = bytearray([len(self.records), 0, 0, 0]) @@ -77,6 +117,71 @@ class FakeDevice: stored_crc, ) + def _profile_list_payload(self) -> bytes: + payload = bytearray([len(self.profile_identities)]) + for identity in self.profile_identities: + payload.extend(identity.to_bytes()) + payload.extend((self.active_profiles[identity.to_bytes()], 0)) + return bytes(payload) + + def _profile_transaction_payload(self) -> bytes: + stored = self.profiles.get( + ( + self.profile_transaction_identity, + self.profile_transaction_index, + ), + bytes(config_manager.PROFILE_SIZE), + ) + return struct.pack( + " None: + assert len(payload) == 19 + self.profile_transaction_id = struct.unpack_from(" None: + assert self.pending_profile_mutation is not None + operation, identity, profile_index = self.pending_profile_mutation + if self.fail_profile_commit_status is not None: + self.profile_transaction_status = self.fail_profile_commit_status + self.pending_profile_mutation = None + return + if operation == config_manager.OP_PROFILE_RESET: + indices = ( + range(config_manager.PROFILE_CAPACITY) + if profile_index == config_manager.PROFILE_NONE_BUTTON + else (profile_index,) + ) + default = config_manager.ControllerProfile.default().to_bytes() + for reset_index in indices: + self.profiles[(identity, reset_index)] = default + else: + assert operation == config_manager.OP_PROFILE_ACTIVATE + self.active_profiles[identity] = profile_index + self.profile_generation += 1 + self.profile_transaction_status = config_manager.STATUS_OK + self.pending_profile_mutation = None + def ctrl_transfer( self, bm_request_type: int, @@ -92,7 +197,9 @@ class FakeDevice: self.requests.append(request) if bm_request_type == 0xC0: if request == config_manager.OP_INFO: - return make_response(request, bytes([0, 2, 0, 2, 0, 0, 0, 2])) + return make_response( + request, bytes([0, 2, 0, 2, 0, 0, 0, 2]) + ) if request == config_manager.OP_CONFIGURATION_READ: return make_response( request, @@ -114,6 +221,44 @@ class FakeDevice: self._pairing_payload(), generation=self.pairing_generation, ) + if request == config_manager.OP_PROFILE_LIST: + return make_response( + request, + self._profile_list_payload(), + schema=config_manager.PROFILE_SCHEMA_VERSION, + generation=self.profile_generation, + ) + if request == config_manager.OP_PROFILE_READ: + response = bytearray( + make_response( + request, + self.profiles[self.selected_profile], + schema=config_manager.PROFILE_SCHEMA_VERSION, + generation=self.profile_generation, + ) + ) + if self.bad_profile_response_crc: + response[-1] ^= 1 + return bytes(response) + if request == config_manager.OP_PROFILE_TRANSACTION_STATUS: + if self.profile_transaction_status == config_manager.STATUS_PENDING: + if self.profile_transaction_pending_reads: + self.profile_transaction_pending_reads -= 1 + elif self.pending_profile_mutation is not None: + self._complete_profile_mutation() + self.profile_status_responses.append( + ( + self.profile_transaction_id, + self.profile_transaction_status, + ) + ) + return make_response( + request, + self._profile_transaction_payload(), + status=self.profile_transaction_status, + schema=config_manager.PROFILE_SCHEMA_VERSION, + generation=self.profile_generation, + ) raise AssertionError(f"unexpected IN request {request}") assert bm_request_type == 0x40 @@ -165,11 +310,138 @@ class FakeDevice: elif request == config_manager.OP_PAIRING_CLEAR: self.records = [] self.pairing_generation += 1 + elif request == config_manager.OP_PROFILE_SELECT: + assert len(payload) == 15 + self.selected_profile = (payload[:14], payload[14]) + assert self.selected_profile in self.profiles + elif request == config_manager.OP_PROFILE_BEGIN: + assert len(payload) == 28 + self.profile_transaction_id = struct.unpack_from(" config_manager.ControllerProfile: + return config_manager.ControllerProfile( + button_map=( + 1, + 0, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 15, + config_manager.PROFILE_NONE_BUTTON, + ), + left_stick=config_manager.StickConfig( + -123, 456, 1000, 30000, 384, True, False + ), + right_stick=config_manager.StickConfig( + 789, -321, 500, 31000, 192, False, True + ), + left_trigger=config_manager.TriggerConfig(100, 65000, 320, 32000), + right_trigger=config_manager.TriggerConfig(200, 64000, 224, 33000), + weak_rumble_scale=77, + strong_rumble_scale=201, + confirmation_policy=2, + switching_chord=(1 << 6) | (1 << 7), + macro_trigger=0, + macro_cancel=1, + macro_steps=( + config_manager.MacroStep( + 0, + config_manager.MACRO_OVERRIDE_MASK, + config_manager.PROFILE_MAXIMUM_WAIT_MS, + (1 << 0) | (1 << 12), + -32768, + 32767, + -1000, + 1000, + 12345, + 54321, + ), + config_manager.MacroStep.end(), + ), + turbo_modes=(0, 1, 2) + (0,) * 13, + ) + + def test_response_validation() -> None: payload = make_response(config_manager.OP_INFO, b"12345678") envelope = config_manager.parse_response(payload, config_manager.OP_INFO) @@ -205,6 +477,352 @@ def test_configuration_transaction_and_reset() -> None: assert config_manager.read_configuration(device).pairing_window_seconds == 60 +def test_identity_and_profile_binary_json_round_trip() -> None: + identity = config_manager.ControllerIdentity( + True, + config_manager.TRANSPORT_BLE, + 3, + bytes.fromhex("A1B2C3D4E5F6"), + 0x1234, + 0xABCD, + ) + encoded_identity = identity.to_bytes() + assert encoded_identity == bytes.fromhex( + "01020300A1B2C3D4E5F63412CDAB" + ) + assert config_manager.ControllerIdentity.from_bytes(encoded_identity) == identity + assert ( + config_manager.ControllerIdentity.global_fallback().to_bytes() + == bytes(config_manager.CONTROLLER_IDENTITY_SIZE) + ) + malformed_identity = bytearray(encoded_identity) + malformed_identity[3] = 1 + with pytest.raises(config_manager.ConfigManagerError): + config_manager.ControllerIdentity.from_bytes(malformed_identity) + + profile = custom_profile() + encoded = profile.to_bytes() + assert len(encoded) == config_manager.PROFILE_SIZE + assert struct.unpack_from(" None: + device = FakeDevice() + entries = config_manager.list_profiles(device) + assert entries == ( + config_manager.ProfileListEntry(device.global_identity, 0), + config_manager.ProfileListEntry(device.stable_identity, 1), + ) + assert ( + config_manager.read_profile(device, device.stable_identity, 1) + == config_manager.ControllerProfile.default() + ) + assert device.requests[-2:] == [ + config_manager.OP_PROFILE_SELECT, + config_manager.OP_PROFILE_READ, + ] + + profile = custom_profile() + status = config_manager.write_profile( + device, device.stable_identity, 2, profile, 1.0 + ) + assert status.status == config_manager.STATUS_OK + assert status.stored_generation == 8 + assert device.profile_chunk_sizes == [40, 40, 40, 40, 40, 40, 16] + assert config_manager.OP_PROFILE_TRANSACTION_STATUS in device.requests + assert ( + config_manager.read_profile(device, device.stable_identity, 2) + == profile + ) + + +def test_profile_reset_and_activate_wait_for_correlated_transactions( + monkeypatch: pytest.MonkeyPatch, +) -> None: + device = FakeDevice() + generated_ids = iter((0, 0xA5A55A5A)) + monkeypatch.setattr( + config_manager.secrets, "randbits", lambda _bits: next(generated_ids) + ) + identity = device.stable_identity + profile_key = (identity.to_bytes(), 2) + device.profiles[profile_key] = custom_profile().to_bytes() + + reset = config_manager.reset_profile(device, identity, 2, 1.0) + + assert reset.transaction_id == device.profile_transaction_id == 1 + assert reset.status == config_manager.STATUS_OK + assert reset.stored_generation == 8 + assert device.profile_status_responses == [ + (1, config_manager.STATUS_PENDING), + (1, config_manager.STATUS_OK), + ] + assert ( + device.profiles[profile_key] + == config_manager.ControllerProfile.default().to_bytes() + ) + + device.profile_status_responses.clear() + activated = config_manager.activate_profile(device, identity, 3, 1.0) + + assert ( + activated.transaction_id + == device.profile_transaction_id + == 0xA5A55A5A + ) + assert activated.status == config_manager.STATUS_OK + assert activated.stored_generation == 9 + assert device.profile_status_responses == [ + (0xA5A55A5A, config_manager.STATUS_PENDING), + (0xA5A55A5A, config_manager.STATUS_OK), + ] + assert device.active_profiles[identity.to_bytes()] == 3 + + +def test_profile_cli_surfaces_late_storage_failure( + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], +) -> None: + device = FakeDevice() + device.fail_profile_commit_status = 8 + monkeypatch.setattr(config_manager, "_candidate_devices", lambda: [device]) + previous_active = device.active_profiles[device.stable_identity.to_bytes()] + + assert ( + config_manager.main( + ["profiles", "activate", "4", "--identity", "1"] + ) + == 1 + ) + + output = capsys.readouterr() + assert output.out == "" + assert "storage failure" in output.err + assert ( + device.active_profiles[device.stable_identity.to_bytes()] + == previous_active + ) + assert device.profile_status_responses == [ + (device.profile_transaction_id, config_manager.STATUS_PENDING), + (device.profile_transaction_id, 8), + ] + + +def test_profile_cli_json_round_trip_activate_and_reset( + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], + tmp_path: Path, +) -> None: + device = FakeDevice() + monkeypatch.setattr(config_manager, "_candidate_devices", lambda: [device]) + export_path = tmp_path / "profile.json" + + assert config_manager.main(["profiles", "list"]) == 0 + output = capsys.readouterr().out + assert "0: global fallback (active profile 1)" in output + assert "1: Classic 10:20:30:40:50:60" in output + + device.profiles[(device.stable_identity.to_bytes(), 1)] = ( + custom_profile().to_bytes() + ) + assert ( + config_manager.main( + [ + "profiles", + "export", + "2", + str(export_path), + "--identity", + "1", + ] + ) + == 0 + ) + _ = capsys.readouterr() + exported = config_manager.ControllerProfile.from_json( + export_path.read_text(encoding="utf-8") + ) + assert exported == custom_profile() + + device.profiles[(device.stable_identity.to_bytes(), 1)] = ( + config_manager.ControllerProfile.default().to_bytes() + ) + assert ( + config_manager.main( + [ + "profiles", + "import", + "2", + str(export_path), + "--identity", + "1", + ] + ) + == 0 + ) + assert ( + device.profiles[(device.stable_identity.to_bytes(), 1)] + == custom_profile().to_bytes() + ) + _ = capsys.readouterr() + + assert ( + config_manager.main( + ["profiles", "activate", "4", "--identity", "1"] + ) + == 0 + ) + assert device.active_profiles[device.stable_identity.to_bytes()] == 3 + _ = capsys.readouterr() + + before_reset_requests = len(device.requests) + assert ( + config_manager.main( + ["profiles", "reset", "2", "--identity", "1"] + ) + == 2 + ) + assert "requires --yes" in capsys.readouterr().err + assert len(device.requests) == before_reset_requests + + assert ( + config_manager.main( + [ + "profiles", + "reset", + "2", + "--identity", + "1", + "--yes", + ] + ) + == 0 + ) + assert ( + device.profiles[(device.stable_identity.to_bytes(), 1)] + == config_manager.ControllerProfile.default().to_bytes() + ) + _ = capsys.readouterr() + assert ( + config_manager.main( + [ + "profiles", + "reset", + "all", + "--identity", + "1", + "--yes", + ] + ) + == 0 + ) + + +def test_malformed_profiles_are_rejected_before_usb( + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], + tmp_path: Path, +) -> None: + malformed_binary = bytearray(config_manager.ControllerProfile.default().to_bytes()) + malformed_binary[75] = 1 + with pytest.raises( + config_manager.ConfigManagerError, match="reserved fields" + ): + config_manager.ControllerProfile.from_bytes(malformed_binary) + + profile_object = config_manager.ControllerProfile.default().to_json_object() + del profile_object["turbo"] + missing_path = tmp_path / "missing.json" + missing_path.write_text(json.dumps(profile_object), encoding="utf-8") + + profile_object = config_manager.ControllerProfile.default().to_json_object() + profile_object["reserved"] = 0 + unknown_path = tmp_path / "unknown.json" + unknown_path.write_text(json.dumps(profile_object), encoding="utf-8") + + profile_object = config_manager.ControllerProfile.default().to_json_object() + profile_object["rumble"]["confirmation_policy"] = "invalid" + enum_path = tmp_path / "enum.json" + enum_path.write_text(json.dumps(profile_object), encoding="utf-8") + + profile_object = config_manager.ControllerProfile.default().to_json_object() + profile_object["sticks"]["left"]["outer_saturation"] = 0 + range_path = tmp_path / "range.json" + range_path.write_text(json.dumps(profile_object), encoding="utf-8") + + usb_lookups = 0 + + def candidates() -> list[FakeDevice]: + nonlocal usb_lookups + usb_lookups += 1 + return [FakeDevice()] + + monkeypatch.setattr(config_manager, "_candidate_devices", candidates) + for path in (missing_path, unknown_path, enum_path, range_path): + assert ( + config_manager.main(["profiles", "import", "1", str(path)]) == 1 + ) + assert "error:" in capsys.readouterr().err + assert usb_lookups == 0 + + +def test_profile_crc_status_failures_and_identity_bounds( + monkeypatch: pytest.MonkeyPatch, + capsys: pytest.CaptureFixture[str], + tmp_path: Path, +) -> None: + device = FakeDevice() + device.bad_profile_response_crc = True + with pytest.raises( + config_manager.ConfigManagerError, match="response CRC mismatch" + ): + config_manager.read_profile(device, device.global_identity, 0) + + failing_device = FakeDevice() + failing_device.fail_profile_commit_status = 6 + with pytest.raises(config_manager.ConfigManagerError, match="CRC mismatch"): + config_manager.write_profile( + failing_device, + failing_device.global_identity, + 0, + custom_profile(), + 1.0, + ) + + bounded_device = FakeDevice() + monkeypatch.setattr( + config_manager, "_candidate_devices", lambda: [bounded_device] + ) + assert ( + config_manager.main( + [ + "profiles", + "export", + "1", + str(tmp_path / "unused.json"), + "--identity", + "2", + ] + ) + == 1 + ) + assert "identity index 2 is out of range" in capsys.readouterr().err + assert config_manager.OP_PROFILE_SELECT not in bounded_device.requests + + def test_status_and_pairing_commands( monkeypatch: pytest.MonkeyPatch, capsys: pytest.CaptureFixture[str], diff --git a/tests/test_controller_profile_native.py b/tests/test_controller_profile_native.py new file mode 100644 index 0000000..3cf5575 --- /dev/null +++ b/tests/test_controller_profile_native.py @@ -0,0 +1,30 @@ +import shutil +import subprocess +from pathlib import Path + + +def test_controller_profile_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 / "controller_profile_test" + subprocess.run( + [ + compiler, + "-std=c++17", + "-Wall", + "-Wextra", + "-Werror", + "-pedantic", + f"-I{root}", + str(root / "tests" / "controller_profile_test.cpp"), + str(root / "controller_identity.cpp"), + str(root / "controller_profile.cpp"), + "-o", + str(executable), + ], + check=True, + cwd=root, + ) + subprocess.run([str(executable)], check=True, cwd=root) diff --git a/tests/test_profile_service_native.py b/tests/test_profile_service_native.py new file mode 100644 index 0000000..20fd48b --- /dev/null +++ b/tests/test_profile_service_native.py @@ -0,0 +1,33 @@ +import shutil +import subprocess +from pathlib import Path + + +def test_profile_service_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 / "profile_service_test" + subprocess.run( + [ + compiler, + "-std=c++17", + "-Wall", + "-Wextra", + "-Werror", + "-pedantic", + f"-I{root / 'tests' / 'bluepad32_native_stubs'}", + f"-I{root}", + str(root / "tests" / "profile_service_test.cpp"), + str(root / "controller_identity.cpp"), + str(root / "controller_profile.cpp"), + str(root / "profile_storage.cpp"), + str(root / "profile_service.cpp"), + "-o", + str(executable), + ], + check=True, + cwd=root, + ) + subprocess.run([str(executable)], check=True, cwd=root) diff --git a/tests/test_profile_storage_native.py b/tests/test_profile_storage_native.py new file mode 100644 index 0000000..80e204f --- /dev/null +++ b/tests/test_profile_storage_native.py @@ -0,0 +1,31 @@ +import shutil +import subprocess +from pathlib import Path + + +def test_profile_storage_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 / "profile_storage_test" + subprocess.run( + [ + compiler, + "-std=c++17", + "-Wall", + "-Wextra", + "-Werror", + "-pedantic", + f"-I{root}", + str(root / "tests" / "profile_storage_test.cpp"), + str(root / "controller_identity.cpp"), + str(root / "controller_profile.cpp"), + str(root / "profile_storage.cpp"), + "-o", + str(executable), + ], + check=True, + cwd=root, + ) + subprocess.run([str(executable)], check=True, cwd=root) diff --git a/tests/usb_configuration_management_test.cpp b/tests/usb_configuration_management_test.cpp index 1ddf306..093c8b3 100644 --- a/tests/usb_configuration_management_test.cpp +++ b/tests/usb_configuration_management_test.cpp @@ -11,6 +11,9 @@ namespace { Bluepad32PairingSnapshot current_pairings{}; ConfigurationServiceSnapshot current_configuration{}; +ProfileServiceListSnapshot current_profile_list{}; +ProfileServiceSelectedSnapshot current_profile_selected{}; +ProfileServiceTransactionSnapshot current_profile_transaction{}; bool refresh_requested = false; bool clear_requested = false; std::vector control_payload; @@ -20,6 +23,16 @@ uint32_t append_transaction_id = 0; uint32_t commit_transaction_id = 0; size_t append_offset = 0; std::vector appended_bytes; +ControllerIdentity profile_identity{}; +uint8_t profile_index = 0; +uint16_t profile_schema = 0; +size_t profile_size = 0; +uint32_t profile_crc = 0; +bool profile_reset_requested = false; +uint32_t profile_reset_transaction_id = 0; +uint32_t profile_commit_transaction_id = 0; +bool profile_activate_requested = false; +uint32_t profile_activate_transaction_id = 0; void require(bool condition, const char* message) { if (!condition) { @@ -42,6 +55,13 @@ void write_u32(std::vector* output, size_t offset, (*output)[offset + 3] = static_cast(value >> 24); } +uint32_t read_u32(const std::vector& input, size_t offset) { + return static_cast(input[offset]) | + (static_cast(input[offset + 1]) << 8) | + (static_cast(input[offset + 2]) << 16) | + (static_cast(input[offset + 3]) << 24); +} + std::vector make_request( UsbConfigurationManagement::Operation operation, const std::vector& payload) { @@ -213,6 +233,173 @@ void test_vendor_requests() { "request with invalid magic was accepted"); } +void test_profile_vendor_requests() { + using namespace UsbConfigurationManagement; + ControllerIdentity expected_identity{}; + expected_identity.stable = true; + expected_identity.transport = ControllerTransport::kClassic; + expected_identity.address[5] = 7; + expected_identity.vendor_id = 0x057e; + expected_identity.product_id = 0x2009; + + current_profile_list = {}; + current_profile_list.metadata.state = ProfileServiceState::kReady; + current_profile_list.metadata.generation = 9; + current_profile_list.count = 2; + current_profile_list.rows[0].identity = controller_identity_global(); + current_profile_list.rows[1].identity = expected_identity; + current_profile_list.rows[1].active_profile = 2; + tusb_control_request_t request = setup_request( + Operation::kProfileList, TUSB_DIR_IN, kMaximumResponseSize); + require(tud_vendor_control_xfer_cb( + 0, CONTROL_STAGE_SETUP, &request) && + control_payload.size() == kResponseHeaderSize + 33 && + control_payload[5] == + static_cast(Operation::kProfileList) && + control_payload[10] == CONTROLLER_PROFILE_SCHEMA_VERSION && + control_payload[kResponseHeaderSize] == 2 && + control_payload[kResponseHeaderSize + 31] == 2, + "profile list response was not encoded"); + + current_profile_selected = {}; + current_profile_selected.metadata.state = + ProfileServiceState::kReady; + current_profile_selected.metadata.generation = 9; + current_profile_selected.valid = true; + current_profile_selected.status = + ConfigurationTransactionStatus::kCommitted; + current_profile_selected.identity = expected_identity; + current_profile_selected.profile_index = 2; + current_profile_selected.profile = + controller_profile_default(expected_identity, 2); + request = setup_request( + Operation::kProfileRead, TUSB_DIR_IN, kMaximumResponseSize); + require(tud_vendor_control_xfer_cb( + 0, CONTROL_STAGE_SETUP, &request) && + control_payload.size() == + kResponseHeaderSize + + CONTROLLER_PROFILE_ENCODED_SIZE && + control_payload[kResponseHeaderSize] == 1 && + control_payload[kResponseHeaderSize + 2] == 0 && + control_payload[kResponseHeaderSize + 3] == 1, + "selected profile response was not encoded"); + + current_profile_transaction = {}; + current_profile_transaction.metadata.state = + ProfileServiceState::kReady; + current_profile_transaction.metadata.generation = 9; + current_profile_transaction.transaction.transaction_id = 0x01020304; + current_profile_transaction.transaction.status = + ConfigurationTransactionStatus::kPending; + request = setup_request( + Operation::kProfileTransactionStatus, TUSB_DIR_IN, + kMaximumResponseSize); + require(tud_vendor_control_xfer_cb( + 0, CONTROL_STAGE_SETUP, &request) && + control_payload.size() == kResponseHeaderSize + 20 && + control_payload[6] == + static_cast(Status::kPending) && + read_u32(control_payload, kResponseHeaderSize) == + 0x01020304, + "pending profile transaction status lost its transaction ID"); + + current_profile_transaction.transaction.status = + ConfigurationTransactionStatus::kCommitted; + current_profile_transaction.transaction.stored_generation = + 0x11223344; + current_profile_transaction.transaction.stored_crc = 0xaabbccdd; + require(tud_vendor_control_xfer_cb( + 0, CONTROL_STAGE_SETUP, &request) && + control_payload[6] == static_cast(Status::kOk) && + read_u32(control_payload, kResponseHeaderSize) == + 0x01020304 && + read_u32(control_payload, kResponseHeaderSize + 12) == + 0x11223344 && + read_u32(control_payload, kResponseHeaderSize + 16) == + 0xaabbccdd, + "final profile transaction status lost its commit result"); + + std::vector identity_payload(15); + require(controller_identity_encode( + expected_identity, identity_payload.data(), + CONTROLLER_IDENTITY_ENCODED_SIZE), + "profile test identity did not encode"); + identity_payload[14] = 2; + perform_out(Operation::kProfileSelect, identity_payload); + require(controller_identity_equal(expected_identity, + profile_identity) && + profile_index == 2, + "profile selection was not dispatched"); + + std::vector begin(28); + write_u32(&begin, 0, 0x55667788); + require(controller_identity_encode( + expected_identity, &begin[4], + CONTROLLER_IDENTITY_ENCODED_SIZE), + "profile begin identity did not encode"); + begin[18] = 1; + write_u16(&begin, 20, CONTROLLER_PROFILE_SCHEMA_VERSION); + write_u16(&begin, 22, CONTROLLER_PROFILE_ENCODED_SIZE); + write_u32(&begin, 24, 0xaabbccdd); + perform_out(Operation::kProfileBegin, begin); + require(begin_transaction_id == 0x55667788 && + profile_index == 1 && + profile_schema == CONTROLLER_PROFILE_SCHEMA_VERSION && + profile_size == CONTROLLER_PROFILE_ENCODED_SIZE && + profile_crc == 0xaabbccdd, + "profile begin was not dispatched"); + + std::vector chunk(48); + write_u32(&chunk, 0, 0x55667788); + write_u16(&chunk, 4, 0); + write_u16(&chunk, 6, 40); + perform_out(Operation::kProfileChunk, chunk); + require(append_transaction_id == 0x55667788 && + append_offset == 0 && appended_bytes.size() == 40, + "profile chunk was not dispatched"); + + std::vector commit(4); + write_u32(&commit, 0, 0x55667788); + perform_out(Operation::kProfileCommit, commit); + require(profile_commit_transaction_id == 0x55667788, + "profile commit was not dispatched"); + + std::vector mutation(19); + write_u32(&mutation, 0, 0x10203040); + require(controller_identity_encode( + expected_identity, &mutation[4], + CONTROLLER_IDENTITY_ENCODED_SIZE), + "profile mutation identity did not encode"); + mutation[18] = CONTROLLER_PROFILE_ALL; + perform_out(Operation::kProfileReset, mutation); + require(profile_reset_requested && + profile_reset_transaction_id == 0x10203040, + "profile reset transaction was not dispatched"); + write_u32(&mutation, 0, 0x50607080); + mutation[18] = 3; + perform_out(Operation::kProfileActivate, mutation); + require(profile_activate_requested && profile_index == 3 && + profile_activate_transaction_id == 0x50607080, + "profile activation transaction was not dispatched"); + + write_u32(&mutation, 0, 0); + perform_out(Operation::kProfileActivate, mutation, false); + request = setup_request( + Operation::kProfileReset, TUSB_DIR_OUT, kRequestHeaderSize + 15); + require(!tud_vendor_control_xfer_cb( + 0, CONTROL_STAGE_SETUP, &request), + "legacy profile reset payload was accepted"); + + begin[19] = 1; + perform_out(Operation::kProfileBegin, begin, false); + request = setup_request( + Operation::kProfileSelect, TUSB_DIR_OUT, + kRequestHeaderSize + 14); + require(!tud_vendor_control_xfer_cb( + 0, CONTROL_STAGE_SETUP, &request), + "short profile selection request was accepted"); +} + } // namespace uint32_t configuration_crc32(const uint8_t* data, size_t size) { @@ -256,6 +443,75 @@ ConfigurationTransactionStatus configuration_service_reset(uint32_t) { return ConfigurationTransactionStatus::kPending; } +ConfigurationTransactionStatus profile_service_select( + const ControllerIdentity& identity, uint8_t selected_profile) { + profile_identity = identity; + profile_index = selected_profile; + return ConfigurationTransactionStatus::kPending; +} + +ConfigurationTransactionStatus profile_service_begin( + uint32_t transaction_id, const ControllerIdentity& identity, + uint8_t selected_profile, uint16_t schema_version, + size_t payload_size, uint32_t payload_crc) { + begin_transaction_id = transaction_id; + profile_identity = identity; + profile_index = selected_profile; + profile_schema = schema_version; + profile_size = payload_size; + profile_crc = payload_crc; + return ConfigurationTransactionStatus::kReceiving; +} + +ConfigurationTransactionStatus profile_service_append( + uint32_t transaction_id, size_t offset, const uint8_t* data, + size_t size) { + append_transaction_id = transaction_id; + append_offset = offset; + appended_bytes.assign(data, data + size); + return ConfigurationTransactionStatus::kReceiving; +} + +ConfigurationTransactionStatus profile_service_commit( + uint32_t transaction_id) { + profile_commit_transaction_id = transaction_id; + return ConfigurationTransactionStatus::kPending; +} + +ConfigurationTransactionStatus profile_service_reset( + uint32_t transaction_id, const ControllerIdentity& identity, + uint8_t selected_profile) { + profile_reset_transaction_id = transaction_id; + profile_identity = identity; + profile_index = selected_profile; + profile_reset_requested = true; + return ConfigurationTransactionStatus::kPending; +} + +ConfigurationTransactionStatus profile_service_activate( + uint32_t transaction_id, const ControllerIdentity& identity, + uint8_t selected_profile) { + profile_activate_transaction_id = transaction_id; + profile_identity = identity; + profile_index = selected_profile; + profile_activate_requested = true; + return ConfigurationTransactionStatus::kPending; +} + +void profile_service_list_snapshot(ProfileServiceListSnapshot* output) { + *output = current_profile_list; +} + +void profile_service_selected_snapshot( + ProfileServiceSelectedSnapshot* output) { + *output = current_profile_selected; +} + +void profile_service_transaction_snapshot( + ProfileServiceTransactionSnapshot* output) { + *output = current_profile_transaction; +} + void bluepad32_input_backend_request_pairing_snapshot() { refresh_requested = true; } @@ -288,6 +544,8 @@ bool tud_control_status(uint8_t, const tusb_control_request_t*) { } #include "../adapter_configuration.cpp" +#include "../controller_identity.cpp" +#include "../controller_profile.cpp" #include "../usb_configuration_management.cpp" int main() { @@ -297,5 +555,6 @@ int main() { test_envelope_encoding(); test_pairing_encoding(); test_vendor_requests(); + test_profile_vendor_requests(); return 0; } diff --git a/usb_configuration_management.cpp b/usb_configuration_management.cpp index 697cdb5..517651e 100644 --- a/usb_configuration_management.cpp +++ b/usb_configuration_management.cpp @@ -61,17 +61,39 @@ Status transaction_status(ConfigurationTransactionStatus status) { } return Status::kStorageError; } +Status profile_service_status(const ProfileServiceMetadata& metadata) { + if (metadata.state == ProfileServiceState::kLoading) { + return Status::kPending; + } + if (metadata.state == ProfileServiceState::kStorageError) { + return Status::kStorageError; + } + return Status::kOk; +} + bool valid_out_size(Operation operation, size_t size) { switch (operation) { case Operation::kConfigurationBegin: return size == kRequestHeaderSize + 12; + case Operation::kProfileBegin: + return size == kRequestHeaderSize + 28; case Operation::kConfigurationChunk: return size > kRequestHeaderSize + 8 && size <= kMaximumRequestSize; + case Operation::kProfileChunk: + return size > kRequestHeaderSize + 8 && + size <= kMaximumRequestSize; case Operation::kConfigurationCommit: case Operation::kConfigurationReset: return size == kRequestHeaderSize + 4; + case Operation::kProfileCommit: + return size == kRequestHeaderSize + 4; + case Operation::kProfileSelect: + return size == kRequestHeaderSize + 15; + case Operation::kProfileReset: + case Operation::kProfileActivate: + return size == kRequestHeaderSize + 19; case Operation::kPairingRefresh: case Operation::kPairingClear: return size == kRequestHeaderSize; @@ -218,6 +240,74 @@ size_t encode_pairing_snapshot(const Bluepad32PairingSnapshot& snapshot, payload, offset, output, output_size); } +size_t encode_profile_list(const ProfileServiceListSnapshot& snapshot, + uint8_t* output, size_t output_size) { + if (snapshot.count > PROFILE_SERVICE_LIST_CAPACITY) { + return 0; + } + uint8_t payload[kProfileListPayloadSize]{}; + payload[0] = snapshot.count; + size_t offset = 1; + for (uint8_t index = 0; index < snapshot.count; ++index) { + if (!controller_identity_encode(snapshot.rows[index].identity, + &payload[offset], + CONTROLLER_IDENTITY_ENCODED_SIZE) || + snapshot.rows[index].active_profile >= + CONTROLLER_PROFILE_COUNT) { + return 0; + } + payload[offset + 14] = snapshot.rows[index].active_profile; + offset += 16; + } + return encode_response( + Operation::kProfileList, profile_service_status(snapshot.metadata), + 0, CONTROLLER_PROFILE_SCHEMA_VERSION, snapshot.metadata.generation, + payload, offset, output, output_size); +} + +size_t encode_profile_read(const ProfileServiceSelectedSnapshot& snapshot, + uint8_t* output, size_t output_size) { + Status status = profile_service_status(snapshot.metadata); + if (status == Status::kOk) { + status = transaction_status(snapshot.status); + } + uint8_t payload[CONTROLLER_PROFILE_ENCODED_SIZE]{}; + size_t payload_size = 0; + if (snapshot.valid) { + if (!controller_profile_encode(snapshot.profile, payload, + sizeof(payload))) { + return 0; + } + payload_size = sizeof(payload); + } + return encode_response( + Operation::kProfileRead, status, 0, + CONTROLLER_PROFILE_SCHEMA_VERSION, snapshot.metadata.generation, + payload, payload_size, output, output_size); +} + +size_t encode_profile_transaction( + const ProfileServiceTransactionSnapshot& snapshot, + uint8_t* output, size_t output_size) { + const ConfigurationTransactionSnapshot& transaction = + snapshot.transaction; + uint8_t payload[20]{}; + write_u32(&payload[0], transaction.transaction_id); + write_u16(&payload[4], transaction.received_size); + write_u16(&payload[6], transaction.expected_size); + write_u32(&payload[8], transaction.expected_crc); + write_u32(&payload[12], transaction.stored_generation); + write_u32(&payload[16], transaction.stored_crc); + Status status = profile_service_status(snapshot.metadata); + if (status == Status::kOk) { + status = transaction_status(transaction.status); + } + return encode_response( + Operation::kProfileTransactionStatus, status, 0, + CONTROLLER_PROFILE_SCHEMA_VERSION, snapshot.metadata.generation, + payload, sizeof(payload), output, output_size); +} + } // namespace UsbConfigurationManagement namespace { @@ -286,6 +376,96 @@ bool process_out_request() { (static_cast(payload[2]) << 16) | (static_cast(payload[3]) << 24)); return true; + case Operation::kProfileSelect: { + ControllerIdentity identity{}; + if (payload[14] >= CONTROLLER_PROFILE_COUNT || + !controller_identity_decode( + payload, CONTROLLER_IDENTITY_ENCODED_SIZE, &identity)) { + return false; + } + const ConfigurationTransactionStatus status = + profile_service_select(identity, payload[14]); + return status == ConfigurationTransactionStatus::kCommitted || + status == ConfigurationTransactionStatus::kPending; + } + case Operation::kProfileBegin: { + ControllerIdentity identity{}; + if (payload[19] != 0 || + !controller_identity_decode( + &payload[4], CONTROLLER_IDENTITY_ENCODED_SIZE, + &identity)) { + return false; + } + profile_service_begin( + static_cast(payload[0]) | + (static_cast(payload[1]) << 8) | + (static_cast(payload[2]) << 16) | + (static_cast(payload[3]) << 24), + identity, payload[18], + static_cast(payload[20] | + (payload[21] << 8)), + static_cast(payload[22] | + (payload[23] << 8)), + static_cast(payload[24]) | + (static_cast(payload[25]) << 8) | + (static_cast(payload[26]) << 16) | + (static_cast(payload[27]) << 24)); + return true; + } + case Operation::kProfileChunk: { + const uint16_t chunk_size = + static_cast(payload[6] | + (payload[7] << 8)); + if (request.payload_size != 8 + chunk_size || + chunk_size > kMaximumChunkSize) { + return false; + } + profile_service_append( + static_cast(payload[0]) | + (static_cast(payload[1]) << 8) | + (static_cast(payload[2]) << 16) | + (static_cast(payload[3]) << 24), + static_cast(payload[4] | + (payload[5] << 8)), + &payload[8], chunk_size); + return true; + } + case Operation::kProfileCommit: + profile_service_commit( + static_cast(payload[0]) | + (static_cast(payload[1]) << 8) | + (static_cast(payload[2]) << 16) | + (static_cast(payload[3]) << 24)); + return true; + case Operation::kProfileReset: + case Operation::kProfileActivate: { + const uint32_t transaction_id = + static_cast(payload[0]) | + (static_cast(payload[1]) << 8) | + (static_cast(payload[2]) << 16) | + (static_cast(payload[3]) << 24); + ControllerIdentity identity{}; + if (transaction_id == 0 || + (request.operation == Operation::kProfileActivate && + payload[18] >= CONTROLLER_PROFILE_COUNT) || + (request.operation == Operation::kProfileReset && + payload[18] != CONTROLLER_PROFILE_ALL && + payload[18] >= CONTROLLER_PROFILE_COUNT) || + !controller_identity_decode( + &payload[4], CONTROLLER_IDENTITY_ENCODED_SIZE, + &identity)) { + return false; + } + const ConfigurationTransactionStatus status = + request.operation == Operation::kProfileReset + ? profile_service_reset( + transaction_id, identity, payload[18]) + : profile_service_activate( + transaction_id, identity, payload[18]); + return status == ConfigurationTransactionStatus::kPending || + status == ConfigurationTransactionStatus::kUnchanged || + status == ConfigurationTransactionStatus::kCommitted; + } case Operation::kPairingRefresh: bluepad32_input_backend_request_pairing_snapshot(); return true; @@ -367,6 +547,27 @@ extern "C" bool tud_vendor_control_xfer_cb( snapshot, response, sizeof(response)); break; } + case Operation::kProfileList: { + ProfileServiceListSnapshot snapshot{}; + profile_service_list_snapshot(&snapshot); + response_size = encode_profile_list( + snapshot, response, sizeof(response)); + break; + } + case Operation::kProfileRead: { + ProfileServiceSelectedSnapshot snapshot{}; + profile_service_selected_snapshot(&snapshot); + response_size = encode_profile_read( + snapshot, response, sizeof(response)); + break; + } + case Operation::kProfileTransactionStatus: { + ProfileServiceTransactionSnapshot snapshot{}; + profile_service_transaction_snapshot(&snapshot); + response_size = encode_profile_transaction( + snapshot, response, sizeof(response)); + break; + } default: return false; } diff --git a/usb_configuration_management.h b/usb_configuration_management.h index 738a132..21e07f5 100644 --- a/usb_configuration_management.h +++ b/usb_configuration_management.h @@ -5,6 +5,7 @@ #include "bluepad32_input_backend.h" #include "configuration_service.h" +#include "profile_service.h" namespace UsbConfigurationManagement { @@ -16,11 +17,14 @@ constexpr size_t kResponseHeaderSize = 20; constexpr size_t kPairingRecordSize = 8; constexpr size_t kPairingPayloadHeaderSize = 4; constexpr size_t kMaximumRequestSize = 64; +constexpr size_t kProfileListPayloadSize = + 1 + PROFILE_SERVICE_LIST_CAPACITY * 16; constexpr size_t kMaximumResponseSize = - kResponseHeaderSize + kPairingPayloadHeaderSize + - BLUEPAD32_PAIRING_RECORD_CAPACITY * kPairingRecordSize; + kResponseHeaderSize + kProfileListPayloadSize; constexpr size_t kMaximumChunkSize = kMaximumRequestSize - kRequestHeaderSize - 8; +static_assert(kMaximumResponseSize == 293, + "profile list no longer fits the EP0 response buffer"); enum class Operation : uint8_t { kInfo = 0x01, @@ -33,6 +37,15 @@ enum class Operation : uint8_t { kPairingRead = 0x20, kPairingRefresh = 0x21, kPairingClear = 0x22, + kProfileList = 0x30, + kProfileSelect = 0x31, + kProfileRead = 0x32, + kProfileBegin = 0x33, + kProfileChunk = 0x34, + kProfileCommit = 0x35, + kProfileReset = 0x36, + kProfileActivate = 0x37, + kProfileTransactionStatus = 0x38, }; enum class Status : uint8_t { @@ -61,5 +74,12 @@ size_t encode_response(Operation operation, Status status, uint8_t flags, uint8_t* output, size_t output_size); size_t encode_pairing_snapshot(const Bluepad32PairingSnapshot& snapshot, uint8_t* output, size_t output_size); +size_t encode_profile_list(const ProfileServiceListSnapshot& snapshot, + uint8_t* output, size_t output_size); +size_t encode_profile_read(const ProfileServiceSelectedSnapshot& snapshot, + uint8_t* output, size_t output_size); +size_t encode_profile_transaction( + const ProfileServiceTransactionSnapshot& snapshot, + uint8_t* output, size_t output_size); } // namespace UsbConfigurationManagement