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