l2cap ertm: fix outgoing fragment management

CHANGELOG.md

@@ -13,7 +13,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/spec/v2.0.0.
 - HCI: fix bug in gap_inquiry_stop that triggered additional GAP_EVENT_INQUIRY_COMPLETE instead of stopping the inquiry
 - L2CAP: fix issue with outgoing connection before read remote supported complete when other channels exist
 - L2CAP ERTM: allow SDU of szie MPS in first packet that contains L2CAP SDU Length
-- L2CAP ERTM: fix memory corruption triggered if local_mtu > mps 
+- L2CAP ERTM: fix memory corruption triggered if local_mtu > mps
+- L2CAP ERTM: fix outgoing fragment management
 - HFP: decline incoming RFCOMM connection after outgoing connection was started
 - AVRCP: fix crash on disconnect of connection established by remote
 

src/l2cap.c

@@ -193,12 +193,8 @@ static int l2cap_next_ertm_seq_nr(int seq_nr){
 }
 
 static int l2cap_ertm_can_store_packet_now(l2cap_channel_t * channel){
-     // get num free tx buffers
-    int num_tx_buffers_used = channel->tx_write_index - channel->tx_read_index;
-    if (num_tx_buffers_used < 0){
-        num_tx_buffers_used += channel->num_tx_buffers;
-    }
-    int num_free_tx_buffers = channel->num_tx_buffers - num_tx_buffers_used;
+    // get num free tx buffers
+    int num_free_tx_buffers = channel->num_tx_buffers - channel->num_stored_tx_frames;
     // calculate num tx buffers for remote MTU
     int num_tx_buffers_for_max_remote_mtu;
     if (channel->remote_mtu <= channel->remote_mps){
@@ -208,6 +204,7 @@ static int l2cap_ertm_can_store_packet_now(l2cap_channel_t * channel){
         // include SDU Length
         num_tx_buffers_for_max_remote_mtu = (channel->remote_mtu + 2 + (channel->remote_mps - 1)) / channel->remote_mps;
     }
+    log_debug("num_free_tx_buffers %u, num_tx_buffers_for_max_remote_mtu %u", num_free_tx_buffers, num_tx_buffers_for_max_remote_mtu);
     return num_tx_buffers_for_max_remote_mtu <= num_free_tx_buffers;
 }
 
@@ -313,7 +310,7 @@ static void l2cap_ertm_store_fragment(l2cap_channel_t * channel, l2cap_segmentat
     tx_state->retry_count = 0;
 
     uint8_t * tx_packet = &channel->tx_packets_data[index * channel->local_mps];
-    log_info("index %u, mtu %u, packet tx %p", index, channel->local_mtu, tx_packet);
+    log_debug("index %u, mtu %u, packet tx %p", index, channel->local_mtu, tx_packet);
     int pos = 0;
     if (sar == L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU){
         little_endian_store_16(tx_packet, 0, sdu_length);
@@ -322,19 +319,25 @@ static void l2cap_ertm_store_fragment(l2cap_channel_t * channel, l2cap_segmentat
     memcpy(&tx_packet[pos], data, len);
 
     // update
+    channel->num_stored_tx_frames++;
     channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq);
     l2cap_ertm_next_tx_write_index(channel);
 
-    log_info("l2cap_ertm_store_fragment: after store, tx_read_index %u, tx_write_index %u", channel->tx_read_index, channel->tx_write_index);
+    log_info("l2cap_ertm_store_fragment: tx_read_index %u, tx_write_index %u, num stored %u", channel->tx_read_index, channel->tx_write_index, channel->num_stored_tx_frames);
 
 }
 
 static int l2cap_ertm_send(l2cap_channel_t * channel, uint8_t * data, uint16_t len){
     if (len > channel->remote_mtu){
-        log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", channel->local_cid);
+        log_error("l2cap_ertm_send cid 0x%02x, data length exceeds remote MTU.", channel->local_cid);
         return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU;
     }
 
+    if (!l2cap_ertm_can_store_packet_now(channel)){
+        log_error("l2cap_ertm_send cid 0x%02x, fragment store full", channel->local_cid);
+        return BTSTACK_ACL_BUFFERS_FULL;
+    }
+
     // check if it needs to get fragmented
     if (len > channel->remote_mps){
         // fragmentation needed.
@@ -630,6 +633,7 @@ static void l2cap_ertm_process_req_seq(l2cap_channel_t * l2cap_channel, uint8_t
         if (delta > l2cap_channel->remote_tx_window_size) break;   
 
         num_buffers_acked++;
+        l2cap_channel->num_stored_tx_frames--;
         l2cap_channel->unacked_frames--;
         log_info("RR seq %u => packet with tx_seq %u done", req_seq, tx_state->tx_seq);
 
@@ -638,8 +642,8 @@ static void l2cap_ertm_process_req_seq(l2cap_channel_t * l2cap_channel, uint8_t
             l2cap_channel->tx_read_index = 0;
         }
     }
-
     if (num_buffers_acked){
+        log_info("num_buffers_acked %u", num_buffers_acked);
         l2cap_ertm_notify_channel_can_send(l2cap_channel);
     }
 }     
@@ -1941,8 +1945,15 @@ static void l2cap_notify_channel_can_send(void){
 #endif
             } else {
 #ifdef ENABLE_CLASSIC
+#ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
+                // skip ertm channels as they only depend on free buffers in storage
+                if (channel->mode == L2CAP_CHANNEL_MODE_BASIC){
+                    can_send = hci_can_send_acl_classic_packet_now();
+                }
+#else
                 can_send = hci_can_send_acl_classic_packet_now();
-#endif
+#endif /* ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE */
+#endif /* ENABLE_CLASSIC */
             }
             if (!can_send) continue;
             // requeue for fairness

src/l2cap.h

@@ -292,6 +292,9 @@ typedef struct {
     // sender: max num of stored outgoing frames
     uint8_t num_tx_buffers;
 
+    // sender: num stored outgoing frames
+    uint8_t num_stored_tx_frames;
+
     // sender: number of unacknowledeged I-Frames - frames have been sent, but not acknowledged yet
     uint8_t unacked_frames;