From 675e6881a7fc83a76c055a359c267dd5590a5d09 Mon Sep 17 00:00:00 2001
From: Matthias Ringwald <matthias@ringwald.ch>
Date: Mon, 17 Jul 2017 15:06:38 +0200
Subject: [PATCH] l2cap-ertm: send tx packets from l2cap_run

---
 src/l2cap.c | 18 ++++++++++++++++--
 src/l2cap.h |  7 +++++--
 2 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/src/l2cap.c b/src/l2cap.c
index b91a4df9e..16f2c5f7a 100644
--- a/src/l2cap.c
+++ b/src/l2cap.c
@@ -89,6 +89,7 @@
 #endif
 
 // prototypes
+static void l2cap_run(void);
 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size );
 static void l2cap_notify_channel_can_send(void);
@@ -591,8 +592,8 @@ static int l2cap_ertm_send(l2cap_channel_t * channel, uint8_t * data, uint16_t l
     // update
     channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq);
     l2cap_ertm_next_tx_write_index(channel);
-    // test sendiging it right away
-    l2cap_ertm_send_information_frame(channel, index);
+    // try to send
+    l2cap_run();
     return 0;
 }
 #endif
@@ -938,11 +939,24 @@ static void l2cap_run(void){
 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
         // send s-frame to acknowledge received packets
         if (!hci_can_send_acl_packet_now(channel->con_handle)) continue;
+
+        if (channel->tx_send_index != channel->tx_write_index){
+            // packet ready to send!
+            int index = channel->tx_send_index;
+            channel->tx_send_index++;
+            if (channel->tx_send_index >= channel->num_tx_buffers){
+                channel->tx_send_index = 0;          
+            }
+            l2cap_ertm_send_information_frame(channel, index);
+            continue;
+        }
+
         if (channel->send_supervisor_frame_receiver_ready){
             channel->send_supervisor_frame_receiver_ready = 0;;
             log_info("Send S-Frame: RR %u", channel->req_seq);
             uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 0, 0, channel->req_seq);
             l2cap_ertm_send_supervisor_frame(channel, control);
+            continue;
         }
 #endif
 
diff --git a/src/l2cap.h b/src/l2cap.h
index a24b5e7db..e44543d7c 100644
--- a/src/l2cap.h
+++ b/src/l2cap.h
@@ -211,11 +211,14 @@ typedef struct {
     // if ertm is not mandatory, allow fallback to L2CAP Basic Mode - flag
     uint8_t ertm_mandatory;
 
-    // sender: buffer index to store tx packet
+    // sender: buffer index of oldest packet
+    uint8_t tx_read_index;
+
+    // sender: buffer index to store next tx packet
     uint8_t tx_write_index;
 
     // sender: buffer index of packet to send next
-    uint8_t tx_read_index;
+    uint8_t tx_send_index;
 
     // sender: next seq nr used for sending
     uint8_t next_tx_seq;