Switch to using l2cap_can_send_packet_now

shermp · Oct 31, 2024 · ff85d86 · ff85d86
1 parent 0856842
commit ff85d86
Show file tree

Hide file tree

Showing 4 changed files with 91 additions and 20 deletions.
diff --git a/src/asha_bt_ha.cpp b/src/asha_bt_ha.cpp
@@ -143,7 +143,7 @@ HearingAid::HearingAid(BT::Remote* r) : remote(r)
 
 bool HearingAid::is_streaming()
 {
-    return state == State::AudioStarted || state == State::AudioWorking;
+    return state == State::AudioStarted || state == State::AudioPending || state == State::AudioSending;
 }
 
 void HearingAid::set_characteristic_data()
@@ -537,25 +537,49 @@ void HearingAid::set_volume(int8_t new_volume)
 
 void HearingAid::send_audio(uint32_t write_index)
 {
-    if (write_index > audio_w_index) {
-        audio_w_index = write_index;
-    }
-    if (outgoing_credits == 0) {
-        LOG_INFO("%s: Available credits: 0, restarting stream", side_str);
-        stop_audio();
-        return;
-    }
-    if (audio_r_index >= audio_w_index) {
-        return;
-    }
-    state = State::AudioWorking;
-    if ((audio_w_index - audio_r_index) >= 2) {
-        audio_r_index = audio_w_index - 1;
+    switch (state) {
+        case State::AudioStarted:
+        {
+            audio_w_index = write_index;
+            if (audio_r_index >= audio_w_index) { return; }
+
+            if ((audio_w_index - audio_r_index) >= 2) {
+                audio_r_index = audio_w_index - 1;
+            }
+            AudioBuffer::G722Buff& packet = audio_buff.get_g_buff(audio_r_index);
+            audio_data = rop.side == Side::Left ? packet.l.data() : packet.r.data();
+            ++audio_r_index;
+            state = State::AudioPending;
+            BT::Result res = remote->prepare_l2cap_data(audio_data, sdu_size_bytes);
+            log_bt_res(res, bt_err, "preparing audio");
+        };
+        [[fallthrough]];
+        case State::AudioPending:
+        {
+            if (outgoing_credits == 0) {
+                LOG_INFO("%s: Available credits: 0, restarting stream", side_str);
+                stop_audio();
+                return;
+            }
+            state = State::AudioSending;
+            BT::Result res = remote->try_send_current_l2cap_data(&bt_err);
+            switch (res) {
+                case BT::Result::Ok:
+                    break;
+                case BT::Result::TryAgain:
+                    state = State::AudioPending;
+                    break;
+                default:
+                    log_bt_res(res, bt_err, "sending audio");
+                    state = State::AudioStarted;
+                    break;
+            }
+            break;
+        }
+        default:
+            break;
     }
-    AudioBuffer::G722Buff& packet = audio_buff.get_g_buff(audio_r_index);
-    audio_data = rop.side == Side::Left ? packet.l.data() : packet.r.data();
-    ++audio_r_index;
-    remote->send_l2cap_data(audio_data, sdu_size_bytes, &bt_err);
+
 }
 
 struct HearingAidMgr
@@ -741,6 +765,7 @@ static void handle_bt_audio_pending_worker([[maybe_unused]] async_context_t *con
                 }
                 break;
             case AudioStarted:
+            case AudioPending:
                 ha.outgoing_credits = l2cap_cbm_available_credits(ha.remote->local_cid);
                 if (!pcm_is_streaming) {
                     LOG_INFO("%s: USB audio no longer streaming. Stopping ASHA stream", ha.side_str);
@@ -754,6 +779,7 @@ static void handle_bt_audio_pending_worker([[maybe_unused]] async_context_t *con
 
                 ha.set_volume(ha.rop.side == Side::Left ? vol.l : vol.r);
                 ha.send_audio(write_index);
+                break;
             default:
                 break;
         }

diff --git a/src/asha_bt_ha.hpp b/src/asha_bt_ha.hpp
@@ -68,7 +68,8 @@ struct HearingAid
         AudioReady,
         AudioStarting,
         AudioStarted,
-        AudioWorking,
+        AudioPending,
+        AudioSending,
         AudioStopping,
     };
 

diff --git a/src/pico_bt.cpp b/src/pico_bt.cpp
@@ -380,6 +380,37 @@ BT::Result BT::Remote::create_l2cap_cbm_conn(uint16_t psm,
     return Result::WrongState;
 }
 
+BT::Result BT::Remote::prepare_l2cap_data(uint8_t const* data,
+            uint16_t len)
+{
+    if (state == RemoteState::Connected) {
+        state = RemoteState::L2CAPDataPending;
+        l2cap_data = data;
+        l2cap_len = len;
+        return Result::Ok;
+    }
+    return Result::WrongState;
+}
+
+BT::Result BT::Remote::try_send_current_l2cap_data(uint8_t* bt_err)
+{
+    if (state == RemoteState::L2CAPDataPending)
+    {
+        if (l2cap_can_send_packet_now(local_cid)) {
+            state = RemoteState::SendL2CAPData;
+            uint8_t err = l2cap_send(local_cid, l2cap_data, l2cap_len);
+            if (err != ERROR_CODE_SUCCESS) {
+                state = RemoteState::Connected;
+                *bt_err = err;
+                return Result::BTError;
+            }
+            return Result::Ok;
+        }
+        return Result::TryAgain;
+    }
+    return Result::WrongState;
+}
+
 BT::Result BT::Remote::send_l2cap_data(uint8_t const* data,
         uint16_t len,
         uint8_t* bt_err)

diff --git a/src/pico_bt.hpp b/src/pico_bt.hpp
@@ -113,6 +113,7 @@ class BT {
         WriteCharVal,
         EnableNotification,
         CreateL2CAP,
+        L2CAPDataPending,
         SendL2CAPData,
     };
 
@@ -121,6 +122,7 @@ class BT {
         InternalError,
         WrongState,
         MaxConnections,
+        TryAgain,
         BTError,
     };
 
@@ -297,6 +299,17 @@ class BT {
             uint8_t* bt_err
         );
 
+        /**
+         * Prepare to send data over l2cap.
+         * Call try_send_current_l2cap_data to try and send data.
+         */
+        Result prepare_l2cap_data(
+            uint8_t const* data,
+            uint16_t len
+        );
+
+        Result try_send_current_l2cap_data(uint8_t* bt_err);
+
         /**
          * Send data on currently open L2CAP CoC channel
          * data is the data to send. It must remain available until