le central handle connection update params

src/hci.c

@@ -59,6 +59,7 @@
 #include "debug.h"
 #include "hci_dump.h"
 
+#include <btstack/linked_list.h>
 #include <btstack/hci_cmds.h>
 
 #define HCI_CONNECTION_TIMEOUT_MS 10000
@@ -108,22 +109,59 @@ static hci_connection_t * create_connection_for_bd_addr_and_type(bd_addr_t addr,
     conn->acl_recombination_length = 0;
     conn->acl_recombination_pos = 0;
     conn->num_acl_packets_sent = 0;
+    conn->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE;
     linked_list_add(&hci_stack->connections, (linked_item_t *) conn);
     return conn;
 }
 
+
+/**
+ * get le connection parameter range
+*
+ * @return le connection parameter range struct
+ */
+le_connection_parameter_range_t gap_le_get_connection_parameter_range(){
+    return hci_stack->le_connection_parameter_range;
+}
+
+/**
+ * set le connection parameter range
+ *
+ */
+
+void gap_le_set_connection_parameter_range(le_connection_parameter_range_t range){
+    hci_stack->le_connection_parameter_range.le_conn_interval_min = range.le_conn_interval_min;
+    hci_stack->le_connection_parameter_range.le_conn_interval_max = range.le_conn_interval_max;
+    hci_stack->le_connection_parameter_range.le_conn_interval_min = range.le_conn_latency_min;
+    hci_stack->le_connection_parameter_range.le_conn_interval_max = range.le_conn_latency_max;
+    hci_stack->le_connection_parameter_range.le_supervision_timeout_min = range.le_supervision_timeout_min;
+    hci_stack->le_connection_parameter_range.le_supervision_timeout_max = range.le_supervision_timeout_max;
+}
+
+/**
+ * get hci connections iterator
+ *
+ * @return hci connections iterator
+ */
+
+void hci_connections_get_iterator(linked_list_iterator_t *it){
+    linked_list_iterator_init(it, &hci_stack->connections);
+}
+
 /**
  * get connection for a given handle
  *
  * @return connection OR NULL, if not found
  */
 hci_connection_t * hci_connection_for_handle(hci_con_handle_t con_handle){
-    linked_item_t *it;
+    linked_list_iterator_t it;
-    for (it = (linked_item_t *) hci_stack->connections; it ; it = it->next){
+    linked_list_iterator_init(&it, &hci_stack->connections);
-        if ( ((hci_connection_t *) it)->con_handle == con_handle){
+    while (linked_list_iterator_has_next(&it)){
-            return (hci_connection_t *) it;
+        hci_connection_t * item = (hci_connection_t *) linked_list_iterator_next(&it);
+        if ( item->con_handle != con_handle ) {
+            return item;
         }
-    }
+    } 
     return NULL;
 }
 
@@ -133,13 +171,14 @@ hci_connection_t * hci_connection_for_handle(hci_con_handle_t con_handle){
  * @return connection OR NULL, if not found
  */
 hci_connection_t * hci_connection_for_bd_addr_and_type(bd_addr_t * addr, bd_addr_type_t addr_type){
-    linked_item_t *it;
+    linked_list_iterator_t it;
-    for (it = (linked_item_t *) hci_stack->connections; it ; it = it->next){
+    linked_list_iterator_init(&it, &hci_stack->connections);
-        hci_connection_t * connection = (hci_connection_t *) it;
+    while (linked_list_iterator_has_next(&it)){
+        hci_connection_t * connection = (hci_connection_t *) linked_list_iterator_next(&it);
         if (connection->address_type != addr_type)  continue;
         if (memcmp(addr, connection->address, 6) != 0) continue;
-        return connection;
+        return connection;   
-    }
+    } 
     return NULL;
 }
 
@@ -1374,6 +1413,12 @@ static void hci_state_reset(){
     memset(hci_stack->adv_address, 0, 6);
     hci_stack->le_scanning_state = LE_SCAN_IDLE;
     hci_stack->le_scan_type = 0xff; 
+    hci_stack->le_connection_parameter_range.le_conn_interval_min = 0x0006;
+    hci_stack->le_connection_parameter_range.le_conn_interval_max = 0x0C80;
+    hci_stack->le_connection_parameter_range.le_conn_latency_min = 0x0000;
+    hci_stack->le_connection_parameter_range.le_conn_latency_max = 0x03E8;
+    hci_stack->le_connection_parameter_range.le_supervision_timeout_min = 0x000A;
+    hci_stack->le_connection_parameter_range.le_supervision_timeout_max = 0x0C80;
 }
 
 void hci_init(hci_transport_t *transport, void *config, bt_control_t *control, remote_device_db_t const* remote_device_db){
@@ -1913,8 +1958,11 @@ void hci_run(){
             hci_send_cmd(&hci_set_connection_encryption, connection->con_handle, 1);
             return;
         }
+
 #ifdef HAVE_BLE
-        if (connection->le_conn_interval_min){
+        if (connection->le_con_parameter_update_state == CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS){
+            connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 
+            
             uint16_t connection_interval_min = connection->le_conn_interval_min;
             connection->le_conn_interval_min = 0;
             hci_send_cmd(&hci_le_connection_update, connection->con_handle, connection_interval_min,

src/hci.h

@@ -55,6 +55,7 @@
 #include <stdint.h>
 #include <stdlib.h>
 #include <stdarg.h>
+#include <btstack/linked_list.h>
 
 #if defined __cplusplus
 extern "C" {
@@ -233,7 +234,27 @@ extern "C" {
 
 // data: event(8)
 #define DAEMON_EVENT_HCI_PACKET_SENT                       0x54
-    
+
+/**
+ * LE connection parameter update state
+ */ 
+
+typedef enum {
+    CON_PARAMETER_UPDATE_NONE,
+    CON_PARAMETER_UPDATE_SEND_RESPONSE,
+    CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS,
+    CON_PARAMETER_UPDATE_DENY
+} le_con_parameter_update_state_t;
+
+typedef struct le_connection_parameter_range{
+    uint16_t le_conn_interval_min;
+    uint16_t le_conn_interval_max;
+    uint16_t le_conn_latency_min;
+    uint16_t le_conn_latency_max;
+    uint16_t le_supervision_timeout_min;
+    uint16_t le_supervision_timeout_max;
+} le_connection_parameter_range_t;
+
 /**
  * Connection State 
  */
@@ -345,10 +366,12 @@ typedef struct {
     uint8_t num_acl_packets_sent;
 
     // connection parameter update
+    le_con_parameter_update_state_t le_con_parameter_update_state;
     uint16_t le_conn_interval_min;
     uint16_t le_conn_interval_max;
     uint16_t le_conn_latency;
     uint16_t le_supervision_timeout;
+    uint16_t le_update_con_parameter_response;
 } hci_connection_t;
 
 
@@ -428,8 +451,18 @@ typedef struct {
     uint8_t  le_scan_type;
     uint16_t le_scan_interval;  
     uint16_t le_scan_window;
+
+    le_connection_parameter_range_t le_connection_parameter_range;
 } hci_stack_t;
 
+/**
+ * set connection iterator
+ */
+void hci_connections_get_iterator(linked_list_iterator_t *it);
+
+le_connection_parameter_range_t gap_le_get_connection_parameter_range();
+void gap_le_set_connection_parameter_range(le_connection_parameter_range_t range);
+
 //*************** le client start
 
 le_command_status_t le_central_start_scan(void);

src/l2cap.c

@@ -684,6 +684,34 @@ void l2cap_run(void){
                 break;
         }
     }
+
+    // send l2cap con paramter update if necessary
+    hci_connections_get_iterator(&it);
+    while(linked_list_iterator_has_next(&it)){
+        hci_connection_t * connection = (hci_connection_t *) linked_list_iterator_next(&it);
+        int result;
+
+        switch (connection->le_con_parameter_update_state){
+            case CON_PARAMETER_UPDATE_SEND_RESPONSE:
+                result = 0;
+                break;
+            case CON_PARAMETER_UPDATE_DENY:
+                result = 1;
+                break;
+            default:
+                result = -1;
+                break;
+        }
+        if (result < 0) break;
+        
+        if (!hci_can_send_acl_packet_now(connection->con_handle)) break;
+        hci_reserve_packet_buffer();
+        uint8_t *acl_buffer = hci_get_outgoing_packet_buffer();
+        connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS;
+        uint16_t len = l2cap_le_create_connection_parameter_update_response(acl_buffer, connection->con_handle, 0);
+        hci_send_acl_packet_buffer(len);
+    }
+
 }
 
 uint16_t l2cap_max_mtu(void){
@@ -1370,8 +1398,39 @@ void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
                     event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST;
                     event[1] = 8;
                     memcpy(&event[2], &packet[12], 8);
+                
+                    hci_connection_t * connection = hci_connection_for_handle(handle);
+                    if (connection){ 
+                        int update_parameter = 1;
+                        le_connection_parameter_range_t existing_range = gap_le_get_connection_parameter_range();
+                        uint16_t le_conn_interval_min = READ_BT_16(packet,12);
+                        uint16_t le_conn_interval_max = READ_BT_16(packet,14);
+                        uint16_t le_conn_latency = READ_BT_16(packet,16);
+                        uint16_t le_supervision_timeout = READ_BT_16(packet,18);
+
+                        if (le_conn_interval_min < existing_range.le_conn_interval_min) update_parameter = 0;
+                        if (le_conn_interval_max > existing_range.le_conn_interval_max) update_parameter = 0;
+                        
+                        if (le_conn_latency < existing_range.le_conn_latency_min) update_parameter = 0;
+                        if (le_conn_latency > existing_range.le_conn_latency_max) update_parameter = 0;
+
+                        if (le_supervision_timeout < existing_range.le_supervision_timeout_min) update_parameter = 0;
+                        if (le_supervision_timeout > existing_range.le_supervision_timeout_max) update_parameter = 0;
+
+                        if (update_parameter){
+                            connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE;
+                            connection->le_conn_interval_min = le_conn_interval_min;
+                            connection->le_conn_interval_max = le_conn_interval_max;
+                            connection->le_conn_latency = le_conn_latency;
+                            connection->le_supervision_timeout = le_supervision_timeout;
+                        } else {
+                            connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY;
+                        }
+                    }
+                
                     hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
                     (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, event, sizeof(event));
+
                     break;
                 }
                 default: {
@@ -1497,6 +1556,7 @@ void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint1
 }
 
 #ifdef HAVE_BLE
+
 // Request LE connection parameter update
 int l2cap_le_request_connection_parameter_update(uint16_t handle, uint16_t interval_min, uint16_t interval_max, uint16_t slave_latency, uint16_t timeout_multiplier){
     if (!hci_can_send_acl_packet_now(handle)){

src/l2cap_signaling.c

@@ -179,4 +179,30 @@ uint16_t l2cap_le_create_connection_parameter_update_request(uint8_t * acl_buffe
     bt_store_16(acl_buffer, 10, pos - 12);
     return pos;
 } 
+
+uint16_t l2cap_le_create_connection_parameter_update_response(uint8_t * acl_buffer, uint16_t handle, uint16_t response){
+
+    int pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02;
+
+    // 0 - Connection handle : PB=pb : BC=00 
+    bt_store_16(acl_buffer, 0, handle | (pb << 12) | (0 << 14));
+    // 6 - L2CAP LE Signaling channel = 5
+    bt_store_16(acl_buffer, 6, 5);
+    // 8 - Code
+    acl_buffer[8] = CONNECTION_PARAMETER_UPDATE_REQUEST;
+    // 9 - id (!= 0 sequentially)
+    acl_buffer[9] = 1;
+    uint16_t pos = 12;
+    bt_store_16(acl_buffer, pos, response);
+    pos += 2;
+    // 2 - ACL length
+    bt_store_16(acl_buffer, 2,  pos - 4);
+    // 4 - L2CAP packet length
+    bt_store_16(acl_buffer, 4,  pos - 6 - 2);
+    // 10 - L2CAP signaling parameter length
+    bt_store_16(acl_buffer, 10, pos - 12);
+    return pos;
+} 
+
+
 #endif

src/l2cap_signaling.h

@@ -72,6 +72,7 @@ typedef enum {
 uint16_t l2cap_create_signaling_classic(uint8_t * acl_buffer,hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, va_list argptr);
 uint16_t l2cap_create_signaling_le(uint8_t * acl_buffer, hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, va_list argptr);
 uint16_t l2cap_le_create_connection_parameter_update_request(uint8_t * acl_buffer, uint16_t handle, uint16_t interval_min, uint16_t interval_max, uint16_t slave_latency, uint16_t timeout_multiplier);
+uint16_t l2cap_le_create_connection_parameter_update_response(uint8_t * acl_buffer, uint16_t handle, uint16_t response);
 uint8_t  l2cap_next_sig_id(void);
 uint16_t l2cap_next_local_cid(void);