1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define __BTSTACK_FILE__ "l2cap.c" 39 40 /* 41 * l2cap.c 42 * 43 * Logical Link Control and Adaption Protocl (L2CAP) 44 * 45 * Created by Matthias Ringwald on 5/16/09. 46 */ 47 48 #include "l2cap.h" 49 #include "hci.h" 50 #include "hci_dump.h" 51 #include "bluetooth_sdp.h" 52 #include "btstack_debug.h" 53 #include "btstack_event.h" 54 #include "btstack_memory.h" 55 56 #ifdef ENABLE_LE_DATA_CHANNELS 57 #include "ble/sm.h" 58 #endif 59 60 #include <stdarg.h> 61 #include <string.h> 62 63 #include <stdio.h> 64 65 // nr of buffered acl packets in outgoing queue to get max performance 66 #define NR_BUFFERED_ACL_PACKETS 3 67 68 // used to cache l2cap rejects, echo, and informational requests 69 #define NR_PENDING_SIGNALING_RESPONSES 3 70 71 // nr of credits provided to remote if credits fall below watermark 72 #define L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK 5 73 #define L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT 5 74 75 // offsets for L2CAP SIGNALING COMMANDS 76 #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET 0 77 #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET 1 78 #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2 79 #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET 4 80 81 // internal table 82 #define L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL 0 83 #define L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL 1 84 #define L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL 2 85 #define L2CAP_FIXED_CHANNEL_TABLE_SIZE (L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL+1) 86 87 #if defined(ENABLE_LE_DATA_CHANNELS) || defined(ENABLE_CLASSIC) 88 #define L2CAP_USES_CHANNELS 89 #endif 90 91 // prototypes 92 static void l2cap_run(void); 93 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 94 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size ); 95 static void l2cap_notify_channel_can_send(void); 96 static void l2cap_emit_can_send_now(btstack_packet_handler_t packet_handler, uint16_t channel); 97 #ifdef ENABLE_CLASSIC 98 static void l2cap_finialize_channel_close(l2cap_channel_t *channel); 99 static inline l2cap_service_t * l2cap_get_service(uint16_t psm); 100 static void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status); 101 static void l2cap_emit_channel_closed(l2cap_channel_t *channel); 102 static void l2cap_emit_incoming_connection(l2cap_channel_t *channel); 103 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel); 104 #endif 105 #ifdef ENABLE_LE_DATA_CHANNELS 106 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status); 107 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel); 108 static l2cap_channel_t * l2cap_le_get_channel_for_local_cid(uint16_t local_cid); 109 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel); 110 static void l2cap_le_finialize_channel_close(l2cap_channel_t *channel); 111 static inline l2cap_service_t * l2cap_le_get_service(uint16_t psm); 112 #endif 113 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 114 static void l2cap_ertm_notify_channel_can_send(l2cap_channel_t * channel); 115 static int l2cap_ertm_num_unacknowledged_tx_packets(l2cap_channel_t * channel); 116 static void l2cap_ertm_monitor_timeout_callback(btstack_timer_source_t * ts); 117 static void l2cap_ertm_retransmission_timeout_callback(btstack_timer_source_t * ts); 118 #endif 119 120 typedef struct l2cap_fixed_channel { 121 btstack_packet_handler_t callback; 122 uint8_t waiting_for_can_send_now; 123 } l2cap_fixed_channel_t; 124 125 #ifdef ENABLE_CLASSIC 126 static btstack_linked_list_t l2cap_channels; 127 static btstack_linked_list_t l2cap_services; 128 static uint8_t require_security_level2_for_outgoing_sdp; 129 #endif 130 131 #ifdef ENABLE_LE_DATA_CHANNELS 132 static btstack_linked_list_t l2cap_le_channels; 133 static btstack_linked_list_t l2cap_le_services; 134 #endif 135 136 // used to cache l2cap rejects, echo, and informational requests 137 static l2cap_signaling_response_t signaling_responses[NR_PENDING_SIGNALING_RESPONSES]; 138 static int signaling_responses_pending; 139 140 static btstack_packet_callback_registration_t hci_event_callback_registration; 141 static l2cap_fixed_channel_t fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_SIZE]; 142 143 #ifdef ENABLE_BLE 144 // only used for connection parameter update events 145 static btstack_packet_handler_t l2cap_event_packet_handler; 146 #endif 147 148 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 149 150 /* 151 * CRC lookup table for generator polynom D^16 + D^15 + D^2 + 1 152 */ 153 static const uint16_t crc16_table[256] = { 154 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, 155 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40, 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, 156 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40, 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41, 157 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641, 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040, 158 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, 159 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, 160 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40, 161 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640, 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041, 162 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240, 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441, 163 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, 164 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, 165 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640, 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041, 166 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241, 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440, 167 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, 168 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, 169 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040, 170 }; 171 172 static uint16_t crc16_calc(uint8_t * data, uint16_t len){ 173 uint16_t crc = 0; // initial value = 0 174 while (len--){ 175 crc = (crc >> 8) ^ crc16_table[ (crc ^ ((uint16_t) *data++)) & 0x00FF ]; 176 } 177 return crc; 178 } 179 180 #endif 181 182 183 static uint16_t l2cap_fixed_channel_table_channel_id_for_index(int index){ 184 switch (index){ 185 case L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL: 186 return L2CAP_CID_ATTRIBUTE_PROTOCOL; 187 case L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL: 188 return L2CAP_CID_SECURITY_MANAGER_PROTOCOL; 189 case L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL: 190 return L2CAP_CID_CONNECTIONLESS_CHANNEL; 191 default: 192 return 0; 193 } 194 } 195 static int l2cap_fixed_channel_table_index_for_channel_id(uint16_t channel_id){ 196 switch (channel_id){ 197 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 198 return L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL; 199 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 200 return L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL; 201 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 202 return L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL; 203 default: 204 return -1; 205 } 206 } 207 208 static int l2cap_fixed_channel_table_index_is_le(int index){ 209 if (index == L2CAP_CID_CONNECTIONLESS_CHANNEL) return 0; 210 return 1; 211 } 212 213 void l2cap_init(void){ 214 signaling_responses_pending = 0; 215 216 #ifdef ENABLE_CLASSIC 217 l2cap_channels = NULL; 218 l2cap_services = NULL; 219 require_security_level2_for_outgoing_sdp = 0; 220 #endif 221 222 #ifdef ENABLE_LE_DATA_CHANNELS 223 l2cap_le_services = NULL; 224 l2cap_le_channels = NULL; 225 #endif 226 227 #ifdef ENABLE_BLE 228 l2cap_event_packet_handler = NULL; 229 #endif 230 memset(fixed_channels, 0, sizeof(fixed_channels)); 231 232 // 233 // register callback with HCI 234 // 235 hci_event_callback_registration.callback = &l2cap_hci_event_handler; 236 hci_add_event_handler(&hci_event_callback_registration); 237 238 hci_register_acl_packet_handler(&l2cap_acl_handler); 239 240 #ifdef ENABLE_CLASSIC 241 gap_connectable_control(0); // no services yet 242 #endif 243 } 244 245 void l2cap_register_packet_handler(void (*handler)(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 246 #ifdef ENABLE_BLE 247 l2cap_event_packet_handler = handler; 248 #else 249 UNUSED(handler); 250 #endif 251 } 252 253 void l2cap_request_can_send_fix_channel_now_event(hci_con_handle_t con_handle, uint16_t channel_id){ 254 UNUSED(con_handle); 255 256 int index = l2cap_fixed_channel_table_index_for_channel_id(channel_id); 257 if (index < 0) return; 258 fixed_channels[index].waiting_for_can_send_now = 1; 259 l2cap_notify_channel_can_send(); 260 } 261 262 int l2cap_can_send_fixed_channel_packet_now(hci_con_handle_t con_handle, uint16_t channel_id){ 263 UNUSED(channel_id); 264 265 return hci_can_send_acl_packet_now(con_handle); 266 } 267 268 uint8_t *l2cap_get_outgoing_buffer(void){ 269 return hci_get_outgoing_packet_buffer() + COMPLETE_L2CAP_HEADER; // 8 bytes 270 } 271 272 int l2cap_reserve_packet_buffer(void){ 273 return hci_reserve_packet_buffer(); 274 } 275 276 void l2cap_release_packet_buffer(void){ 277 hci_release_packet_buffer(); 278 } 279 280 static void l2cap_setup_header(uint8_t * acl_buffer, hci_con_handle_t con_handle, uint8_t packet_boundary, uint16_t remote_cid, uint16_t len){ 281 // 0 - Connection handle : PB=pb : BC=00 282 little_endian_store_16(acl_buffer, 0, con_handle | (packet_boundary << 12) | (0 << 14)); 283 // 2 - ACL length 284 little_endian_store_16(acl_buffer, 2, len + 4); 285 // 4 - L2CAP packet length 286 little_endian_store_16(acl_buffer, 4, len + 0); 287 // 6 - L2CAP channel DEST 288 little_endian_store_16(acl_buffer, 6, remote_cid); 289 } 290 291 // assumption - only on LE connections 292 int l2cap_send_prepared_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint16_t len){ 293 294 if (!hci_is_packet_buffer_reserved()){ 295 log_error("l2cap_send_prepared_connectionless called without reserving packet first"); 296 return BTSTACK_ACL_BUFFERS_FULL; 297 } 298 299 if (!hci_can_send_prepared_acl_packet_now(con_handle)){ 300 log_info("l2cap_send_prepared_connectionless handle 0x%02x, cid 0x%02x, cannot send", con_handle, cid); 301 return BTSTACK_ACL_BUFFERS_FULL; 302 } 303 304 log_debug("l2cap_send_prepared_connectionless handle %u, cid 0x%02x", con_handle, cid); 305 306 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 307 l2cap_setup_header(acl_buffer, con_handle, 0, cid, len); 308 // send 309 return hci_send_acl_packet_buffer(len+8); 310 } 311 312 // assumption - only on LE connections 313 int l2cap_send_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint8_t *data, uint16_t len){ 314 315 if (!hci_can_send_acl_packet_now(con_handle)){ 316 log_info("l2cap_send cid 0x%02x, cannot send", cid); 317 return BTSTACK_ACL_BUFFERS_FULL; 318 } 319 320 hci_reserve_packet_buffer(); 321 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 322 323 memcpy(&acl_buffer[8], data, len); 324 325 return l2cap_send_prepared_connectionless(con_handle, cid, len); 326 } 327 328 static void l2cap_emit_can_send_now(btstack_packet_handler_t packet_handler, uint16_t channel) { 329 log_debug("L2CAP_EVENT_CHANNEL_CAN_SEND_NOW local_cid 0x%x", channel); 330 uint8_t event[4]; 331 event[0] = L2CAP_EVENT_CAN_SEND_NOW; 332 event[1] = sizeof(event) - 2; 333 little_endian_store_16(event, 2, channel); 334 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 335 packet_handler(HCI_EVENT_PACKET, channel, event, sizeof(event)); 336 } 337 338 #ifdef L2CAP_USES_CHANNELS 339 static void l2cap_dispatch_to_channel(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){ 340 (* (channel->packet_handler))(type, channel->local_cid, data, size); 341 } 342 343 static void l2cap_emit_simple_event_with_cid(l2cap_channel_t * channel, uint8_t event_code){ 344 uint8_t event[4]; 345 event[0] = event_code; 346 event[1] = sizeof(event) - 2; 347 little_endian_store_16(event, 2, channel->local_cid); 348 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 349 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 350 } 351 #endif 352 353 #ifdef ENABLE_CLASSIC 354 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 355 log_info("L2CAP_EVENT_CHANNEL_OPENED status 0x%x addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x local_mtu %u, remote_mtu %u, flush_timeout %u", 356 status, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 357 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu, channel->flush_timeout); 358 uint8_t event[24]; 359 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 360 event[1] = sizeof(event) - 2; 361 event[2] = status; 362 reverse_bd_addr(channel->address, &event[3]); 363 little_endian_store_16(event, 9, channel->con_handle); 364 little_endian_store_16(event, 11, channel->psm); 365 little_endian_store_16(event, 13, channel->local_cid); 366 little_endian_store_16(event, 15, channel->remote_cid); 367 little_endian_store_16(event, 17, channel->local_mtu); 368 little_endian_store_16(event, 19, channel->remote_mtu); 369 little_endian_store_16(event, 21, channel->flush_timeout); 370 event[23] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 371 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 372 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 373 } 374 375 static void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 376 log_info("L2CAP_EVENT_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 377 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 378 } 379 380 static void l2cap_emit_incoming_connection(l2cap_channel_t *channel) { 381 log_info("L2CAP_EVENT_INCOMING_CONNECTION addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x", 382 bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid); 383 uint8_t event[16]; 384 event[0] = L2CAP_EVENT_INCOMING_CONNECTION; 385 event[1] = sizeof(event) - 2; 386 reverse_bd_addr(channel->address, &event[2]); 387 little_endian_store_16(event, 8, channel->con_handle); 388 little_endian_store_16(event, 10, channel->psm); 389 little_endian_store_16(event, 12, channel->local_cid); 390 little_endian_store_16(event, 14, channel->remote_cid); 391 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 392 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 393 } 394 395 static l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){ 396 btstack_linked_list_iterator_t it; 397 btstack_linked_list_iterator_init(&it, &l2cap_channels); 398 while (btstack_linked_list_iterator_has_next(&it)){ 399 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 400 if ( channel->local_cid == local_cid) { 401 return channel; 402 } 403 } 404 return NULL; 405 } 406 407 /// 408 409 void l2cap_request_can_send_now_event(uint16_t local_cid){ 410 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 411 if (!channel) return; 412 channel->waiting_for_can_send_now = 1; 413 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 414 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 415 l2cap_ertm_notify_channel_can_send(channel); 416 return; 417 } 418 #endif 419 l2cap_notify_channel_can_send(); 420 } 421 422 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 423 static int l2cap_ertm_can_store_packet_now(l2cap_channel_t * channel){ 424 // get num free tx buffers 425 int num_tx_buffers_used = channel->tx_write_index - channel->tx_read_index; 426 if (num_tx_buffers_used < 0){ 427 num_tx_buffers_used += channel->num_tx_buffers; 428 } 429 int num_free_tx_buffers = channel->num_tx_buffers - num_tx_buffers_used; 430 // calculate num tx buffers for remote MTU 431 int num_tx_buffers_for_max_remote_mtu; 432 if (channel->remote_mtu <= channel->remote_mps){ 433 // MTU fits into single packet 434 num_tx_buffers_for_max_remote_mtu = 1; 435 } else { 436 // include SDU Length 437 num_tx_buffers_for_max_remote_mtu = (channel->remote_mtu + 2 + (channel->remote_mps - 1)) / channel->remote_mps; 438 } 439 return num_tx_buffers_for_max_remote_mtu <= num_free_tx_buffers; 440 } 441 #endif 442 443 int l2cap_can_send_packet_now(uint16_t local_cid){ 444 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 445 if (!channel) return 0; 446 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 447 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 448 return l2cap_ertm_can_store_packet_now(channel); 449 } 450 #endif 451 return hci_can_send_acl_packet_now(channel->con_handle); 452 } 453 454 int l2cap_can_send_prepared_packet_now(uint16_t local_cid){ 455 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 456 if (!channel) return 0; 457 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 458 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 459 return 0; 460 } 461 #endif 462 return hci_can_send_prepared_acl_packet_now(channel->con_handle); 463 } 464 465 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){ 466 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 467 if (channel) { 468 return channel->remote_mtu; 469 } 470 return 0; 471 } 472 473 static l2cap_channel_t * l2cap_channel_for_rtx_timer(btstack_timer_source_t * ts){ 474 btstack_linked_list_iterator_t it; 475 btstack_linked_list_iterator_init(&it, &l2cap_channels); 476 while (btstack_linked_list_iterator_has_next(&it)){ 477 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 478 if ( &channel->rtx == ts) { 479 return channel; 480 } 481 } 482 return NULL; 483 } 484 485 static void l2cap_rtx_timeout(btstack_timer_source_t * ts){ 486 l2cap_channel_t * channel = l2cap_channel_for_rtx_timer(ts); 487 if (!channel) return; 488 489 log_info("l2cap_rtx_timeout for local cid 0x%02x", channel->local_cid); 490 491 // "When terminating the channel, it is not necessary to send a L2CAP_DisconnectReq 492 // and enter WAIT_DISCONNECT state. Channels can be transitioned directly to the CLOSED state." 493 // notify client 494 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_RTX_TIMEOUT); 495 496 // discard channel 497 // no need to stop timer here, it is removed from list during timer callback 498 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 499 btstack_memory_l2cap_channel_free(channel); 500 } 501 502 static void l2cap_stop_rtx(l2cap_channel_t * channel){ 503 log_info("l2cap_stop_rtx for local cid 0x%02x", channel->local_cid); 504 btstack_run_loop_remove_timer(&channel->rtx); 505 } 506 507 static void l2cap_start_rtx(l2cap_channel_t * channel){ 508 l2cap_stop_rtx(channel); 509 log_info("l2cap_start_rtx for local cid 0x%02x", channel->local_cid); 510 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 511 btstack_run_loop_set_timer(&channel->rtx, L2CAP_RTX_TIMEOUT_MS); 512 btstack_run_loop_add_timer(&channel->rtx); 513 } 514 515 static void l2cap_start_ertx(l2cap_channel_t * channel){ 516 log_info("l2cap_start_ertx for local cid 0x%02x", channel->local_cid); 517 l2cap_stop_rtx(channel); 518 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 519 btstack_run_loop_set_timer(&channel->rtx, L2CAP_ERTX_TIMEOUT_MS); 520 btstack_run_loop_add_timer(&channel->rtx); 521 } 522 523 void l2cap_require_security_level_2_for_outgoing_sdp(void){ 524 require_security_level2_for_outgoing_sdp = 1; 525 } 526 527 static int l2cap_security_level_0_allowed_for_PSM(uint16_t psm){ 528 return (psm == BLUETOOTH_PROTOCOL_SDP) && (!require_security_level2_for_outgoing_sdp); 529 } 530 531 static int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 532 if (!hci_can_send_acl_packet_now(handle)){ 533 log_info("l2cap_send_signaling_packet, cannot send"); 534 return BTSTACK_ACL_BUFFERS_FULL; 535 } 536 537 // log_info("l2cap_send_signaling_packet type %u", cmd); 538 hci_reserve_packet_buffer(); 539 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 540 va_list argptr; 541 va_start(argptr, identifier); 542 uint16_t len = l2cap_create_signaling_classic(acl_buffer, handle, cmd, identifier, argptr); 543 va_end(argptr); 544 // log_info("l2cap_send_signaling_packet con %u!", handle); 545 return hci_send_acl_packet_buffer(len); 546 } 547 548 // assumption - only on Classic connections 549 int l2cap_send_prepared(uint16_t local_cid, uint16_t len){ 550 551 if (!hci_is_packet_buffer_reserved()){ 552 log_error("l2cap_send_prepared called without reserving packet first"); 553 return BTSTACK_ACL_BUFFERS_FULL; 554 } 555 556 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 557 if (!channel) { 558 log_error("l2cap_send_prepared no channel for cid 0x%02x", local_cid); 559 return -1; // TODO: define error 560 } 561 562 if (!hci_can_send_prepared_acl_packet_now(channel->con_handle)){ 563 log_info("l2cap_send_prepared cid 0x%02x, cannot send", local_cid); 564 return BTSTACK_ACL_BUFFERS_FULL; 565 } 566 567 log_debug("l2cap_send_prepared cid 0x%02x, handle %u, 1 credit used", local_cid, channel->con_handle); 568 569 int fcs_size = 0; 570 571 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 572 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 573 fcs_size = 2; 574 } 575 #endif 576 577 // set non-flushable packet boundary flag if supported on Controller 578 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 579 uint8_t packet_boundary_flag = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 580 l2cap_setup_header(acl_buffer, channel->con_handle, packet_boundary_flag, channel->remote_cid, len + fcs_size); 581 582 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 583 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 584 // calculate FCS over l2cap data 585 uint16_t fcs = crc16_calc(acl_buffer + 4, 4 + len); 586 log_info("I-Frame: fcs 0x%04x", fcs); 587 little_endian_store_16(acl_buffer, 8 + len, fcs); 588 } 589 #endif 590 591 // send 592 return hci_send_acl_packet_buffer(len+8+fcs_size); 593 } 594 595 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 596 static inline uint16_t l2cap_encanced_control_field_for_information_frame(uint8_t tx_seq, int final, uint8_t req_seq, l2cap_segmentation_and_reassembly_t sar){ 597 return (((uint16_t) sar) << 14) | (req_seq << 8) | (final << 7) | (tx_seq << 1) | 0; 598 } 599 static inline uint16_t l2cap_encanced_control_field_for_supevisor_frame(l2cap_supervisory_function_t supervisory_function, int poll, int final, uint8_t req_seq){ 600 return (req_seq << 8) | (final << 7) | (poll << 4) | (((int) supervisory_function) << 2) | 1; 601 } 602 static int l2cap_next_ertm_seq_nr(int seq_nr){ 603 return (seq_nr + 1) & 0x3f; 604 } 605 static void l2cap_ertm_next_tx_write_index(l2cap_channel_t * channel){ 606 channel->tx_write_index++; 607 if (channel->tx_write_index < channel->num_tx_buffers) return; 608 channel->tx_write_index = 0; 609 } 610 611 static void l2cap_ertm_start_monitor_timer(l2cap_channel_t * channel){ 612 btstack_run_loop_remove_timer(&channel->monitor_timer); 613 btstack_run_loop_set_timer_handler(&channel->monitor_timer, &l2cap_ertm_monitor_timeout_callback); 614 btstack_run_loop_set_timer_context(&channel->monitor_timer, channel); 615 btstack_run_loop_set_timer(&channel->monitor_timer, channel->local_monitor_timeout_ms); 616 btstack_run_loop_add_timer(&channel->monitor_timer); 617 } 618 619 // static void l2cap_ertm_stop_monitor_timer(l2cap_channel_t * channel){ 620 // } 621 622 static void l2cap_ertm_start_retransmission_timer(l2cap_channel_t * channel){ 623 btstack_run_loop_remove_timer(&channel->retransmission_timer); 624 btstack_run_loop_set_timer_handler(&channel->retransmission_timer, &l2cap_ertm_retransmission_timeout_callback); 625 btstack_run_loop_set_timer_context(&channel->retransmission_timer, channel); 626 btstack_run_loop_set_timer(&channel->retransmission_timer, channel->local_retransmission_timeout_ms); 627 btstack_run_loop_add_timer(&channel->retransmission_timer); 628 } 629 630 static void l2cap_ertm_stop_retransmission_timer(l2cap_channel_t * l2cap_channel){ 631 btstack_run_loop_remove_timer(&l2cap_channel->retransmission_timer); 632 } 633 634 static void l2cap_ertm_monitor_timeout_callback(btstack_timer_source_t * ts){ 635 log_info("l2cap_ertm_monitor_timeout_callback"); 636 l2cap_channel_t * l2cap_channel = (l2cap_channel_t *) btstack_run_loop_get_timer_context(ts); 637 638 // TODO: we assume that it's the oldest packet 639 l2cap_ertm_tx_packet_state_t * tx_state; 640 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 641 642 // check retry count 643 if (tx_state->retry_count < l2cap_channel->remote_max_transmit){ 644 // increment retry count 645 tx_state->retry_count++; 646 647 l2cap_ertm_start_monitor_timer(l2cap_channel); 648 649 // send RR/P=1 650 l2cap_channel->send_supervisor_frame_receiver_ready_poll = 1; 651 } else { 652 log_info("Monitor timer expired & retry count >= max transmit -> disconnect"); 653 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 654 } 655 l2cap_run(); 656 } 657 658 static void l2cap_ertm_retransmission_timeout_callback(btstack_timer_source_t * ts){ 659 log_info("l2cap_ertm_retransmission_timeout_callback"); 660 l2cap_channel_t * l2cap_channel = (l2cap_channel_t *) btstack_run_loop_get_timer_context(ts); 661 662 // TODO: we assume that it's the oldest packet 663 l2cap_ertm_tx_packet_state_t * tx_state; 664 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 665 666 // set retry count = 1 667 tx_state->retry_count = 1; 668 669 // start monitor timer 670 l2cap_ertm_start_monitor_timer(l2cap_channel); 671 672 // send RR/P=1 673 l2cap_channel->send_supervisor_frame_receiver_ready_poll = 1; 674 l2cap_run(); 675 } 676 677 static int l2cap_ertm_send_information_frame(l2cap_channel_t * channel, int index, int final){ 678 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index]; 679 hci_reserve_packet_buffer(); 680 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 681 uint16_t control = l2cap_encanced_control_field_for_information_frame(tx_state->tx_seq, final, channel->req_seq, tx_state->sar); 682 log_info("I-Frame: control 0x%04x", control); 683 little_endian_store_16(acl_buffer, 8, control); 684 memcpy(&acl_buffer[8+2], &channel->tx_packets_data[index * channel->local_mtu], tx_state->len); 685 // send 686 return l2cap_send_prepared(channel->local_cid, 2 + tx_state->len); 687 } 688 689 690 691 static void l2cap_ertm_store_fragment(l2cap_channel_t * channel, l2cap_segmentation_and_reassembly_t sar, uint16_t sdu_length, uint8_t * data, uint16_t len){ 692 // get next index for storing packets 693 int index = channel->tx_write_index; 694 695 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index]; 696 tx_state->tx_seq = channel->next_tx_seq; 697 tx_state->len = len; 698 tx_state->sar = sar; 699 tx_state->retry_count = 0; 700 701 uint8_t * tx_packet = &channel->tx_packets_data[index * channel->local_mtu]; 702 int pos = 0; 703 if (sar == L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU){ 704 little_endian_store_16(tx_packet, 0, sdu_length); 705 pos += 2; 706 } 707 memcpy(&tx_packet[pos], data, len); 708 709 // update 710 channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq); 711 l2cap_ertm_next_tx_write_index(channel); 712 713 l2cap_ertm_start_retransmission_timer(channel); 714 } 715 716 static int l2cap_ertm_send(l2cap_channel_t * channel, uint8_t * data, uint16_t len){ 717 if (len > channel->remote_mtu){ 718 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", channel->local_cid); 719 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 720 } 721 722 // check if it needs to get fragmented 723 if (len > channel->remote_mps){ 724 // fragmentation needed. 725 l2cap_segmentation_and_reassembly_t sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU; 726 int chunk_len; 727 while (len){ 728 switch (sar){ 729 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 730 chunk_len = channel->remote_mps - 2; // sdu_length 731 l2cap_ertm_store_fragment(channel, sar, len, data, chunk_len); 732 len -= chunk_len; 733 sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU; 734 break; 735 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 736 chunk_len = channel->remote_mps; 737 if (chunk_len >= len){ 738 sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU; 739 chunk_len = len; 740 } 741 l2cap_ertm_store_fragment(channel, sar, len, data, chunk_len); 742 len -= chunk_len; 743 break; 744 default: 745 break; 746 } 747 } 748 749 } else { 750 l2cap_ertm_store_fragment(channel, L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU, 0, data, len); 751 } 752 753 // try to send 754 l2cap_run(); 755 return 0; 756 } 757 #endif 758 759 // assumption - only on Classic connections 760 int l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){ 761 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 762 if (!channel) { 763 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 764 return -1; // TODO: define error 765 } 766 767 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 768 // send in ERTM 769 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 770 return l2cap_ertm_send(channel, data, len); 771 } 772 #endif 773 774 if (len > channel->remote_mtu){ 775 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 776 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 777 } 778 779 if (!hci_can_send_acl_packet_now(channel->con_handle)){ 780 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 781 return BTSTACK_ACL_BUFFERS_FULL; 782 } 783 784 hci_reserve_packet_buffer(); 785 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 786 memcpy(&acl_buffer[8], data, len); 787 return l2cap_send_prepared(local_cid, len); 788 } 789 790 int l2cap_send_echo_request(hci_con_handle_t con_handle, uint8_t *data, uint16_t len){ 791 return l2cap_send_signaling_packet(con_handle, ECHO_REQUEST, 0x77, len, data); 792 } 793 794 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 795 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var | flag); 796 } 797 798 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 799 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var & ~flag); 800 } 801 #endif 802 803 804 #ifdef ENABLE_BLE 805 static int l2cap_send_le_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 806 807 if (!hci_can_send_acl_packet_now(handle)){ 808 log_info("l2cap_send_le_signaling_packet, cannot send"); 809 return BTSTACK_ACL_BUFFERS_FULL; 810 } 811 812 // log_info("l2cap_send_le_signaling_packet type %u", cmd); 813 hci_reserve_packet_buffer(); 814 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 815 va_list argptr; 816 va_start(argptr, identifier); 817 uint16_t len = l2cap_create_signaling_le(acl_buffer, handle, cmd, identifier, argptr); 818 va_end(argptr); 819 // log_info("l2cap_send_le_signaling_packet con %u!", handle); 820 return hci_send_acl_packet_buffer(len); 821 } 822 #endif 823 824 uint16_t l2cap_max_mtu(void){ 825 return HCI_ACL_PAYLOAD_SIZE - L2CAP_HEADER_SIZE; 826 } 827 828 uint16_t l2cap_max_le_mtu(void){ 829 return l2cap_max_mtu(); 830 } 831 832 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 833 static uint16_t l2cap_setup_options_ertm(l2cap_channel_t * channel, uint8_t * config_options){ 834 config_options[0] = 0x04; // RETRANSMISSION AND FLOW CONTROL OPTION 835 config_options[1] = 9; // length 836 config_options[2] = (uint8_t) channel->mode; 837 config_options[3] = channel->num_rx_buffers; // == TxWindows size 838 config_options[4] = channel->local_max_transmit; 839 little_endian_store_16( config_options, 5, channel->local_retransmission_timeout_ms); 840 little_endian_store_16( config_options, 7, channel->local_monitor_timeout_ms); 841 little_endian_store_16( config_options, 9, channel->local_mps); 842 return 11; 843 } 844 static int l2cap_ertm_send_supervisor_frame(l2cap_channel_t * channel, uint16_t control){ 845 hci_reserve_packet_buffer(); 846 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 847 log_info("S-Frame: control 0x%04x", control); 848 little_endian_store_16(acl_buffer, 8, control); 849 return l2cap_send_prepared(channel->local_cid, 2); 850 } 851 static int l2cap_ertm_num_unacknowledged_tx_packets(l2cap_channel_t * channel){ 852 int unacknowledged_packets = channel->tx_send_index - channel->tx_read_index; 853 if (unacknowledged_packets < 0){ 854 unacknowledged_packets += channel->num_tx_buffers; 855 } 856 return unacknowledged_packets; 857 } 858 #endif 859 860 #ifdef ENABLE_CLASSIC 861 862 static uint16_t l2cap_setup_options_mtu(l2cap_channel_t * channel, uint8_t * config_options){ 863 config_options[0] = 1; // MTU 864 config_options[1] = 2; // len param 865 little_endian_store_16( (uint8_t*)&config_options, 2, channel->local_mtu); 866 return 4; 867 } 868 869 static uint16_t l2cap_setup_options(l2cap_channel_t * channel, uint8_t * config_options){ 870 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 871 // use ERTM options if supported 872 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 873 if ((connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_DONE) && (connection->l2cap_state.extended_feature_mask & 0x08)){ 874 return l2cap_setup_options_ertm(channel, config_options); 875 876 } 877 #endif 878 return l2cap_setup_options_mtu(channel, config_options); 879 } 880 881 static uint32_t l2cap_extended_features_mask(void){ 882 // extended features request supported, features: fixed channels, unicast connectionless data reception 883 uint32_t features = 0x280; 884 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 885 features |= 0x0008; 886 #endif 887 return features; 888 } 889 #endif 890 891 // MARK: L2CAP_RUN 892 // process outstanding signaling tasks 893 static void l2cap_run(void){ 894 895 // log_info("l2cap_run: entered"); 896 897 // check pending signaling responses 898 while (signaling_responses_pending){ 899 900 hci_con_handle_t handle = signaling_responses[0].handle; 901 902 if (!hci_can_send_acl_packet_now(handle)) break; 903 904 uint8_t sig_id = signaling_responses[0].sig_id; 905 uint8_t response_code = signaling_responses[0].code; 906 uint16_t infoType = signaling_responses[0].data; // INFORMATION_REQUEST 907 uint16_t result = signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT 908 #ifdef ENABLE_CLASSIC 909 uint16_t source_cid = signaling_responses[0].cid; // CONNECTION_REQUEST 910 #endif 911 UNUSED(infoType); 912 913 // remove first item before sending (to avoid sending response mutliple times) 914 signaling_responses_pending--; 915 int i; 916 for (i=0; i < signaling_responses_pending; i++){ 917 memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t)); 918 } 919 920 switch (response_code){ 921 #ifdef ENABLE_CLASSIC 922 case CONNECTION_REQUEST: 923 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 924 // also disconnect if result is 0x0003 - security blocked 925 if (result == 0x0003){ 926 hci_disconnect_security_block(handle); 927 } 928 break; 929 case ECHO_REQUEST: 930 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 931 break; 932 case INFORMATION_REQUEST: 933 switch (infoType){ 934 case 1: { // Connectionless MTU 935 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 936 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(connectionless_mtu), &connectionless_mtu); 937 } 938 break; 939 case 2: { // Extended Features Supported 940 uint32_t features = l2cap_extended_features_mask(); 941 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(features), &features); 942 } 943 break; 944 case 3: { // Fixed Channels Supported 945 uint8_t map[8]; 946 memset(map, 0, 8); 947 map[0] = 0x06; // L2CAP Signaling Channel (0x02) + Connectionless reception (0x04) 948 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(map), &map); 949 } 950 break; 951 default: 952 // all other types are not supported 953 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL); 954 break; 955 } 956 break; 957 case COMMAND_REJECT: 958 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 959 break; 960 #endif 961 #ifdef ENABLE_BLE 962 case LE_CREDIT_BASED_CONNECTION_REQUEST: 963 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 964 break; 965 case COMMAND_REJECT_LE: 966 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 967 break; 968 #endif 969 default: 970 // should not happen 971 break; 972 } 973 } 974 975 btstack_linked_list_iterator_t it; 976 UNUSED(it); 977 978 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 979 // send l2cap information request if neccessary 980 hci_connections_get_iterator(&it); 981 while(btstack_linked_list_iterator_has_next(&it)){ 982 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 983 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST){ 984 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE; 985 // send information request for extended features 986 uint8_t sig_id = l2cap_next_sig_id(); 987 uint8_t info_type = 2; 988 l2cap_send_signaling_packet(connection->con_handle, INFORMATION_REQUEST, sig_id, info_type); 989 return; 990 } 991 } 992 #endif 993 994 #ifdef ENABLE_CLASSIC 995 uint8_t config_options[10]; 996 btstack_linked_list_iterator_init(&it, &l2cap_channels); 997 while (btstack_linked_list_iterator_has_next(&it)){ 998 999 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1000 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1001 switch (channel->state){ 1002 1003 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1004 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 1005 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1006 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 1007 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 1008 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0); 1009 } 1010 break; 1011 1012 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1013 if (!hci_can_send_command_packet_now()) break; 1014 // send connection request - set state first 1015 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 1016 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 1017 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1); 1018 break; 1019 1020 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 1021 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1022 channel->state = L2CAP_STATE_INVALID; 1023 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0); 1024 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1025 l2cap_stop_rtx(channel); 1026 btstack_linked_list_iterator_remove(&it); 1027 btstack_memory_l2cap_channel_free(channel); 1028 break; 1029 1030 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 1031 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1032 channel->state = L2CAP_STATE_CONFIG; 1033 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1034 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 1035 break; 1036 1037 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 1038 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1039 // success, start l2cap handshake 1040 channel->local_sig_id = l2cap_next_sig_id(); 1041 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 1042 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 1043 l2cap_start_rtx(channel); 1044 break; 1045 1046 case L2CAP_STATE_CONFIG: 1047 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1048 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 1049 uint16_t flags = 0; 1050 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1051 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 1052 flags = 1; 1053 } else { 1054 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1055 } 1056 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 1057 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1058 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL); 1059 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1060 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 1061 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1062 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1063 uint16_t options_size = l2cap_setup_options(channel, config_options); 1064 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS, options_size, &config_options); 1065 #endif 1066 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 1067 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1068 uint16_t options_size = l2cap_setup_options(channel, config_options); 1069 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, options_size, &config_options); 1070 } else { 1071 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 1072 } 1073 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1074 } 1075 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 1076 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1077 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 1078 channel->local_sig_id = l2cap_next_sig_id(); 1079 uint16_t options_size = l2cap_setup_options(channel, config_options); 1080 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, options_size, &config_options); 1081 l2cap_start_rtx(channel); 1082 } 1083 if (l2cap_channel_ready_for_open(channel)){ 1084 channel->state = L2CAP_STATE_OPEN; 1085 l2cap_emit_channel_opened(channel, 0); // success 1086 } 1087 break; 1088 1089 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1090 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1091 channel->state = L2CAP_STATE_INVALID; 1092 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1093 // we don't start an RTX timer for a disconnect - there's no point in closing the channel if the other side doesn't respond :) 1094 l2cap_finialize_channel_close(channel); // -- remove from list 1095 break; 1096 1097 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1098 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1099 channel->local_sig_id = l2cap_next_sig_id(); 1100 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1101 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1102 break; 1103 default: 1104 break; 1105 } 1106 1107 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1108 // send s-frame to acknowledge received packets 1109 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1110 1111 if (channel->tx_send_index != channel->tx_write_index){ 1112 int unacknowledged_packets = l2cap_ertm_num_unacknowledged_tx_packets(channel); 1113 // check remote tx window 1114 log_info("unacknowledged_packets %u, remote tx window size %u", unacknowledged_packets, channel->remote_tx_window_size); 1115 if (unacknowledged_packets < channel->remote_tx_window_size){ 1116 int index = channel->tx_send_index; 1117 channel->tx_send_index++; 1118 if (channel->tx_send_index >= channel->num_tx_buffers){ 1119 channel->tx_send_index = 0; 1120 } 1121 l2cap_ertm_send_information_frame(channel, index, 0); // final = 0 1122 continue; 1123 } 1124 } 1125 1126 if (channel->send_supervisor_frame_receiver_ready){ 1127 channel->send_supervisor_frame_receiver_ready = 0; 1128 log_info("Send S-Frame: RR %u, final %u", channel->req_seq, channel->set_final_bit_after_packet_with_poll_bit_set); 1129 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 0, channel->set_final_bit_after_packet_with_poll_bit_set, channel->req_seq); 1130 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1131 l2cap_ertm_send_supervisor_frame(channel, control); 1132 continue; 1133 } 1134 if (channel->send_supervisor_frame_receiver_ready_poll){ 1135 channel->send_supervisor_frame_receiver_ready_poll = 0; 1136 log_info("Send S-Frame: RR %u with poll=1 ", channel->req_seq); 1137 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 1, 0, channel->req_seq); 1138 l2cap_ertm_send_supervisor_frame(channel, control); 1139 continue; 1140 } 1141 if (channel->send_supervisor_frame_receiver_not_ready){ 1142 channel->send_supervisor_frame_receiver_not_ready = 0; 1143 log_info("Send S-Frame: RNR %u", channel->req_seq); 1144 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 1145 l2cap_ertm_send_supervisor_frame(channel, control); 1146 continue; 1147 } 1148 if (channel->send_supervisor_frame_reject){ 1149 channel->send_supervisor_frame_reject = 0; 1150 log_info("Send S-Frame: REJ %u", channel->req_seq); 1151 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT, 0, 0, channel->req_seq); 1152 l2cap_ertm_send_supervisor_frame(channel, control); 1153 continue; 1154 } 1155 if (channel->send_supervisor_frame_selective_reject){ 1156 channel->send_supervisor_frame_selective_reject = 0; 1157 log_info("Send S-Frame: SREJ %u", channel->expected_tx_seq); 1158 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT, 0, channel->set_final_bit_after_packet_with_poll_bit_set, channel->expected_tx_seq); 1159 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1160 l2cap_ertm_send_supervisor_frame(channel, control); 1161 continue; 1162 } 1163 1164 if (channel->srej_active){ 1165 int i; 1166 for (i=0;i<channel->num_tx_buffers;i++){ 1167 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[i]; 1168 if (tx_state->retransmission_requested) { 1169 tx_state->retransmission_requested = 0; 1170 uint8_t final = channel->set_final_bit_after_packet_with_poll_bit_set; 1171 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1172 l2cap_ertm_send_information_frame(channel, i, final); 1173 break; 1174 } 1175 } 1176 if (i == channel->num_tx_buffers){ 1177 // no retransmission request found 1178 channel->srej_active = 0; 1179 } else { 1180 // packet was sent 1181 continue; 1182 } 1183 } 1184 #endif 1185 1186 } 1187 #endif 1188 1189 #ifdef ENABLE_LE_DATA_CHANNELS 1190 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1191 while (btstack_linked_list_iterator_has_next(&it)){ 1192 uint8_t * acl_buffer; 1193 uint8_t * l2cap_payload; 1194 uint16_t pos; 1195 uint16_t payload_size; 1196 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1197 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1198 switch (channel->state){ 1199 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1200 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1201 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1202 // le psm, source cid, mtu, mps, initial credits 1203 channel->local_sig_id = l2cap_next_sig_id(); 1204 channel->credits_incoming = channel->new_credits_incoming; 1205 channel->new_credits_incoming = 0; 1206 l2cap_send_le_signaling_packet( channel->con_handle, LE_CREDIT_BASED_CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid, channel->local_mtu, 23, channel->credits_incoming); 1207 break; 1208 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1209 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1210 // TODO: support larger MPS 1211 channel->state = L2CAP_STATE_OPEN; 1212 channel->credits_incoming = channel->new_credits_incoming; 1213 channel->new_credits_incoming = 0; 1214 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->local_mtu, 23, channel->credits_incoming, 0); 1215 // notify client 1216 l2cap_emit_le_channel_opened(channel, 0); 1217 break; 1218 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1219 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1220 channel->state = L2CAP_STATE_INVALID; 1221 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1222 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1223 l2cap_stop_rtx(channel); 1224 btstack_linked_list_iterator_remove(&it); 1225 btstack_memory_l2cap_channel_free(channel); 1226 break; 1227 case L2CAP_STATE_OPEN: 1228 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1229 1230 // send credits 1231 if (channel->new_credits_incoming){ 1232 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1233 channel->local_sig_id = l2cap_next_sig_id(); 1234 uint16_t new_credits = channel->new_credits_incoming; 1235 channel->new_credits_incoming = 0; 1236 channel->credits_incoming += new_credits; 1237 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1238 break; 1239 } 1240 1241 // send data 1242 if (!channel->send_sdu_buffer) break; 1243 if (!channel->credits_outgoing) break; 1244 1245 // send part of SDU 1246 hci_reserve_packet_buffer(); 1247 acl_buffer = hci_get_outgoing_packet_buffer(); 1248 l2cap_payload = acl_buffer + 8; 1249 pos = 0; 1250 if (!channel->send_sdu_pos){ 1251 // store SDU len 1252 channel->send_sdu_pos += 2; 1253 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 1254 pos += 2; 1255 } 1256 payload_size = btstack_min(channel->send_sdu_len + 2 - channel->send_sdu_pos, channel->remote_mps - pos); 1257 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 1258 memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len 1259 pos += payload_size; 1260 channel->send_sdu_pos += payload_size; 1261 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 1262 // done 1263 1264 channel->credits_outgoing--; 1265 1266 if (channel->send_sdu_pos >= channel->send_sdu_len + 2){ 1267 channel->send_sdu_buffer = NULL; 1268 // send done event 1269 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 1270 // inform about can send now 1271 l2cap_le_notify_channel_can_send(channel); 1272 } 1273 hci_send_acl_packet_buffer(8 + pos); 1274 break; 1275 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1276 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1277 channel->local_sig_id = l2cap_next_sig_id(); 1278 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1279 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1280 break; 1281 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1282 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1283 channel->state = L2CAP_STATE_INVALID; 1284 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1285 l2cap_le_finialize_channel_close(channel); // -- remove from list 1286 break; 1287 default: 1288 break; 1289 } 1290 } 1291 #endif 1292 1293 #ifdef ENABLE_BLE 1294 // send l2cap con paramter update if necessary 1295 hci_connections_get_iterator(&it); 1296 while(btstack_linked_list_iterator_has_next(&it)){ 1297 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1298 if (connection->address_type != BD_ADDR_TYPE_LE_PUBLIC && connection->address_type != BD_ADDR_TYPE_LE_RANDOM) continue; 1299 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1300 switch (connection->le_con_parameter_update_state){ 1301 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1302 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1303 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, connection->le_con_param_update_identifier, 1304 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1305 break; 1306 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1307 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1308 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1309 break; 1310 case CON_PARAMETER_UPDATE_DENY: 1311 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1312 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1313 break; 1314 default: 1315 break; 1316 } 1317 } 1318 #endif 1319 1320 // log_info("l2cap_run: exit"); 1321 } 1322 1323 #ifdef ENABLE_CLASSIC 1324 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1325 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 1326 log_info("l2cap_handle_connection_complete expected state"); 1327 // success, start l2cap handshake 1328 channel->con_handle = con_handle; 1329 // check remote SSP feature first 1330 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1331 } 1332 } 1333 1334 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1335 1336 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1337 // assumption: outgoing connection 1338 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1339 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1340 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1341 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1342 return; 1343 } 1344 #endif 1345 1346 // fine, go ahead 1347 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1348 } 1349 1350 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1351 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1352 1353 // we have been waiting for remote supported features, if both support SSP, 1354 log_info("l2cap received remote supported features, sec_level_0_allowed for psm %u = %u", channel->psm, l2cap_security_level_0_allowed_for_PSM(channel->psm)); 1355 if (gap_ssp_supported_on_both_sides(channel->con_handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){ 1356 // request security level 2 1357 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 1358 gap_request_security_level(channel->con_handle, LEVEL_2); 1359 return; 1360 } 1361 1362 l2cap_ready_to_connect(channel); 1363 } 1364 #endif 1365 1366 #ifdef L2CAP_USES_CHANNELS 1367 static l2cap_channel_t * l2cap_create_channel_entry(btstack_packet_handler_t packet_handler, bd_addr_t address, bd_addr_type_t address_type, 1368 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 1369 1370 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 1371 if (!channel) { 1372 return NULL; 1373 } 1374 1375 // Init memory (make valgrind happy) 1376 memset(channel, 0, sizeof(l2cap_channel_t)); 1377 1378 // fill in 1379 channel->packet_handler = packet_handler; 1380 bd_addr_copy(channel->address, address); 1381 channel->address_type = address_type; 1382 channel->psm = psm; 1383 channel->local_mtu = local_mtu; 1384 channel->remote_mtu = L2CAP_MINIMAL_MTU; 1385 channel->required_security_level = security_level; 1386 1387 // 1388 channel->local_cid = l2cap_next_local_cid(); 1389 channel->con_handle = 0; 1390 1391 // set initial state 1392 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 1393 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 1394 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 1395 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 1396 1397 return channel; 1398 } 1399 #endif 1400 1401 #ifdef ENABLE_CLASSIC 1402 1403 /** 1404 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 1405 * @param packet_handler 1406 * @param address 1407 * @param psm 1408 * @param mtu 1409 * @param local_cid 1410 */ 1411 1412 uint8_t l2cap_create_channel(btstack_packet_handler_t channel_packet_handler, bd_addr_t address, uint16_t psm, uint16_t mtu, uint16_t * out_local_cid){ 1413 // limit MTU to the size of our outtgoing HCI buffer 1414 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 1415 1416 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x mtu %u -> local mtu %u", bd_addr_to_str(address), psm, mtu, local_mtu); 1417 1418 l2cap_channel_t * channel = l2cap_create_channel_entry(channel_packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1419 if (!channel) { 1420 return BTSTACK_MEMORY_ALLOC_FAILED; 1421 } 1422 1423 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1424 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1425 #endif 1426 1427 // add to connections list 1428 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1429 1430 // store local_cid 1431 if (out_local_cid){ 1432 *out_local_cid = channel->local_cid; 1433 } 1434 1435 // check if hci connection is already usable 1436 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1437 if (conn){ 1438 log_info("l2cap_create_channel, hci connection already exists"); 1439 l2cap_handle_connection_complete(conn->con_handle, channel); 1440 // check if remote supported fearures are already received 1441 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1442 l2cap_handle_remote_supported_features_received(channel); 1443 } 1444 } 1445 1446 l2cap_run(); 1447 1448 return 0; 1449 } 1450 1451 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1452 1453 static uint8_t l2cap_ertm_validate_local_config(uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, 1454 uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){ 1455 1456 UNUSED(buffer); 1457 UNUSED(size); 1458 1459 uint8_t result = ERROR_CODE_SUCCESS; 1460 if (max_transmit < 1){ 1461 log_error("max_transmit must be >= 1"); 1462 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1463 } 1464 if (retransmission_timeout_ms < 2000){ 1465 log_error("retransmission_timeout_ms must be >= 2000 ms"); 1466 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1467 } 1468 if (monitor_timeout_ms < 12000){ 1469 log_error("monitor_timeout_ms must be >= 12000 ms"); 1470 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1471 } 1472 if (local_mtu < 48){ 1473 log_error("local_mtu must be >= 48"); 1474 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1475 } 1476 if (num_rx_buffers < 1){ 1477 log_error("num_rx_buffers must be >= 1"); 1478 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1479 } 1480 if (num_tx_buffers < 1){ 1481 log_error("num_rx_buffers must be >= 1"); 1482 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1483 } 1484 return result; 1485 } 1486 1487 static void l2cap_ertm_configure_channel(l2cap_channel_t * channel, int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, 1488 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){ 1489 1490 channel->mode = L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION; 1491 channel->ertm_mandatory = ertm_mandatory; 1492 channel->local_max_transmit = max_transmit; 1493 channel->local_retransmission_timeout_ms = retransmission_timeout_ms; 1494 channel->local_monitor_timeout_ms = monitor_timeout_ms; 1495 channel->local_mtu = local_mtu; 1496 channel->num_rx_buffers = num_rx_buffers; 1497 channel->num_tx_buffers = num_tx_buffers; 1498 1499 // align buffer to 16-byte boundary, just in case 1500 int bytes_till_alignment = 16 - (((uintptr_t) buffer) & 0x0f); 1501 buffer += bytes_till_alignment; 1502 size -= bytes_till_alignment; 1503 1504 // setup state buffers 1505 uint32_t pos = 0; 1506 channel->rx_packets_state = (l2cap_ertm_rx_packet_state_t *) &buffer[pos]; 1507 pos += num_rx_buffers * sizeof(l2cap_ertm_rx_packet_state_t); 1508 channel->tx_packets_state = (l2cap_ertm_tx_packet_state_t *) &buffer[pos]; 1509 pos += num_tx_buffers * sizeof(l2cap_ertm_tx_packet_state_t); 1510 1511 // setup reassembly buffer 1512 channel->reassembly_buffer = &buffer[pos]; 1513 pos += local_mtu; 1514 1515 // divide rest of data equally 1516 channel->local_mps = (size - pos) / (num_rx_buffers + num_tx_buffers); 1517 log_info("Local MPS: %u", channel->local_mtu); 1518 channel->rx_packets_data = &buffer[pos]; 1519 pos += num_rx_buffers * channel->local_mtu; 1520 channel->tx_packets_data = &buffer[pos]; 1521 } 1522 1523 uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, 1524 int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, 1525 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){ 1526 1527 log_info("L2CAP_CREATE_CHANNEL addr %s, psm 0x%x, local mtu %u", bd_addr_to_str(address), psm, local_mtu); 1528 1529 // validate local config 1530 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); 1531 if (result) return result; 1532 1533 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1534 if (!channel) { 1535 return BTSTACK_MEMORY_ALLOC_FAILED; 1536 } 1537 1538 // configure ERTM 1539 l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, 1540 local_mtu, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1541 1542 // add to connections list 1543 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1544 1545 // store local_cid 1546 if (out_local_cid){ 1547 *out_local_cid = channel->local_cid; 1548 } 1549 1550 // check if hci connection is already usable 1551 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1552 if (conn){ 1553 log_info("l2cap_create_channel, hci connection already exists"); 1554 l2cap_handle_connection_complete(conn->con_handle, channel); 1555 // check if remote supported fearures are already received 1556 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1557 l2cap_handle_remote_supported_features_received(channel); 1558 } 1559 } 1560 1561 l2cap_run(); 1562 1563 return 0; 1564 } 1565 #endif 1566 1567 void 1568 l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 1569 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 1570 // find channel for local_cid 1571 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1572 if (channel) { 1573 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1574 } 1575 // process 1576 l2cap_run(); 1577 } 1578 1579 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 1580 btstack_linked_list_iterator_t it; 1581 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1582 while (btstack_linked_list_iterator_has_next(&it)){ 1583 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1584 if ( bd_addr_cmp( channel->address, address) != 0) continue; 1585 // channel for this address found 1586 switch (channel->state){ 1587 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 1588 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1589 // failure, forward error code 1590 l2cap_emit_channel_opened(channel, status); 1591 // discard channel 1592 l2cap_stop_rtx(channel); 1593 btstack_linked_list_iterator_remove(&it); 1594 btstack_memory_l2cap_channel_free(channel); 1595 break; 1596 default: 1597 break; 1598 } 1599 } 1600 } 1601 1602 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 1603 btstack_linked_list_iterator_t it; 1604 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1605 while (btstack_linked_list_iterator_has_next(&it)){ 1606 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1607 if ( ! bd_addr_cmp( channel->address, address) ){ 1608 l2cap_handle_connection_complete(handle, channel); 1609 } 1610 } 1611 // process 1612 l2cap_run(); 1613 } 1614 #endif 1615 1616 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1617 static void l2cap_ertm_notify_channel_can_send(l2cap_channel_t * channel){ 1618 if (l2cap_ertm_can_store_packet_now(channel)){ 1619 channel->waiting_for_can_send_now = 0; 1620 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 1621 } 1622 } 1623 #endif 1624 1625 static void l2cap_notify_channel_can_send(void){ 1626 1627 #ifdef ENABLE_CLASSIC 1628 btstack_linked_list_iterator_t it; 1629 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1630 while (btstack_linked_list_iterator_has_next(&it)){ 1631 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1632 if (!channel->waiting_for_can_send_now) continue; 1633 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1634 channel->waiting_for_can_send_now = 0; 1635 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 1636 } 1637 #endif 1638 1639 int i; 1640 for (i=0;i<L2CAP_FIXED_CHANNEL_TABLE_SIZE;i++){ 1641 if (!fixed_channels[i].callback) continue; 1642 if (!fixed_channels[i].waiting_for_can_send_now) continue; 1643 int can_send = 0; 1644 if (l2cap_fixed_channel_table_index_is_le(i)){ 1645 #ifdef ENABLE_BLE 1646 can_send = hci_can_send_acl_le_packet_now(); 1647 #endif 1648 } else { 1649 #ifdef ENABLE_CLASSIC 1650 can_send = hci_can_send_acl_classic_packet_now(); 1651 #endif 1652 } 1653 if (!can_send) continue; 1654 fixed_channels[i].waiting_for_can_send_now = 0; 1655 l2cap_emit_can_send_now(fixed_channels[i].callback, l2cap_fixed_channel_table_channel_id_for_index(i)); 1656 } 1657 } 1658 1659 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 1660 1661 UNUSED(packet_type); 1662 UNUSED(cid); 1663 UNUSED(size); 1664 1665 bd_addr_t address; 1666 hci_con_handle_t handle; 1667 int hci_con_used; 1668 btstack_linked_list_iterator_t it; 1669 1670 // avoid unused warnings 1671 UNUSED(address); 1672 UNUSED(hci_con_used); 1673 UNUSED(it); 1674 UNUSED(handle); 1675 1676 switch(hci_event_packet_get_type(packet)){ 1677 1678 // Notify channel packet handler if they can send now 1679 case HCI_EVENT_TRANSPORT_PACKET_SENT: 1680 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 1681 l2cap_run(); // try sending signaling packets first 1682 l2cap_notify_channel_can_send(); 1683 break; 1684 1685 case HCI_EVENT_COMMAND_STATUS: 1686 l2cap_run(); // try sending signaling packets first 1687 break; 1688 1689 #ifdef ENABLE_CLASSIC 1690 // handle connection complete events 1691 case HCI_EVENT_CONNECTION_COMPLETE: 1692 reverse_bd_addr(&packet[5], address); 1693 if (packet[2] == 0){ 1694 handle = little_endian_read_16(packet, 3); 1695 l2cap_handle_connection_success_for_addr(address, handle); 1696 } else { 1697 l2cap_handle_connection_failed_for_addr(address, packet[2]); 1698 } 1699 break; 1700 1701 // handle successful create connection cancel command 1702 case HCI_EVENT_COMMAND_COMPLETE: 1703 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 1704 if (packet[5] == 0){ 1705 reverse_bd_addr(&packet[6], address); 1706 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 1707 l2cap_handle_connection_failed_for_addr(address, 0x16); 1708 } 1709 } 1710 l2cap_run(); // try sending signaling packets first 1711 break; 1712 #endif 1713 1714 // handle disconnection complete events 1715 case HCI_EVENT_DISCONNECTION_COMPLETE: 1716 // send l2cap disconnect events for all channels on this handle and free them 1717 #ifdef ENABLE_CLASSIC 1718 handle = little_endian_read_16(packet, 3); 1719 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1720 while (btstack_linked_list_iterator_has_next(&it)){ 1721 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1722 if (channel->con_handle != handle) continue; 1723 l2cap_emit_channel_closed(channel); 1724 l2cap_stop_rtx(channel); 1725 btstack_linked_list_iterator_remove(&it); 1726 btstack_memory_l2cap_channel_free(channel); 1727 } 1728 #endif 1729 #ifdef ENABLE_LE_DATA_CHANNELS 1730 handle = little_endian_read_16(packet, 3); 1731 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1732 while (btstack_linked_list_iterator_has_next(&it)){ 1733 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1734 if (channel->con_handle != handle) continue; 1735 l2cap_emit_channel_closed(channel); 1736 btstack_linked_list_iterator_remove(&it); 1737 btstack_memory_l2cap_channel_free(channel); 1738 } 1739 #endif 1740 break; 1741 1742 // HCI Connection Timeouts 1743 #ifdef ENABLE_CLASSIC 1744 case L2CAP_EVENT_TIMEOUT_CHECK: 1745 handle = little_endian_read_16(packet, 2); 1746 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 1747 if (hci_authentication_active_for_handle(handle)) break; 1748 hci_con_used = 0; 1749 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1750 while (btstack_linked_list_iterator_has_next(&it)){ 1751 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1752 if (channel->con_handle != handle) continue; 1753 hci_con_used = 1; 1754 break; 1755 } 1756 if (hci_con_used) break; 1757 if (!hci_can_send_command_packet_now()) break; 1758 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 1759 break; 1760 1761 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 1762 handle = little_endian_read_16(packet, 3); 1763 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1764 while (btstack_linked_list_iterator_has_next(&it)){ 1765 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1766 if (channel->con_handle != handle) continue; 1767 l2cap_handle_remote_supported_features_received(channel); 1768 break; 1769 } 1770 break; 1771 1772 case GAP_EVENT_SECURITY_LEVEL: 1773 handle = little_endian_read_16(packet, 2); 1774 log_info("l2cap - security level update"); 1775 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1776 while (btstack_linked_list_iterator_has_next(&it)){ 1777 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1778 if (channel->con_handle != handle) continue; 1779 1780 log_info("l2cap - state %u", channel->state); 1781 1782 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 1783 gap_security_level_t required_level = channel->required_security_level; 1784 1785 switch (channel->state){ 1786 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1787 if (actual_level >= required_level){ 1788 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1789 // we need to know if ERTM is supported before sending a config response 1790 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1791 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1792 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 1793 #else 1794 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 1795 l2cap_emit_incoming_connection(channel); 1796 #endif 1797 } else { 1798 channel->reason = 0x0003; // security block 1799 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1800 } 1801 break; 1802 1803 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 1804 if (actual_level >= required_level){ 1805 l2cap_ready_to_connect(channel); 1806 } else { 1807 // disconnnect, authentication not good enough 1808 hci_disconnect_security_block(handle); 1809 } 1810 break; 1811 1812 default: 1813 break; 1814 } 1815 } 1816 break; 1817 #endif 1818 1819 default: 1820 break; 1821 } 1822 1823 l2cap_run(); 1824 } 1825 1826 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 1827 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 1828 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 1829 signaling_responses[signaling_responses_pending].handle = handle; 1830 signaling_responses[signaling_responses_pending].code = code; 1831 signaling_responses[signaling_responses_pending].sig_id = sig_id; 1832 signaling_responses[signaling_responses_pending].cid = cid; 1833 signaling_responses[signaling_responses_pending].data = data; 1834 signaling_responses_pending++; 1835 l2cap_run(); 1836 } 1837 } 1838 1839 #ifdef ENABLE_CLASSIC 1840 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 1841 channel->remote_sig_id = identifier; 1842 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 1843 l2cap_run(); 1844 } 1845 1846 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 1847 1848 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 1849 l2cap_service_t *service = l2cap_get_service(psm); 1850 if (!service) { 1851 // 0x0002 PSM not supported 1852 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 1853 return; 1854 } 1855 1856 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 1857 if (!hci_connection) { 1858 // 1859 log_error("no hci_connection for handle %u", handle); 1860 return; 1861 } 1862 1863 // alloc structure 1864 // log_info("l2cap_handle_connection_request register channel"); 1865 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, hci_connection->address, BD_ADDR_TYPE_CLASSIC, 1866 psm, service->mtu, service->required_security_level); 1867 if (!channel){ 1868 // 0x0004 No resources available 1869 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 1870 return; 1871 } 1872 1873 channel->con_handle = handle; 1874 channel->remote_cid = source_cid; 1875 channel->remote_sig_id = sig_id; 1876 1877 // limit local mtu to max acl packet length - l2cap header 1878 if (channel->local_mtu > l2cap_max_mtu()) { 1879 channel->local_mtu = l2cap_max_mtu(); 1880 } 1881 1882 // set initial state 1883 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 1884 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 1885 1886 // add to connections list 1887 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1888 1889 // assert security requirements 1890 gap_request_security_level(handle, channel->required_security_level); 1891 } 1892 1893 void l2cap_accept_connection(uint16_t local_cid){ 1894 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 1895 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1896 if (!channel) { 1897 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1898 return; 1899 } 1900 1901 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1902 // configure L2CAP Basic mode 1903 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1904 #endif 1905 1906 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1907 1908 // process 1909 l2cap_run(); 1910 } 1911 1912 1913 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1914 uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, 1915 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){ 1916 1917 log_info("L2CAP_ACCEPT_ERTM_CONNECTION local_cid 0x%x", local_cid); 1918 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1919 if (!channel) { 1920 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1921 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1922 } 1923 1924 // validate local config 1925 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); 1926 if (result) return result; 1927 1928 // configure L2CAP ERTM 1929 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); 1930 1931 // continue 1932 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1933 1934 // process 1935 l2cap_run(); 1936 1937 return ERROR_CODE_SUCCESS; 1938 } 1939 1940 uint8_t l2cap_ertm_set_busy(uint16_t local_cid){ 1941 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1942 if (!channel) { 1943 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1944 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1945 } 1946 if (!channel->local_busy){ 1947 channel->local_busy = 1; 1948 channel->send_supervisor_frame_receiver_not_ready = 1; 1949 l2cap_run(); 1950 } 1951 return ERROR_CODE_SUCCESS; 1952 } 1953 1954 uint8_t l2cap_ertm_set_ready(uint16_t local_cid){ 1955 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1956 if (!channel) { 1957 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1958 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1959 } 1960 if (channel->local_busy){ 1961 channel->local_busy = 0; 1962 channel->send_supervisor_frame_receiver_ready_poll = 1; 1963 l2cap_run(); 1964 } 1965 return ERROR_CODE_SUCCESS; 1966 } 1967 1968 static void l2cap_ertm_handle_req_seq(l2cap_channel_t * l2cap_channel, uint8_t req_seq){ 1969 int num_buffers_acked = 0; 1970 l2cap_ertm_tx_packet_state_t * tx_state; 1971 while (1){ 1972 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 1973 // calc delta 1974 int delta = (req_seq - tx_state->tx_seq) & 0x03f; 1975 if (delta == 0) break; // all packets acknowledged 1976 if (delta > l2cap_channel->remote_tx_window_size) break; 1977 1978 num_buffers_acked++; 1979 log_info("RR seq %u => packet with tx_seq %u done", req_seq, tx_state->tx_seq); 1980 1981 // stop retransmission timer 1982 l2cap_ertm_stop_retransmission_timer(l2cap_channel); 1983 1984 l2cap_channel->tx_read_index++; 1985 if (l2cap_channel->tx_read_index >= l2cap_channel->num_rx_buffers){ 1986 l2cap_channel->tx_read_index = 0; 1987 } 1988 1989 // no unack packages 1990 if (l2cap_channel->tx_read_index == l2cap_channel->tx_write_index) break; 1991 } 1992 1993 if (num_buffers_acked){ 1994 l2cap_ertm_notify_channel_can_send(l2cap_channel); 1995 } 1996 } 1997 1998 static l2cap_ertm_tx_packet_state_t * l2cap_ertm_get_tx_state(l2cap_channel_t * l2cap_channel, uint8_t tx_seq){ 1999 int i; 2000 for (i=0;i<l2cap_channel->num_tx_buffers;i++){ 2001 l2cap_ertm_tx_packet_state_t * tx_state = &l2cap_channel->tx_packets_state[i]; 2002 if (tx_state->tx_seq == tx_seq) return tx_state; 2003 } 2004 return NULL; 2005 } 2006 #endif 2007 2008 2009 void l2cap_decline_connection(uint16_t local_cid){ 2010 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 2011 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 2012 if (!channel) { 2013 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 2014 return; 2015 } 2016 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2017 channel->reason = 0x04; // no resources available 2018 l2cap_run(); 2019 } 2020 2021 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 2022 2023 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2024 2025 uint16_t flags = little_endian_read_16(command, 6); 2026 if (flags & 1) { 2027 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 2028 } 2029 2030 // accept the other's configuration options 2031 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2032 uint16_t pos = 8; 2033 while (pos < end_pos){ 2034 uint8_t option_hint = command[pos] >> 7; 2035 uint8_t option_type = command[pos] & 0x7f; 2036 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2037 pos++; 2038 uint8_t length = command[pos++]; 2039 // MTU { type(8): 1, len(8):2, MTU(16) } 2040 if (option_type == 1 && length == 2){ 2041 channel->remote_mtu = little_endian_read_16(command, pos); 2042 log_info("Remote MTU %u", channel->remote_mtu); 2043 if (channel->remote_mtu > l2cap_max_mtu()){ 2044 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 2045 channel->remote_mtu = l2cap_max_mtu(); 2046 } 2047 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2048 } 2049 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2050 if (option_type == 2 && length == 2){ 2051 channel->flush_timeout = little_endian_read_16(command, pos); 2052 log_info("Flush timeout: %u ms", channel->flush_timeout); 2053 } 2054 2055 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2056 // Retransmission and Flow Control Option 2057 if (option_type == 4 && length == 9){ 2058 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2059 switch(channel->mode){ 2060 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2061 // Store remote config 2062 channel->remote_tx_window_size = command[pos+1]; 2063 channel->remote_max_transmit = command[pos+2]; 2064 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2065 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2066 channel->remote_mps = little_endian_read_16(command, pos + 7); 2067 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2068 channel->remote_tx_window_size, 2069 channel->remote_max_transmit, 2070 channel->remote_retransmission_timeout_ms, 2071 channel->remote_monitor_timeout_ms, 2072 channel->remote_mps); 2073 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2074 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2075 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2076 } else { 2077 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2078 } 2079 break; 2080 case L2CAP_CHANNEL_MODE_BASIC: 2081 switch (mode){ 2082 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2083 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2084 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2085 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2086 } 2087 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2088 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2089 break; 2090 default: // case L2CAP_CHANNEL_MODE_BASIC: 2091 // TODO store and evaluate configuration 2092 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2093 break; 2094 } 2095 break; 2096 default: 2097 break; 2098 } 2099 } 2100 #endif 2101 // check for unknown options 2102 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 2103 log_info("l2cap cid %u, unknown options", channel->local_cid); 2104 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2105 } 2106 pos += length; 2107 } 2108 } 2109 2110 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2111 log_info("l2cap_signaling_handle_configure_response"); 2112 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2113 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2114 uint16_t pos = 10; 2115 while (pos < end_pos){ 2116 uint8_t option_hint = command[pos] >> 7; 2117 uint8_t option_type = command[pos] & 0x7f; 2118 log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2119 pos++; 2120 uint8_t length = command[pos++]; 2121 2122 // Retransmission and Flow Control Option 2123 if (option_type == 4 && length == 9){ 2124 switch (channel->mode){ 2125 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2126 if (channel->ertm_mandatory){ 2127 // ?? 2128 } else { 2129 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2130 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2131 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2132 } 2133 } 2134 break; 2135 case L2CAP_CHANNEL_MODE_BASIC: 2136 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2137 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2138 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2139 } 2140 break; 2141 default: 2142 break; 2143 } 2144 } 2145 2146 // check for unknown options 2147 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 2148 log_info("l2cap cid %u, unknown options", channel->local_cid); 2149 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2150 } 2151 2152 pos += length; 2153 } 2154 #endif 2155 } 2156 2157 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2158 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2159 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2160 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2161 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2162 if (channel->state == L2CAP_STATE_OPEN) return 0; 2163 return 1; 2164 } 2165 2166 2167 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2168 2169 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2170 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2171 uint16_t result = 0; 2172 2173 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2174 2175 // handle DISCONNECT REQUESTS seperately 2176 if (code == DISCONNECTION_REQUEST){ 2177 switch (channel->state){ 2178 case L2CAP_STATE_CONFIG: 2179 case L2CAP_STATE_OPEN: 2180 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2181 case L2CAP_STATE_WAIT_DISCONNECT: 2182 l2cap_handle_disconnect_request(channel, identifier); 2183 break; 2184 2185 default: 2186 // ignore in other states 2187 break; 2188 } 2189 return; 2190 } 2191 2192 // @STATEMACHINE(l2cap) 2193 switch (channel->state) { 2194 2195 case L2CAP_STATE_WAIT_CONNECT_RSP: 2196 switch (code){ 2197 case CONNECTION_RESPONSE: 2198 l2cap_stop_rtx(channel); 2199 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2200 switch (result) { 2201 case 0: 2202 // successful connection 2203 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2204 channel->state = L2CAP_STATE_CONFIG; 2205 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2206 break; 2207 case 1: 2208 // connection pending. get some coffee, but start the ERTX 2209 l2cap_start_ertx(channel); 2210 break; 2211 default: 2212 // channel closed 2213 channel->state = L2CAP_STATE_CLOSED; 2214 // map l2cap connection response result to BTstack status enumeration 2215 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2216 2217 // drop link key if security block 2218 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2219 gap_drop_link_key_for_bd_addr(channel->address); 2220 } 2221 2222 // discard channel 2223 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2224 btstack_memory_l2cap_channel_free(channel); 2225 break; 2226 } 2227 break; 2228 2229 default: 2230 //@TODO: implement other signaling packets 2231 break; 2232 } 2233 break; 2234 2235 case L2CAP_STATE_CONFIG: 2236 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2237 switch (code) { 2238 case CONFIGURE_REQUEST: 2239 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 2240 l2cap_signaling_handle_configure_request(channel, command); 2241 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 2242 // only done if continuation not set 2243 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 2244 } 2245 break; 2246 case CONFIGURE_RESPONSE: 2247 l2cap_stop_rtx(channel); 2248 l2cap_signaling_handle_configure_response(channel, result, command); 2249 switch (result){ 2250 case 0: // success 2251 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 2252 break; 2253 case 4: // pending 2254 l2cap_start_ertx(channel); 2255 break; 2256 default: 2257 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2258 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 2259 // remote does not offer ertm but it's required 2260 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2261 break; 2262 } 2263 #endif 2264 // retry on negative result 2265 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2266 break; 2267 } 2268 break; 2269 default: 2270 break; 2271 } 2272 if (l2cap_channel_ready_for_open(channel)){ 2273 // for open: 2274 channel->state = L2CAP_STATE_OPEN; 2275 l2cap_emit_channel_opened(channel, 0); 2276 } 2277 break; 2278 2279 case L2CAP_STATE_WAIT_DISCONNECT: 2280 switch (code) { 2281 case DISCONNECTION_RESPONSE: 2282 l2cap_finialize_channel_close(channel); 2283 break; 2284 default: 2285 //@TODO: implement other signaling packets 2286 break; 2287 } 2288 break; 2289 2290 case L2CAP_STATE_CLOSED: 2291 // @TODO handle incoming requests 2292 break; 2293 2294 case L2CAP_STATE_OPEN: 2295 //@TODO: implement other signaling packets, e.g. re-configure 2296 break; 2297 default: 2298 break; 2299 } 2300 // log_info("new state %u", channel->state); 2301 } 2302 2303 2304 static void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){ 2305 2306 btstack_linked_list_iterator_t it; 2307 2308 // get code, signalind identifier and command len 2309 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2310 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2311 2312 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 2313 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_RESPONSE){ 2314 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2315 return; 2316 } 2317 2318 // general commands without an assigned channel 2319 switch(code) { 2320 2321 case CONNECTION_REQUEST: { 2322 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2323 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2324 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 2325 return; 2326 } 2327 2328 case ECHO_REQUEST: 2329 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 2330 return; 2331 2332 case INFORMATION_REQUEST: { 2333 uint16_t infoType = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2334 l2cap_register_signaling_response(handle, code, sig_id, 0, infoType); 2335 return; 2336 } 2337 2338 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2339 case INFORMATION_RESPONSE: { 2340 hci_connection_t * connection = hci_connection_for_handle(handle); 2341 if (!connection) return; 2342 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2343 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2344 if (result != 0) return; 2345 if (info_type != 0x02) return; 2346 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2347 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2348 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2349 // trigger connection request 2350 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2351 while (btstack_linked_list_iterator_has_next(&it)){ 2352 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2353 if (channel->con_handle != handle) continue; 2354 // bail if ERTM was requested but is not supported 2355 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2356 if (channel->ertm_mandatory){ 2357 // channel closed 2358 channel->state = L2CAP_STATE_CLOSED; 2359 // map l2cap connection response result to BTstack status enumeration 2360 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTD); 2361 // discard channel 2362 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2363 btstack_memory_l2cap_channel_free(channel); 2364 continue; 2365 } else { 2366 // fallback to Basic mode 2367 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2368 } 2369 } 2370 // start connecting 2371 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2372 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 2373 } 2374 // respond to connection request 2375 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2376 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2377 l2cap_emit_incoming_connection(channel); 2378 } 2379 } 2380 return; 2381 } 2382 #endif 2383 2384 default: 2385 break; 2386 } 2387 2388 2389 // Get potential destination CID 2390 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2391 2392 // Find channel for this sig_id and connection handle 2393 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2394 while (btstack_linked_list_iterator_has_next(&it)){ 2395 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2396 if (channel->con_handle != handle) continue; 2397 if (code & 1) { 2398 // match odd commands (responses) by previous signaling identifier 2399 if (channel->local_sig_id == sig_id) { 2400 l2cap_signaling_handler_channel(channel, command); 2401 break; 2402 } 2403 } else { 2404 // match even commands (requests) by local channel id 2405 if (channel->local_cid == dest_cid) { 2406 l2cap_signaling_handler_channel(channel, command); 2407 break; 2408 } 2409 } 2410 } 2411 } 2412 #endif 2413 2414 #ifdef ENABLE_BLE 2415 2416 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 2417 uint8_t event[6]; 2418 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 2419 event[1] = 4; 2420 little_endian_store_16(event, 2, con_handle); 2421 little_endian_store_16(event, 4, result); 2422 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2423 if (!l2cap_event_packet_handler) return; 2424 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 2425 } 2426 2427 // @returns valid 2428 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 2429 hci_connection_t * connection; 2430 uint16_t result; 2431 uint8_t event[10]; 2432 2433 #ifdef ENABLE_LE_DATA_CHANNELS 2434 btstack_linked_list_iterator_t it; 2435 l2cap_channel_t * channel; 2436 uint16_t local_cid; 2437 uint16_t le_psm; 2438 uint16_t new_credits; 2439 uint16_t credits_before; 2440 l2cap_service_t * service; 2441 uint16_t source_cid; 2442 #endif 2443 2444 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2445 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u", code, sig_id); 2446 2447 switch (code){ 2448 2449 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 2450 result = little_endian_read_16(command, 4); 2451 l2cap_emit_connection_parameter_update_response(handle, result); 2452 break; 2453 2454 case CONNECTION_PARAMETER_UPDATE_REQUEST: 2455 connection = hci_connection_for_handle(handle); 2456 if (connection){ 2457 if (connection->role != HCI_ROLE_MASTER){ 2458 // reject command without notifying upper layer when not in master role 2459 return 0; 2460 } 2461 int update_parameter = 1; 2462 le_connection_parameter_range_t existing_range; 2463 gap_get_connection_parameter_range(&existing_range); 2464 uint16_t le_conn_interval_min = little_endian_read_16(command,8); 2465 uint16_t le_conn_interval_max = little_endian_read_16(command,10); 2466 uint16_t le_conn_latency = little_endian_read_16(command,12); 2467 uint16_t le_supervision_timeout = little_endian_read_16(command,14); 2468 2469 if (le_conn_interval_min < existing_range.le_conn_interval_min) update_parameter = 0; 2470 if (le_conn_interval_max > existing_range.le_conn_interval_max) update_parameter = 0; 2471 2472 if (le_conn_latency < existing_range.le_conn_latency_min) update_parameter = 0; 2473 if (le_conn_latency > existing_range.le_conn_latency_max) update_parameter = 0; 2474 2475 if (le_supervision_timeout < existing_range.le_supervision_timeout_min) update_parameter = 0; 2476 if (le_supervision_timeout > existing_range.le_supervision_timeout_max) update_parameter = 0; 2477 2478 if (update_parameter){ 2479 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 2480 connection->le_conn_interval_min = le_conn_interval_min; 2481 connection->le_conn_interval_max = le_conn_interval_max; 2482 connection->le_conn_latency = le_conn_latency; 2483 connection->le_supervision_timeout = le_supervision_timeout; 2484 } else { 2485 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 2486 } 2487 connection->le_con_param_update_identifier = sig_id; 2488 } 2489 2490 if (!l2cap_event_packet_handler) break; 2491 2492 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 2493 event[1] = 8; 2494 memcpy(&event[2], &command[4], 8); 2495 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2496 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2497 break; 2498 2499 #ifdef ENABLE_LE_DATA_CHANNELS 2500 2501 case COMMAND_REJECT: 2502 // Find channel for this sig_id and connection handle 2503 channel = NULL; 2504 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2505 while (btstack_linked_list_iterator_has_next(&it)){ 2506 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2507 if (a_channel->con_handle != handle) continue; 2508 if (a_channel->local_sig_id != sig_id) continue; 2509 channel = a_channel; 2510 break; 2511 } 2512 if (!channel) break; 2513 2514 // if received while waiting for le connection response, assume legacy device 2515 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 2516 channel->state = L2CAP_STATE_CLOSED; 2517 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 2518 l2cap_emit_le_channel_opened(channel, 0x0002); 2519 2520 // discard channel 2521 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2522 btstack_memory_l2cap_channel_free(channel); 2523 break; 2524 } 2525 break; 2526 2527 case LE_CREDIT_BASED_CONNECTION_REQUEST: 2528 2529 // get hci connection, bail if not found (must not happen) 2530 connection = hci_connection_for_handle(handle); 2531 if (!connection) return 0; 2532 2533 // check if service registered 2534 le_psm = little_endian_read_16(command, 4); 2535 service = l2cap_le_get_service(le_psm); 2536 source_cid = little_endian_read_16(command, 6); 2537 2538 if (service){ 2539 if (source_cid < 0x40){ 2540 // 0x0009 Connection refused - Invalid Source CID 2541 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 2542 return 1; 2543 } 2544 2545 // go through list of channels for this ACL connection and check if we get a match 2546 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2547 while (btstack_linked_list_iterator_has_next(&it)){ 2548 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2549 if (a_channel->con_handle != handle) continue; 2550 if (a_channel->remote_cid != source_cid) continue; 2551 // 0x000a Connection refused - Source CID already allocated 2552 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 2553 return 1; 2554 } 2555 2556 // security: check encryption 2557 if (service->required_security_level >= LEVEL_2){ 2558 if (sm_encryption_key_size(handle) == 0){ 2559 // 0x0008 Connection refused - insufficient encryption 2560 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 2561 return 1; 2562 } 2563 // anything less than 16 byte key size is insufficient 2564 if (sm_encryption_key_size(handle) < 16){ 2565 // 0x0007 Connection refused – insufficient encryption key size 2566 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 2567 return 1; 2568 } 2569 } 2570 2571 // security: check authencation 2572 if (service->required_security_level >= LEVEL_3){ 2573 if (!sm_authenticated(handle)){ 2574 // 0x0005 Connection refused – insufficient authentication 2575 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 2576 return 1; 2577 } 2578 } 2579 2580 // security: check authorization 2581 if (service->required_security_level >= LEVEL_4){ 2582 if (sm_authorization_state(handle) != AUTHORIZATION_GRANTED){ 2583 // 0x0006 Connection refused – insufficient authorization 2584 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 2585 return 1; 2586 } 2587 } 2588 2589 // allocate channel 2590 channel = l2cap_create_channel_entry(service->packet_handler, connection->address, 2591 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 2592 if (!channel){ 2593 // 0x0004 Connection refused – no resources available 2594 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2595 return 1; 2596 } 2597 2598 channel->con_handle = handle; 2599 channel->remote_cid = source_cid; 2600 channel->remote_sig_id = sig_id; 2601 channel->remote_mtu = little_endian_read_16(command, 8); 2602 channel->remote_mps = little_endian_read_16(command, 10); 2603 channel->credits_outgoing = little_endian_read_16(command, 12); 2604 2605 // set initial state 2606 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2607 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 2608 2609 // add to connections list 2610 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2611 2612 // post connection request event 2613 l2cap_emit_le_incoming_connection(channel); 2614 2615 } else { 2616 // Connection refused – LE_PSM not supported 2617 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2618 } 2619 break; 2620 2621 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 2622 // Find channel for this sig_id and connection handle 2623 channel = NULL; 2624 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2625 while (btstack_linked_list_iterator_has_next(&it)){ 2626 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2627 if (a_channel->con_handle != handle) continue; 2628 if (a_channel->local_sig_id != sig_id) continue; 2629 channel = a_channel; 2630 break; 2631 } 2632 if (!channel) break; 2633 2634 // cid + 0 2635 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 2636 if (result){ 2637 channel->state = L2CAP_STATE_CLOSED; 2638 // map l2cap connection response result to BTstack status enumeration 2639 l2cap_emit_le_channel_opened(channel, result); 2640 2641 // discard channel 2642 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2643 btstack_memory_l2cap_channel_free(channel); 2644 break; 2645 } 2646 2647 // success 2648 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2649 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2650 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 2651 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 2652 channel->state = L2CAP_STATE_OPEN; 2653 l2cap_emit_le_channel_opened(channel, result); 2654 break; 2655 2656 case LE_FLOW_CONTROL_CREDIT: 2657 // find channel 2658 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2659 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2660 if (!channel) { 2661 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2662 break; 2663 } 2664 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2665 credits_before = channel->credits_outgoing; 2666 channel->credits_outgoing += new_credits; 2667 // check for credit overrun 2668 if (credits_before > channel->credits_outgoing){ 2669 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 2670 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2671 break; 2672 } 2673 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 2674 break; 2675 2676 case DISCONNECTION_REQUEST: 2677 // find channel 2678 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2679 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2680 if (!channel) { 2681 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2682 break; 2683 } 2684 channel->remote_sig_id = sig_id; 2685 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2686 break; 2687 2688 #endif 2689 2690 case DISCONNECTION_RESPONSE: 2691 break; 2692 2693 default: 2694 // command unknown -> reject command 2695 return 0; 2696 } 2697 return 1; 2698 } 2699 #endif 2700 2701 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2702 2703 // @param delta number of frames in the future, >= 1 2704 static void l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel_t * l2cap_channel, l2cap_segmentation_and_reassembly_t sar, int delta, uint8_t * payload, uint16_t size){ 2705 log_info("Store SDU with delta %u", delta); 2706 // get rx state for packet to store 2707 int index = l2cap_channel->rx_store_index + delta - 1; 2708 if (index > l2cap_channel->num_rx_buffers){ 2709 index -= l2cap_channel->num_rx_buffers; 2710 } 2711 log_info("Index of packet to store %u", index); 2712 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 2713 // check if buffer is free 2714 if (rx_state->valid){ 2715 log_error("Packet buffer already used"); 2716 return; 2717 } 2718 rx_state->valid = 1; 2719 rx_state->sar = sar; 2720 rx_state->len = size; 2721 uint8_t * rx_buffer = &l2cap_channel->rx_packets_data[index]; 2722 memcpy(rx_buffer, payload, size); 2723 } 2724 2725 static void l2cap_ertm_handle_in_sequence_sdu(l2cap_channel_t * l2cap_channel, l2cap_segmentation_and_reassembly_t sar, uint8_t * payload, uint16_t size){ 2726 switch (sar){ 2727 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 2728 // packet complete -> disapatch 2729 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, payload, size); 2730 break; 2731 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 2732 // TODO: check if reassembly started 2733 // TODO: check sdu_len against local mtu 2734 l2cap_channel->reassembly_sdu_length = little_endian_read_16(payload, 0); 2735 payload += 2; 2736 size -= 2; 2737 memcpy(&l2cap_channel->reassembly_buffer[0], payload, size); 2738 l2cap_channel->reassembly_pos = size; 2739 break; 2740 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 2741 memcpy(&l2cap_channel->reassembly_buffer[l2cap_channel->reassembly_pos], payload, size); 2742 l2cap_channel->reassembly_pos += size; 2743 break; 2744 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 2745 memcpy(&l2cap_channel->reassembly_buffer[l2cap_channel->reassembly_pos], payload, size); 2746 l2cap_channel->reassembly_pos += size; 2747 // packet complete -> disapatch 2748 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->reassembly_buffer, l2cap_channel->reassembly_pos); 2749 l2cap_channel->reassembly_pos = 0; 2750 break; 2751 } 2752 } 2753 #endif 2754 2755 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size ){ 2756 UNUSED(packet_type); 2757 UNUSED(channel); 2758 2759 l2cap_channel_t * l2cap_channel; 2760 UNUSED(l2cap_channel); 2761 2762 // Get Channel ID 2763 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 2764 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 2765 2766 switch (channel_id) { 2767 2768 #ifdef ENABLE_CLASSIC 2769 case L2CAP_CID_SIGNALING: { 2770 uint16_t command_offset = 8; 2771 while (command_offset < size) { 2772 // handle signaling commands 2773 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 2774 2775 // increment command_offset 2776 command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2777 } 2778 break; 2779 } 2780 #endif 2781 2782 #ifdef ENABLE_BLE 2783 case L2CAP_CID_SIGNALING_LE: { 2784 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 2785 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 2786 if (!valid){ 2787 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2788 } 2789 break; 2790 } 2791 #endif 2792 2793 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 2794 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback) { 2795 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2796 } 2797 break; 2798 2799 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 2800 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback) { 2801 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2802 } 2803 break; 2804 2805 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 2806 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback) { 2807 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2808 } 2809 break; 2810 2811 default: 2812 #ifdef ENABLE_CLASSIC 2813 // Find channel for this channel_id and connection handle 2814 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 2815 if (l2cap_channel) { 2816 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2817 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2818 2819 // verify FCS 2820 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 2821 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 2822 log_info("Packet FCS 0x%04x, calculated FCS 0x%04x", fcs_packet, fcs_calculated); 2823 if (fcs_calculated != fcs_packet){ 2824 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 2825 // TODO: trigger retransmission or something like that 2826 break; 2827 } 2828 2829 // switch on packet type 2830 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2831 uint8_t req_seq = (control >> 8) & 0x3f; 2832 int final = (control >> 7) & 0x01; 2833 if (control & 1){ 2834 // S-Frame 2835 int poll = (control >> 4) & 0x01; 2836 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 2837 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 2838 l2cap_ertm_tx_packet_state_t * tx_state; 2839 switch (s){ 2840 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 2841 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 2842 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2843 if (poll && final){ 2844 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 2845 log_error("P=F=1 in S-Frame"); 2846 break; 2847 } 2848 if (poll){ 2849 // check if we did request selective retransmission before <==> we have stored SDU segments 2850 int i; 2851 int num_stored_out_of_order_packets = 0; 2852 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 2853 int index = l2cap_channel->rx_store_index + i; 2854 if (index >= l2cap_channel->num_rx_buffers){ 2855 index -= l2cap_channel->num_rx_buffers; 2856 } 2857 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 2858 if (!rx_state->valid) continue; 2859 num_stored_out_of_order_packets++; 2860 } 2861 if (num_stored_out_of_order_packets){ 2862 l2cap_channel->send_supervisor_frame_selective_reject = 1; 2863 } else { 2864 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 2865 } 2866 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 2867 } 2868 if (final){ 2869 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 2870 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 2871 } 2872 break; 2873 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 2874 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 2875 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2876 // rsetart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_handle_req_seq) 2877 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 2878 break; 2879 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 2880 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 2881 break; 2882 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 2883 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 2884 if (poll){ 2885 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2886 } 2887 // find requested i-frame 2888 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 2889 if (tx_state){ 2890 log_info("Retransmission for tx_seq %u requested", req_seq); 2891 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 2892 tx_state->retransmission_requested = 1; 2893 l2cap_channel->srej_active = 1; 2894 } 2895 break; 2896 default: 2897 break; 2898 } 2899 break; 2900 } else { 2901 // I-Frame 2902 // get control 2903 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 2904 uint8_t tx_seq = (control >> 1) & 0x3f; 2905 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 2906 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 2907 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 2908 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2909 if (final){ 2910 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 2911 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 2912 } 2913 // check ordering 2914 if (l2cap_channel->expected_tx_seq == tx_seq){ 2915 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 2916 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 2917 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 2918 2919 // process SDU 2920 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, &packet[COMPLETE_L2CAP_HEADER+2], size-(COMPLETE_L2CAP_HEADER+2)); 2921 2922 // process stored segments 2923 while (1){ 2924 int index = l2cap_channel->rx_store_index; 2925 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 2926 if (!rx_state->valid) break; 2927 2928 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 2929 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 2930 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 2931 2932 rx_state->valid = 0; 2933 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 2934 2935 // update rx store index 2936 index++; 2937 if (index >= l2cap_channel->num_rx_buffers){ 2938 index = 0; 2939 } 2940 l2cap_channel->rx_store_index = index; 2941 } 2942 2943 // 2944 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 2945 2946 } else { 2947 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 2948 if (delta < 2){ 2949 // store segment 2950 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, &packet[COMPLETE_L2CAP_HEADER+2], size-(COMPLETE_L2CAP_HEADER+2)); 2951 2952 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 2953 l2cap_channel->send_supervisor_frame_selective_reject = 1; 2954 } else { 2955 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 2956 l2cap_channel->send_supervisor_frame_reject = 1; 2957 } 2958 } 2959 } 2960 break; 2961 } 2962 #endif 2963 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2964 } 2965 #endif 2966 #ifdef ENABLE_LE_DATA_CHANNELS 2967 l2cap_channel = l2cap_le_get_channel_for_local_cid(channel_id); 2968 if (l2cap_channel) { 2969 // credit counting 2970 if (l2cap_channel->credits_incoming == 0){ 2971 log_error("LE Data Channel packet received but no incoming credits"); 2972 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2973 break; 2974 } 2975 l2cap_channel->credits_incoming--; 2976 2977 // automatic credits 2978 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 2979 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 2980 } 2981 2982 // first fragment 2983 uint16_t pos = 0; 2984 if (!l2cap_channel->receive_sdu_len){ 2985 l2cap_channel->receive_sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2986 l2cap_channel->receive_sdu_pos = 0; 2987 pos += 2; 2988 size -= 2; 2989 } 2990 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], size-COMPLETE_L2CAP_HEADER); 2991 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 2992 // done? 2993 log_info("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 2994 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 2995 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 2996 l2cap_channel->receive_sdu_len = 0; 2997 } 2998 } else { 2999 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 3000 } 3001 #endif 3002 break; 3003 } 3004 3005 l2cap_run(); 3006 } 3007 3008 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3009 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 3010 int index = l2cap_fixed_channel_table_index_for_channel_id(channel_id); 3011 if (index < 0) return; 3012 fixed_channels[index].callback = the_packet_handler; 3013 } 3014 3015 #ifdef ENABLE_CLASSIC 3016 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3017 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3018 channel->state = L2CAP_STATE_CLOSED; 3019 l2cap_emit_channel_closed(channel); 3020 // discard channel 3021 l2cap_stop_rtx(channel); 3022 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3023 btstack_memory_l2cap_channel_free(channel); 3024 } 3025 3026 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3027 btstack_linked_list_iterator_t it; 3028 btstack_linked_list_iterator_init(&it, services); 3029 while (btstack_linked_list_iterator_has_next(&it)){ 3030 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3031 if ( service->psm == psm){ 3032 return service; 3033 }; 3034 } 3035 return NULL; 3036 } 3037 3038 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3039 return l2cap_get_service_internal(&l2cap_services, psm); 3040 } 3041 3042 3043 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3044 3045 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3046 3047 // check for alread registered psm 3048 l2cap_service_t *service = l2cap_get_service(psm); 3049 if (service) { 3050 log_error("l2cap_register_service: PSM %u already registered", psm); 3051 return L2CAP_SERVICE_ALREADY_REGISTERED; 3052 } 3053 3054 // alloc structure 3055 service = btstack_memory_l2cap_service_get(); 3056 if (!service) { 3057 log_error("l2cap_register_service: no memory for l2cap_service_t"); 3058 return BTSTACK_MEMORY_ALLOC_FAILED; 3059 } 3060 3061 // fill in 3062 service->psm = psm; 3063 service->mtu = mtu; 3064 service->packet_handler = service_packet_handler; 3065 service->required_security_level = security_level; 3066 3067 // add to services list 3068 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3069 3070 // enable page scan 3071 gap_connectable_control(1); 3072 3073 return 0; 3074 } 3075 3076 uint8_t l2cap_unregister_service(uint16_t psm){ 3077 3078 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 3079 3080 l2cap_service_t *service = l2cap_get_service(psm); 3081 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3082 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 3083 btstack_memory_l2cap_service_free(service); 3084 3085 // disable page scan when no services registered 3086 if (btstack_linked_list_empty(&l2cap_services)) { 3087 gap_connectable_control(0); 3088 } 3089 return 0; 3090 } 3091 #endif 3092 3093 3094 #ifdef ENABLE_LE_DATA_CHANNELS 3095 3096 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 3097 if (!channel->waiting_for_can_send_now) return; 3098 if (channel->send_sdu_buffer) return; 3099 channel->waiting_for_can_send_now = 0; 3100 log_info("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 3101 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 3102 } 3103 3104 // 1BH2222 3105 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 3106 log_info("L2CAP_EVENT_LE_INCOMING_CONNECTION addr_type %u, addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x, remote_mtu %u", 3107 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 3108 uint8_t event[19]; 3109 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 3110 event[1] = sizeof(event) - 2; 3111 event[2] = channel->address_type; 3112 reverse_bd_addr(channel->address, &event[3]); 3113 little_endian_store_16(event, 9, channel->con_handle); 3114 little_endian_store_16(event, 11, channel->psm); 3115 little_endian_store_16(event, 13, channel->local_cid); 3116 little_endian_store_16(event, 15, channel->remote_cid); 3117 little_endian_store_16(event, 17, channel->remote_mtu); 3118 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3119 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3120 } 3121 // 11BH22222 3122 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 3123 log_info("L2CAP_EVENT_LE_CHANNEL_OPENED status 0x%x addr_type %u addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x local_mtu %u, remote_mtu %u", 3124 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 3125 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 3126 uint8_t event[23]; 3127 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 3128 event[1] = sizeof(event) - 2; 3129 event[2] = status; 3130 event[3] = channel->address_type; 3131 reverse_bd_addr(channel->address, &event[4]); 3132 little_endian_store_16(event, 10, channel->con_handle); 3133 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 3134 little_endian_store_16(event, 13, channel->psm); 3135 little_endian_store_16(event, 15, channel->local_cid); 3136 little_endian_store_16(event, 17, channel->remote_cid); 3137 little_endian_store_16(event, 19, channel->local_mtu); 3138 little_endian_store_16(event, 21, channel->remote_mtu); 3139 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3140 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3141 } 3142 3143 static l2cap_channel_t * l2cap_le_get_channel_for_local_cid(uint16_t local_cid){ 3144 btstack_linked_list_iterator_t it; 3145 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 3146 while (btstack_linked_list_iterator_has_next(&it)){ 3147 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3148 if ( channel->local_cid == local_cid) { 3149 return channel; 3150 } 3151 } 3152 return NULL; 3153 } 3154 3155 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3156 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 3157 channel->state = L2CAP_STATE_CLOSED; 3158 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 3159 // discard channel 3160 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 3161 btstack_memory_l2cap_channel_free(channel); 3162 } 3163 3164 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 3165 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 3166 } 3167 3168 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 3169 3170 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 3171 3172 // check for alread registered psm 3173 l2cap_service_t *service = l2cap_le_get_service(psm); 3174 if (service) { 3175 return L2CAP_SERVICE_ALREADY_REGISTERED; 3176 } 3177 3178 // alloc structure 3179 service = btstack_memory_l2cap_service_get(); 3180 if (!service) { 3181 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 3182 return BTSTACK_MEMORY_ALLOC_FAILED; 3183 } 3184 3185 // fill in 3186 service->psm = psm; 3187 service->mtu = 0; 3188 service->packet_handler = packet_handler; 3189 service->required_security_level = security_level; 3190 3191 // add to services list 3192 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 3193 3194 // done 3195 return 0; 3196 } 3197 3198 uint8_t l2cap_le_unregister_service(uint16_t psm) { 3199 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 3200 l2cap_service_t *service = l2cap_le_get_service(psm); 3201 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3202 3203 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 3204 btstack_memory_l2cap_service_free(service); 3205 return 0; 3206 } 3207 3208 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 3209 // get channel 3210 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3211 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3212 3213 // validate state 3214 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3215 return ERROR_CODE_COMMAND_DISALLOWED; 3216 } 3217 3218 // set state accept connection 3219 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 3220 channel->receive_sdu_buffer = receive_sdu_buffer; 3221 channel->local_mtu = mtu; 3222 channel->new_credits_incoming = initial_credits; 3223 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3224 3225 // test 3226 // channel->new_credits_incoming = 1; 3227 3228 // go 3229 l2cap_run(); 3230 return 0; 3231 } 3232 3233 /** 3234 * @brief Deny incoming LE Data Channel connection due to resource constraints 3235 * @param local_cid L2CAP LE Data Channel Identifier 3236 */ 3237 3238 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 3239 // get channel 3240 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3241 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3242 3243 // validate state 3244 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3245 return ERROR_CODE_COMMAND_DISALLOWED; 3246 } 3247 3248 // set state decline connection 3249 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 3250 channel->reason = 0x04; // no resources available 3251 l2cap_run(); 3252 return 0; 3253 } 3254 3255 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 3256 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 3257 uint16_t * out_local_cid) { 3258 3259 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 3260 3261 3262 hci_connection_t * connection = hci_connection_for_handle(con_handle); 3263 if (!connection) { 3264 log_error("no hci_connection for handle 0x%04x", con_handle); 3265 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 3266 } 3267 3268 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, connection->address, connection->address_type, psm, mtu, security_level); 3269 if (!channel) { 3270 return BTSTACK_MEMORY_ALLOC_FAILED; 3271 } 3272 log_info("l2cap_le_create_channel %p", channel); 3273 3274 // store local_cid 3275 if (out_local_cid){ 3276 *out_local_cid = channel->local_cid; 3277 } 3278 3279 // provide buffer 3280 channel->con_handle = con_handle; 3281 channel->receive_sdu_buffer = receive_sdu_buffer; 3282 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 3283 channel->new_credits_incoming = initial_credits; 3284 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3285 3286 // add to connections list 3287 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 3288 3289 // go 3290 l2cap_run(); 3291 return 0; 3292 } 3293 3294 /** 3295 * @brief Provide credtis for LE Data Channel 3296 * @param local_cid L2CAP LE Data Channel Identifier 3297 * @param credits Number additional credits for peer 3298 */ 3299 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 3300 3301 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3302 if (!channel) { 3303 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3304 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3305 } 3306 3307 // check state 3308 if (channel->state != L2CAP_STATE_OPEN){ 3309 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 3310 } 3311 3312 // assert incoming credits + credits <= 0xffff 3313 uint32_t total_credits = channel->credits_incoming; 3314 total_credits += channel->new_credits_incoming; 3315 total_credits += credits; 3316 if (total_credits > 0xffff){ 3317 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 3318 channel->new_credits_incoming, credits); 3319 } 3320 3321 // set credits_granted 3322 channel->new_credits_incoming += credits; 3323 3324 // go 3325 l2cap_run(); 3326 return 0; 3327 } 3328 3329 /** 3330 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 3331 * @param local_cid L2CAP LE Data Channel Identifier 3332 */ 3333 int l2cap_le_can_send_now(uint16_t local_cid){ 3334 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3335 if (!channel) { 3336 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3337 return 0; 3338 } 3339 3340 // check state 3341 if (channel->state != L2CAP_STATE_OPEN) return 0; 3342 3343 // check queue 3344 if (channel->send_sdu_buffer) return 0; 3345 3346 // fine, go ahead 3347 return 1; 3348 } 3349 3350 /** 3351 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 3352 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 3353 * so packet handler should be ready to handle it 3354 * @param local_cid L2CAP LE Data Channel Identifier 3355 */ 3356 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 3357 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3358 if (!channel) { 3359 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 3360 return 0; 3361 } 3362 channel->waiting_for_can_send_now = 1; 3363 l2cap_le_notify_channel_can_send(channel); 3364 return 0; 3365 } 3366 3367 /** 3368 * @brief Send data via LE Data Channel 3369 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 3370 * @param local_cid L2CAP LE Data Channel Identifier 3371 * @param data data to send 3372 * @param size data size 3373 */ 3374 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 3375 3376 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3377 if (!channel) { 3378 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3379 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3380 } 3381 3382 if (len > channel->remote_mtu){ 3383 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 3384 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 3385 } 3386 3387 if (channel->send_sdu_buffer){ 3388 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 3389 return BTSTACK_ACL_BUFFERS_FULL; 3390 } 3391 3392 channel->send_sdu_buffer = data; 3393 channel->send_sdu_len = len; 3394 channel->send_sdu_pos = 0; 3395 3396 l2cap_run(); 3397 return 0; 3398 } 3399 3400 /** 3401 * @brief Disconnect from LE Data Channel 3402 * @param local_cid L2CAP LE Data Channel Identifier 3403 */ 3404 uint8_t l2cap_le_disconnect(uint16_t local_cid) 3405 { 3406 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3407 if (!channel) { 3408 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3409 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3410 } 3411 3412 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3413 l2cap_run(); 3414 return 0; 3415 } 3416 3417 #endif 3418