From f0fb4cd727a7cb54eb1a1c2b1dc9efdf67d42fde Mon Sep 17 00:00:00 2001
From: Matthias Ringwald <matthias@ringwald.ch>
Date: Mon, 17 Jul 2017 11:05:48 +0200
Subject: [PATCH] l2cap-ertm: store tx packets in channel->tx_packets_data
 before sending

---
 src/l2cap.c | 61 ++++++++++++++++++++++++++++++++++++++---------------
 src/l2cap.h | 11 ++++++++--
 2 files changed, 53 insertions(+), 19 deletions(-)

diff --git a/src/l2cap.c b/src/l2cap.c
index 009ebdb85..b91a4df9e 100644
--- a/src/l2cap.c
+++ b/src/l2cap.c
@@ -560,17 +560,58 @@ static inline uint16_t l2cap_encanced_control_field_for_supevisor_frame(l2cap_su
 static int l2cap_next_ertm_seq_nr(int seq_nr){
     return (seq_nr + 1) & 0x3f;
 }
+static void l2cap_ertm_next_tx_write_index(l2cap_channel_t * channel){
+    channel->tx_write_index++;
+    if (channel->tx_write_index < channel->num_tx_buffers) return;
+    channel->tx_write_index = 0;
+}
+static int l2cap_ertm_send_information_frame(l2cap_channel_t * channel, int index){
+    l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index];
+    hci_reserve_packet_buffer();
+    uint8_t *acl_buffer = hci_get_outgoing_packet_buffer();
+    uint16_t control = l2cap_encanced_control_field_for_information_frame(tx_state->tx_seq, 0, 0, L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU);
+    log_info("I-Frame: control 0x%04x", control);
+    little_endian_store_16(acl_buffer, 8, control);
+    memcpy(&acl_buffer[8+2], &channel->tx_packets_data[index * channel->local_mtu], tx_state->len);
+    // send
+    return l2cap_send_prepared(channel->local_cid, 2 + tx_state->len);
+}
+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);
+        return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU;
+    }
+    // TODO: check tx_transmit
+    // store int tx packet buffer
+    int index = channel->tx_write_index;
+    l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index];
+    tx_state->tx_seq = channel->next_tx_seq;
+    tx_state->len = len;
+    memcpy(&channel->tx_packets_data[index * channel->local_mtu], data, len);
+    // 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);
+    return 0;
+}
 #endif
 
 // assumption - only on Classic connections
 int l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){
-
     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
     if (!channel) {
         log_error("l2cap_send no channel for cid 0x%02x", local_cid);
         return -1;   // TODO: define error
     }
 
+#ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
+    // send in ERTM
+    if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){
+        return l2cap_ertm_send(channel, data, len);
+    }
+#endif
+
     if (len > channel->remote_mtu){
         log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid);
         return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU;
@@ -583,22 +624,8 @@ int l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){
 
     hci_reserve_packet_buffer();
     uint8_t *acl_buffer = hci_get_outgoing_packet_buffer();
-
-    int control_size = 0;
-#ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
-    // hack to send i-frames
-    if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){
-        uint16_t control = l2cap_encanced_control_field_for_information_frame(channel->next_tx_seq, 0, 0, L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU);
-        log_info("I-Frame: control 0x%04x", control);
-        little_endian_store_16(acl_buffer, 8, control);
-        channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq);
-        control_size = 2;
-    }
-#endif
-
-    memcpy(&acl_buffer[8+control_size], data, len);
-
-    return l2cap_send_prepared(local_cid, len+control_size);
+    memcpy(&acl_buffer[8], data, len);
+    return l2cap_send_prepared(local_cid, len);
 }
 
 int l2cap_send_echo_request(hci_con_handle_t con_handle, uint8_t *data, uint16_t len){
diff --git a/src/l2cap.h b/src/l2cap.h
index 3c034b800..a24b5e7db 100644
--- a/src/l2cap.h
+++ b/src/l2cap.h
@@ -123,6 +123,7 @@ typedef struct {
 
 typedef struct {
     uint8_t tx_seq;
+    uint16_t len;
 } l2cap_ertm_tx_packet_state_t;
 
 
@@ -210,12 +211,18 @@ typedef struct {
     // if ertm is not mandatory, allow fallback to L2CAP Basic Mode - flag
     uint8_t ertm_mandatory;
 
-    // receiver: send RR frame - flag
-    uint8_t send_supervisor_frame_receiver_ready;
+    // sender: buffer index to store tx packet
+    uint8_t tx_write_index;
+
+    // sender: buffer index of packet to send next
+    uint8_t tx_read_index;
 
     // sender: next seq nr used for sending
     uint8_t next_tx_seq;
 
+    // receiver: send RR frame - flag
+    uint8_t send_supervisor_frame_receiver_ready;
+
     // receiver: value of tx_seq in next expected i-frame
     uint8_t expected_tx_seq;