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