diff --git a/src/asha_bt_ha.cpp b/src/asha_bt_ha.cpp index 60a705a..a0b899f 100644 --- a/src/asha_bt_ha.cpp +++ b/src/asha_bt_ha.cpp @@ -55,6 +55,11 @@ namespace ACPStatus constexpr uint8_t conn_param_updated = 2; } +/* Data length variables */ + +static constexpr uint16_t pdu_len = 167u; +static constexpr uint16_t max_tx_time = 1064; + /* Static function declarations */ static void handle_bt_audio_pending_worker(async_context_t *context, async_when_pending_worker_t *worker); @@ -118,6 +123,7 @@ HearingAid::HearingAid(BT::Remote* r) : remote(r) on_services_discovered_d.set(*this); service_filter_d.set(*this); + on_data_length_set_d.set(*this); on_paired_and_bonded_d.set(*this); on_chars_discovered_d.set(*this); discover_chars_filter_d.set(*this); @@ -226,6 +232,24 @@ void HearingAid::discover_services() return; } +void HearingAid::on_data_length_set(BT::Remote*) +{ + LOG_INFO("%s: Data length set", bd_addr_to_str(remote->addr)); + state = State::DataLengthSet; +} + +void HearingAid::set_data_length() +{ + LOG_INFO("%s: Setting data length", bd_addr_to_str(remote->addr)); + state = State::SetDataLength; + BT::Result res = remote->set_data_length(pdu_len, max_tx_time, on_data_length_set_d); + LOG_BT_RES(res, bt_err); + if (res != BT::Result::Ok) { + LOG_ERROR("%s: Failed to set data length", bd_addr_to_str(remote->addr)); + disconnect(); + } +} + void HearingAid::on_paired_and_bonded(uint8_t status, uint8_t reason, BT::Remote*) { if (status != ERROR_CODE_SUCCESS) { @@ -665,6 +689,10 @@ static void handle_bt_audio_pending_worker([[maybe_unused]] async_context_t *con ha.discover_services(); break; case ServicesDiscovered: + LOG_INFO("%s: Set Data Length", bd_addr_to_str(ha.remote->addr)); + ha.set_data_length(); + break; + case DataLengthSet: LOG_INFO("%s: Pair and bond", bd_addr_to_str(ha.remote->addr)); ha.pair_and_bond(); break; diff --git a/src/asha_bt_ha.hpp b/src/asha_bt_ha.hpp index 6774d92..62eab03 100644 --- a/src/asha_bt_ha.hpp +++ b/src/asha_bt_ha.hpp @@ -53,6 +53,8 @@ struct HearingAid Disconnecting, DiscoveringServices, ServicesDiscovered, + SetDataLength, + DataLengthSet, PairAndBonding, PairedAndBonded, DiscoverChars, @@ -116,6 +118,8 @@ struct HearingAid void discover_services(); void on_services_discovered(uint8_t status, BT::Remote*); bool service_filter(BT::Service const& service); + void set_data_length(); + void on_data_length_set(BT::Remote*); void pair_and_bond(); void on_paired_and_bonded(uint8_t status, uint8_t reason, BT::Remote*); void discover_characteristics(); @@ -142,6 +146,7 @@ struct HearingAid /* Delegate variables */ etl::delegate on_services_discovered_d; etl::delegate service_filter_d; + etl::delegate on_data_length_set_d; etl::delegate on_paired_and_bonded_d; etl::delegate on_chars_discovered_d; etl::delegate discover_chars_filter_d; diff --git a/src/pico_bt.cpp b/src/pico_bt.cpp index 0e34e0b..fb6eafd 100644 --- a/src/pico_bt.cpp +++ b/src/pico_bt.cpp @@ -196,6 +196,24 @@ void BT::clear_bonding_data() } } +BT::Result BT::Remote::set_data_length(uint16_t pdu_len, + uint16_t max_tx_time, + etl::delegate dle_cb) +{ + p_dle_cb = dle_cb; + if (state == RemoteState::Connected) { + state = RemoteState::SetDataLength; + while (1) { + if (hci_can_send_command_packet_now()) { + hci_send_cmd(&hci_le_set_data_length, con_handle, pdu_len, max_tx_time); + break; + } + } + return Result::Ok; + } + return Result::WrongState; +} + BT::Result BT::Remote::disconnect(uint8_t* bt_err) { if (state == RemoteState::Connected) { @@ -533,6 +551,17 @@ void BT::hci_handler(uint8_t packet_type, bt.p_connect_cb(status, nullptr); break; } + case HCI_EVENT_LE_META: + { + if (hci_event_le_meta_get_subevent_code(packet) == HCI_SUBEVENT_LE_DATA_LENGTH_CHANGE) { + Remote* r = bt.get_by_con_handle(hci_subevent_le_data_length_change_get_connection_handle(packet)); + if (!r) { return; } + if (r->state != RemoteState::SetDataLength) { return; } + r->state = RemoteState::Connected; + r->p_dle_cb(r); + } + break; + } case HCI_EVENT_DISCONNECTION_COMPLETE: { Remote* r = bt.get_by_con_handle(hci_event_disconnection_complete_get_connection_handle(packet)); diff --git a/src/pico_bt.hpp b/src/pico_bt.hpp index 3137950..6c838b6 100644 --- a/src/pico_bt.hpp +++ b/src/pico_bt.hpp @@ -105,6 +105,7 @@ class BT { enum class RemoteState { Invalid, Connected, + SetDataLength, Disconnect, Bonding, ServiceDiscovery, @@ -150,6 +151,18 @@ class BT { bd_addr_t addr = {}; etl::vector services = {}; + /** + * Set the le data length + * dle_cb will be called on completion + * Result will be WrongState if in incorrect state, + * or Ok otherwise + */ + Result set_data_length( + uint16_t pdu_len, + uint16_t max_tx_time, + etl::delegate dle_cb + ); + /** * Disconnect from remote. * The disconnect callback set in connect will be called @@ -305,6 +318,7 @@ class BT { private: /* Callbacks */ + etl::delegate p_dle_cb; etl::delegate p_bond_cb; etl::delegate p_services_cb; etl::delegate p_char_cb;