Add set data length

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<HearingAid, &HearingAid::on_services_discovered>(*this);
     service_filter_d.set<HearingAid, &HearingAid::service_filter>(*this);
+    on_data_length_set_d.set<HearingAid, &HearingAid::on_data_length_set>(*this);
     on_paired_and_bonded_d.set<HearingAid, &HearingAid::on_paired_and_bonded>(*this);
     on_chars_discovered_d.set<HearingAid, &HearingAid::on_chars_discovered>(*this);
     discover_chars_filter_d.set<HearingAid, &HearingAid::discover_chars_filter>(*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;

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<void(uint8_t, BT::Remote*)> on_services_discovered_d;
     etl::delegate<bool(BT::Service const&)> service_filter_d;
+    etl::delegate<void(BT::Remote*)> on_data_length_set_d;
     etl::delegate<void(uint8_t, uint8_t, BT::Remote*)> on_paired_and_bonded_d;
     etl::delegate<void(uint8_t, BT::Remote*)> on_chars_discovered_d;
     etl::delegate<bool(gatt_client_characteristic_t*)> discover_chars_filter_d;

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<void(Remote* remote)> 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));

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<Service*, max_num_services> 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<void(Remote* remote)> dle_cb
+        );
+
         /**
          * Disconnect from remote.
          * The disconnect callback set in connect will be called 
@@ -305,6 +318,7 @@ class BT {
 
     private:
         /* Callbacks */
+        etl::delegate<void(Remote*)> p_dle_cb;
         etl::delegate<void(uint8_t, uint8_t, Remote*)> p_bond_cb;
         etl::delegate<void(uint8_t, Remote*)> p_services_cb;
         etl::delegate<void(uint8_t, Remote*)> p_char_cb;