l2cap-ertm: reserve buffer for packet reassembly, too. calculate local mps and use in ertm config

src/l2cap.c

@@ -734,7 +734,7 @@ static uint16_t l2cap_setup_options_ertm(l2cap_channel_t * channel, uint8_t * co
     config_options[4] = channel->local_max_transmit;
     little_endian_store_16( config_options, 5, channel->local_retransmission_timeout_ms); 
     little_endian_store_16( config_options, 7, channel->local_monitor_timeout_ms);
-    little_endian_store_16( config_options, 9, channel->local_mtu);
+    little_endian_store_16( config_options, 9, channel->local_mps);
     return 11;
 }
 static int l2cap_ertm_send_supervisor_frame(l2cap_channel_t * channel, uint16_t control){
@@ -1345,8 +1345,9 @@ uint8_t l2cap_create_channel(btstack_packet_handler_t channel_packet_handler, bd
 
 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
 
-static uint8_t l2cap_ertm_validate_local_config(uint8_t max_transmit, uint16_t retransmission_timeout_ms,
+static uint8_t l2cap_ertm_validate_local_config(uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms,
-    uint16_t monitor_timeout_ms, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){
+    uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){
+    
     UNUSED(buffer);
     UNUSED(size);
 
@@ -1363,6 +1364,10 @@ static uint8_t l2cap_ertm_validate_local_config(uint8_t max_transmit, uint16_t r
         log_error("monitor_timeout_ms must be >= 12000 ms");
         result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
     }
+    if (local_mtu < 48){
+        log_error("local_mtu must be >= 48");
+        result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
+    }
     if (num_rx_buffers < 1){
         log_error("num_rx_buffers must be >= 1");
         result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
@@ -1374,43 +1379,50 @@ static uint8_t l2cap_ertm_validate_local_config(uint8_t max_transmit, uint16_t r
     return result;
 }
 
-static void l2cap_ertm_configure_channel(l2cap_channel_t * channel, int ertm_mandatory, uint8_t max_transmit,
+static void l2cap_ertm_configure_channel(l2cap_channel_t * channel, int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms,
-    uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){
+    uint16_t monitor_timeout_ms, uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){
 
     channel->mode  = L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION;
     channel->ertm_mandatory = ertm_mandatory;
     channel->local_max_transmit = max_transmit;
     channel->local_retransmission_timeout_ms = retransmission_timeout_ms;
     channel->local_monitor_timeout_ms = monitor_timeout_ms;
+    channel->local_mtu = local_mtu;
     channel->num_rx_buffers = num_rx_buffers;
     channel->num_tx_buffers = num_tx_buffers;
 
-    // TODO: align buffer pointer
+    // align buffer to 16-byte boundary, just in case
+    int bytes_till_alignment = 16 - (((uintptr_t) buffer) & 0x0f);
+    buffer += bytes_till_alignment;
+    size   -= bytes_till_alignment;
+
+    // setup state buffers
     uint32_t pos = 0;
     channel->rx_packets_state = (l2cap_ertm_rx_packet_state_t *) &buffer[pos];
     pos += num_rx_buffers * sizeof(l2cap_ertm_rx_packet_state_t);
     channel->tx_packets_state = (l2cap_ertm_tx_packet_state_t *) &buffer[pos];
     pos += num_tx_buffers * sizeof(l2cap_ertm_tx_packet_state_t);
-    // calculate MTU
+
-    channel->local_mtu = (size - pos) / (num_rx_buffers + num_tx_buffers);
+    // setup reassembly buffer
-    log_info("Local ERTM MTU: %u", channel->local_mtu);
+    channel->reassembly_buffer = &buffer[pos];
+    pos += local_mtu;
+
+    // divide rest of data equally
+    channel->local_mps = (size - pos) / (num_rx_buffers + num_tx_buffers);
+    log_info("Local MPS: %u", channel->local_mtu);
     channel->rx_packets_data = &buffer[pos];
     pos += num_rx_buffers * channel->local_mtu;
     channel->tx_packets_data = &buffer[pos];
-    log_info("RX packets %p, TX packets %p", channel->rx_packets_data, channel->tx_packets_data);
 }
 
 uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, 
     int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms,
-    uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size, uint16_t * out_local_cid){
+    uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size, uint16_t * out_local_cid){
 
-    // limit MTU to the size of our outtgoing HCI buffer
+    log_info("L2CAP_CREATE_CHANNEL addr %s, psm 0x%x, local mtu %u", bd_addr_to_str(address), psm, local_mtu);
-    uint16_t local_mtu = l2cap_max_mtu();
-
-    log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x -> local mtu %u", bd_addr_to_str(address), psm, local_mtu);
 
     // validate local config
-    uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size);
+    uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, local_mtu, num_tx_buffers, num_rx_buffers, buffer, size);
     if (result) return result;
 
     l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0);
@@ -1420,7 +1432,7 @@ uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_ad
 
     // configure ERTM
     l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms,
-        monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size);
+        local_mtu, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size);
 
     // add to connections list
     btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel);
@@ -1785,8 +1797,8 @@ void l2cap_accept_connection(uint16_t local_cid){
 
 
 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
-uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, int ertm_mandatory, uint8_t max_transmit,
+uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms,
-    uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){
+     uint16_t monitor_timeout_ms, uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){
 
     log_info("L2CAP_ACCEPT_ERTM_CONNECTION local_cid 0x%x", local_cid);
     l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid);
@@ -1796,11 +1808,11 @@ uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, int ertm_mandatory, uin
     }
 
     // validate local config
-    uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size);
+    uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, local_mtu, num_tx_buffers, num_rx_buffers, buffer, size);
     if (result) return result;
 
     // configure L2CAP ERTM
-    l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size);
+    l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, monitor_timeout_ms, local_mtu, num_tx_buffers, num_rx_buffers, buffer, size);
 
     // continue
     channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT;

src/l2cap.h

@@ -257,23 +257,30 @@ typedef struct {
     // local busy condition
     uint8_t local_busy;
 
-    // max um out-of-order packets // tx_window
+    // max num out-of-order packets // tx_window
     uint8_t num_rx_buffers;
 
     // max num of unacknowledged outgoing packets
     uint8_t num_tx_buffers;
 
+    // local mps = size of rx/tx packets
+    uint16_t local_mps;
+
     // re-assembly state
     l2cap_ertm_rx_packet_state_t * rx_packets_state;
 
     // retransmission state
     l2cap_ertm_tx_packet_state_t * tx_packets_state;
 
+    // reassembly buffer
+    uint8_t * reassembly_buffer;
+
     // data, each of size local_mtu
     uint8_t * rx_packets_data;
 
     // data, each of size local_mtu
     uint8_t * tx_packets_data;
+
 #endif    
 } l2cap_channel_t;
 
@@ -362,16 +369,18 @@ uint8_t l2cap_create_channel(btstack_packet_handler_t packet_handler, bd_addr_t
  * @param max_transmit Number of retransmissions that L2CAP is allowed to try before accepting that a packet and the channel is lost.
  * @param retransmission_timeout_ms Recommended : 2000 ms (ACL Flush timeout not used)
  * @param monitor_timeout_ms Recommended: 12000 ms (ACL Flush timeout not used)
+ * @param local_mtu 
  * @param num_tx_buffers Number of unacknowledged packets stored in buffer
  * @param num_rx_buffers Number of packets that can be received out of order (-> our tx_window size)
- * @param buffer to store out-of-order packets and unacknowledged outgoing packets with their tretransmission timers
+ * @param buffer to store reassembled rx packet, out-of-order packets and unacknowledged outgoing packets with their tretransmission timers
  * @param size of buffer
  * @param local_cid
  * @return status
  */
 uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, 
     int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms,
-    uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size, uint16_t * out_local_cid);
+    uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size,
+    uint16_t * out_local_cid);
 
 /** 
  * @brief Disconnects L2CAP channel with given identifier. 
@@ -409,14 +418,15 @@ void l2cap_accept_connection(uint16_t local_cid);
  * @param max_transmit Number of retransmissions that L2CAP is allowed to try before accepting that a packet and the channel is lost. Recommended: 1
  * @param retransmission_timeout_ms Recommended : 2000 ms (ACL Flush timeout not used)
  * @param monitor_timeout_ms Recommended: 12000 ms (ACL Flush timeout not used)
+ * @param local_mtu 
  * @param num_tx_buffers Number of unacknowledged packets stored in buffer
  * @param num_rx_buffers Number of packets that can be received out of order (-> our tx_window size)
- * @param buffer to store out-of-order packets and unacknowledged outgoing packets with their tretransmission timers
+ * @param buffer to store reassembled rx packet, out-of-order packets and unacknowledged outgoing packets with their tretransmission timers
  * @param size of buffer
  * @return status
  */
 uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, int ertm_mandatory, uint8_t max_transmit,
-    uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size);
+    uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size);
 
 /** 
  * @brief Deny incoming L2CAP connection.

test/pts/l2cap_test.c

@@ -70,6 +70,7 @@ static uint16_t local_cid;
 static int l2cap_ertm;
 static int l2cap_ertm_mandatory;
 static int l2cap_max_transmit = 2;  // some tests require > 1
+static int l2cap_local_mtu = 144;   
 
 static btstack_packet_callback_registration_t hci_event_callback_registration;
 static uint8_t ertm_buffer[10000];
@@ -114,7 +115,7 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack
                     uint16_t l2cap_cid  = little_endian_read_16(packet, 12);
                     if (l2cap_ertm){
                         printf("L2CAP Accepting incoming connection request in ERTM\n"); 
-                        l2cap_accept_ertm_connection(l2cap_cid, l2cap_ertm_mandatory, l2cap_max_transmit, 2000, 12000, 4, 4, ertm_buffer, sizeof(ertm_buffer));
+                        l2cap_accept_ertm_connection(l2cap_cid, l2cap_ertm_mandatory, l2cap_max_transmit, 2000, 12000, l2cap_local_mtu, 4, 4, ertm_buffer, sizeof(ertm_buffer));
                     } else {
                         printf("L2CAP Accepting incoming connection request in Basic Mode\n"); 
                         l2cap_accept_connection(l2cap_cid);
@@ -157,7 +158,7 @@ static void stdin_process(char buffer){
         case 'c':
             printf("Creating L2CAP Connection to %s, PSM SDP\n", bd_addr_to_str(remote));
             if (l2cap_ertm){
-                l2cap_create_ertm_channel(packet_handler, remote, BLUETOOTH_PROTOCOL_SDP, l2cap_ertm_mandatory, l2cap_max_transmit, 2000, 12000, 4, 4, ertm_buffer, sizeof(ertm_buffer), &local_cid);
+                l2cap_create_ertm_channel(packet_handler, remote, BLUETOOTH_PROTOCOL_SDP, l2cap_ertm_mandatory, l2cap_max_transmit, 2000, 12000, l2cap_local_mtu, 4, 4, ertm_buffer, sizeof(ertm_buffer), &local_cid);
             } else {
                 l2cap_create_channel(packet_handler, remote, BLUETOOTH_PROTOCOL_SDP, 100, &local_cid);
             }