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