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, L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU); 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 static int l2cap_ertm_send(l2cap_channel_t * channel, uint8_t * data, uint16_t len){ 629 if (len > channel->remote_mtu){ 630 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", channel->local_cid); 631 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 632 } 633 // TODO: check tx_transmit 634 // store int tx packet bufferx 635 int index = channel->tx_write_index; 636 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index]; 637 tx_state->tx_seq = channel->next_tx_seq; 638 tx_state->len = len; 639 tx_state->retry_count = 0; 640 memcpy(&channel->tx_packets_data[index * channel->local_mtu], data, len); 641 // update 642 channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq); 643 l2cap_ertm_next_tx_write_index(channel); 644 // set retransmission timer 645 btstack_run_loop_set_timer_handler(&tx_state->retransmission_timer, &l2cap_ertm_retransmission_timeout_callback); 646 btstack_run_loop_set_timer_context(&tx_state->retransmission_timer, channel); 647 btstack_run_loop_set_timer(&tx_state->retransmission_timer, channel->local_retransmission_timeout_ms); 648 btstack_run_loop_add_timer(&tx_state->retransmission_timer); 649 // try to send 650 l2cap_run(); 651 return 0; 652 } 653 #endif 654 655 // assumption - only on Classic connections 656 int l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){ 657 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 658 if (!channel) { 659 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 660 return -1; // TODO: define error 661 } 662 663 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 664 // send in ERTM 665 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 666 return l2cap_ertm_send(channel, data, len); 667 } 668 #endif 669 670 if (len > channel->remote_mtu){ 671 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 672 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 673 } 674 675 if (!hci_can_send_acl_packet_now(channel->con_handle)){ 676 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 677 return BTSTACK_ACL_BUFFERS_FULL; 678 } 679 680 hci_reserve_packet_buffer(); 681 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 682 memcpy(&acl_buffer[8], data, len); 683 return l2cap_send_prepared(local_cid, len); 684 } 685 686 int l2cap_send_echo_request(hci_con_handle_t con_handle, uint8_t *data, uint16_t len){ 687 return l2cap_send_signaling_packet(con_handle, ECHO_REQUEST, 0x77, len, data); 688 } 689 690 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 691 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var | flag); 692 } 693 694 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 695 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var & ~flag); 696 } 697 #endif 698 699 700 #ifdef ENABLE_BLE 701 static int l2cap_send_le_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 702 703 if (!hci_can_send_acl_packet_now(handle)){ 704 log_info("l2cap_send_le_signaling_packet, cannot send"); 705 return BTSTACK_ACL_BUFFERS_FULL; 706 } 707 708 // log_info("l2cap_send_le_signaling_packet type %u", cmd); 709 hci_reserve_packet_buffer(); 710 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 711 va_list argptr; 712 va_start(argptr, identifier); 713 uint16_t len = l2cap_create_signaling_le(acl_buffer, handle, cmd, identifier, argptr); 714 va_end(argptr); 715 // log_info("l2cap_send_le_signaling_packet con %u!", handle); 716 return hci_send_acl_packet_buffer(len); 717 } 718 #endif 719 720 uint16_t l2cap_max_mtu(void){ 721 return HCI_ACL_PAYLOAD_SIZE - L2CAP_HEADER_SIZE; 722 } 723 724 uint16_t l2cap_max_le_mtu(void){ 725 return l2cap_max_mtu(); 726 } 727 728 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 729 static uint16_t l2cap_setup_options_ertm(l2cap_channel_t * channel, uint8_t * config_options){ 730 config_options[0] = 0x04; // RETRANSMISSION AND FLOW CONTROL OPTION 731 config_options[1] = 9; // length 732 config_options[2] = (uint8_t) channel->mode; 733 config_options[3] = channel->num_rx_buffers; // == TxWindows size 734 config_options[4] = channel->local_max_transmit; 735 little_endian_store_16( config_options, 5, channel->local_retransmission_timeout_ms); 736 little_endian_store_16( config_options, 7, channel->local_monitor_timeout_ms); 737 little_endian_store_16( config_options, 9, channel->local_mps); 738 return 11; 739 } 740 static int l2cap_ertm_send_supervisor_frame(l2cap_channel_t * channel, uint16_t control){ 741 hci_reserve_packet_buffer(); 742 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 743 log_info("S-Frame: control 0x%04x", control); 744 little_endian_store_16(acl_buffer, 8, control); 745 return l2cap_send_prepared(channel->local_cid, 2); 746 } 747 static int l2cap_ertm_num_unacknowledged_tx_packets(l2cap_channel_t * channel){ 748 int unacknowledged_packets = channel->tx_send_index - channel->tx_read_index; 749 if (unacknowledged_packets < 0){ 750 unacknowledged_packets += channel->num_tx_buffers; 751 } 752 return unacknowledged_packets; 753 } 754 #endif 755 756 #ifdef ENABLE_CLASSIC 757 758 static uint16_t l2cap_setup_options_mtu(l2cap_channel_t * channel, uint8_t * config_options){ 759 config_options[0] = 1; // MTU 760 config_options[1] = 2; // len param 761 little_endian_store_16( (uint8_t*)&config_options, 2, channel->local_mtu); 762 return 4; 763 } 764 765 static uint16_t l2cap_setup_options(l2cap_channel_t * channel, uint8_t * config_options){ 766 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 767 // use ERTM options if supported 768 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 769 if ((connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_DONE) && (connection->l2cap_state.extended_feature_mask & 0x08)){ 770 return l2cap_setup_options_ertm(channel, config_options); 771 772 } 773 #endif 774 return l2cap_setup_options_mtu(channel, config_options); 775 } 776 777 static uint32_t l2cap_extended_features_mask(void){ 778 // extended features request supported, features: fixed channels, unicast connectionless data reception 779 uint32_t features = 0x280; 780 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 781 features |= 0x0008; 782 #endif 783 return features; 784 } 785 #endif 786 787 // MARK: L2CAP_RUN 788 // process outstanding signaling tasks 789 static void l2cap_run(void){ 790 791 // log_info("l2cap_run: entered"); 792 793 // check pending signaling responses 794 while (signaling_responses_pending){ 795 796 hci_con_handle_t handle = signaling_responses[0].handle; 797 798 if (!hci_can_send_acl_packet_now(handle)) break; 799 800 uint8_t sig_id = signaling_responses[0].sig_id; 801 uint8_t response_code = signaling_responses[0].code; 802 uint16_t infoType = signaling_responses[0].data; // INFORMATION_REQUEST 803 uint16_t result = signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT 804 #ifdef ENABLE_CLASSIC 805 uint16_t source_cid = signaling_responses[0].cid; // CONNECTION_REQUEST 806 #endif 807 UNUSED(infoType); 808 809 // remove first item before sending (to avoid sending response mutliple times) 810 signaling_responses_pending--; 811 int i; 812 for (i=0; i < signaling_responses_pending; i++){ 813 memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t)); 814 } 815 816 switch (response_code){ 817 #ifdef ENABLE_CLASSIC 818 case CONNECTION_REQUEST: 819 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 820 // also disconnect if result is 0x0003 - security blocked 821 if (result == 0x0003){ 822 hci_disconnect_security_block(handle); 823 } 824 break; 825 case ECHO_REQUEST: 826 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 827 break; 828 case INFORMATION_REQUEST: 829 switch (infoType){ 830 case 1: { // Connectionless MTU 831 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 832 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(connectionless_mtu), &connectionless_mtu); 833 } 834 break; 835 case 2: { // Extended Features Supported 836 uint32_t features = l2cap_extended_features_mask(); 837 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(features), &features); 838 } 839 break; 840 case 3: { // Fixed Channels Supported 841 uint8_t map[8]; 842 memset(map, 0, 8); 843 map[0] = 0x06; // L2CAP Signaling Channel (0x02) + Connectionless reception (0x04) 844 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(map), &map); 845 } 846 break; 847 default: 848 // all other types are not supported 849 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL); 850 break; 851 } 852 break; 853 case COMMAND_REJECT: 854 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 855 break; 856 #endif 857 #ifdef ENABLE_BLE 858 case LE_CREDIT_BASED_CONNECTION_REQUEST: 859 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 860 break; 861 case COMMAND_REJECT_LE: 862 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 863 break; 864 #endif 865 default: 866 // should not happen 867 break; 868 } 869 } 870 871 btstack_linked_list_iterator_t it; 872 UNUSED(it); 873 874 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 875 // send l2cap information request if neccessary 876 hci_connections_get_iterator(&it); 877 while(btstack_linked_list_iterator_has_next(&it)){ 878 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 879 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST){ 880 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE; 881 // send information request for extended features 882 uint8_t sig_id = l2cap_next_sig_id(); 883 uint8_t info_type = 2; 884 l2cap_send_signaling_packet(connection->con_handle, INFORMATION_REQUEST, sig_id, info_type); 885 return; 886 } 887 } 888 #endif 889 890 #ifdef ENABLE_CLASSIC 891 uint8_t config_options[10]; 892 btstack_linked_list_iterator_init(&it, &l2cap_channels); 893 while (btstack_linked_list_iterator_has_next(&it)){ 894 895 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 896 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 897 switch (channel->state){ 898 899 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 900 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 901 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 902 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 903 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 904 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0); 905 } 906 break; 907 908 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 909 if (!hci_can_send_command_packet_now()) break; 910 // send connection request - set state first 911 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 912 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 913 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1); 914 break; 915 916 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 917 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 918 channel->state = L2CAP_STATE_INVALID; 919 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0); 920 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 921 l2cap_stop_rtx(channel); 922 btstack_linked_list_iterator_remove(&it); 923 btstack_memory_l2cap_channel_free(channel); 924 break; 925 926 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 927 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 928 channel->state = L2CAP_STATE_CONFIG; 929 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 930 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 931 break; 932 933 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 934 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 935 // success, start l2cap handshake 936 channel->local_sig_id = l2cap_next_sig_id(); 937 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 938 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 939 l2cap_start_rtx(channel); 940 break; 941 942 case L2CAP_STATE_CONFIG: 943 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 944 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 945 uint16_t flags = 0; 946 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 947 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 948 flags = 1; 949 } else { 950 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 951 } 952 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 953 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 954 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL); 955 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 956 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 957 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 958 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 959 uint16_t options_size = l2cap_setup_options(channel, config_options); 960 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); 961 #endif 962 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 963 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 964 uint16_t options_size = l2cap_setup_options(channel, config_options); 965 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); 966 } else { 967 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 968 } 969 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 970 } 971 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 972 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 973 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 974 channel->local_sig_id = l2cap_next_sig_id(); 975 uint16_t options_size = l2cap_setup_options(channel, config_options); 976 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, options_size, &config_options); 977 l2cap_start_rtx(channel); 978 } 979 if (l2cap_channel_ready_for_open(channel)){ 980 channel->state = L2CAP_STATE_OPEN; 981 l2cap_emit_channel_opened(channel, 0); // success 982 } 983 break; 984 985 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 986 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 987 channel->state = L2CAP_STATE_INVALID; 988 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 989 // 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 :) 990 l2cap_finialize_channel_close(channel); // -- remove from list 991 break; 992 993 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 994 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 995 channel->local_sig_id = l2cap_next_sig_id(); 996 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 997 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 998 break; 999 default: 1000 break; 1001 } 1002 1003 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1004 // send s-frame to acknowledge received packets 1005 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1006 1007 if (channel->tx_send_index != channel->tx_write_index){ 1008 int unacknowledged_packets = l2cap_ertm_num_unacknowledged_tx_packets(channel); 1009 // check remote tx window 1010 log_info("unacknowledged_packets %u, remote tx window size %u", unacknowledged_packets, channel->remote_tx_window_size); 1011 if (unacknowledged_packets < channel->remote_tx_window_size){ 1012 int index = channel->tx_send_index; 1013 channel->tx_send_index++; 1014 if (channel->tx_send_index >= channel->num_tx_buffers){ 1015 channel->tx_send_index = 0; 1016 } 1017 l2cap_ertm_send_information_frame(channel, index, 0); // final = 0 1018 continue; 1019 } 1020 } 1021 1022 if (channel->send_supervisor_frame_receiver_ready){ 1023 channel->send_supervisor_frame_receiver_ready = 0; 1024 log_info("Send S-Frame: RR %u, final %u", channel->req_seq, channel->set_final_bit_after_packet_with_poll_bit_set); 1025 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); 1026 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1027 l2cap_ertm_send_supervisor_frame(channel, control); 1028 continue; 1029 } 1030 if (channel->send_supervisor_frame_receiver_ready_poll){ 1031 channel->send_supervisor_frame_receiver_ready_poll = 0; 1032 log_info("Send S-Frame: RR %u with poll=1 ", channel->req_seq); 1033 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 1, 0, channel->req_seq); 1034 l2cap_ertm_send_supervisor_frame(channel, control); 1035 continue; 1036 } 1037 if (channel->send_supervisor_frame_receiver_not_ready){ 1038 channel->send_supervisor_frame_receiver_not_ready = 0; 1039 log_info("Send S-Frame: RNR %u", channel->req_seq); 1040 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 1041 l2cap_ertm_send_supervisor_frame(channel, control); 1042 continue; 1043 } 1044 if (channel->send_supervisor_frame_reject){ 1045 channel->send_supervisor_frame_reject = 0; 1046 log_info("Send S-Frame: REJ %u", channel->req_seq); 1047 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT, 0, 0, channel->req_seq); 1048 l2cap_ertm_send_supervisor_frame(channel, control); 1049 continue; 1050 } 1051 if (channel->send_supervisor_frame_selective_reject){ 1052 channel->send_supervisor_frame_selective_reject = 0; 1053 log_info("Send S-Frame: SREJ %u", channel->expected_tx_seq); 1054 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT, 0, 0, channel->expected_tx_seq); 1055 l2cap_ertm_send_supervisor_frame(channel, control); 1056 continue; 1057 } 1058 1059 if (channel->srej_active){ 1060 int i; 1061 for (i=0;i<channel->num_tx_buffers;i++){ 1062 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[i]; 1063 if (tx_state->retransmission_requested) { 1064 tx_state->retransmission_requested = 0; 1065 uint8_t final = channel->set_final_bit_after_packet_with_poll_bit_set; 1066 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1067 l2cap_ertm_send_information_frame(channel, i, final); 1068 break; 1069 } 1070 } 1071 if (i == channel->num_tx_buffers){ 1072 // no retransmission request found 1073 channel->srej_active = 0; 1074 } else { 1075 // packet was sent 1076 continue; 1077 } 1078 } 1079 #endif 1080 1081 } 1082 #endif 1083 1084 #ifdef ENABLE_LE_DATA_CHANNELS 1085 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1086 while (btstack_linked_list_iterator_has_next(&it)){ 1087 uint8_t * acl_buffer; 1088 uint8_t * l2cap_payload; 1089 uint16_t pos; 1090 uint16_t payload_size; 1091 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1092 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1093 switch (channel->state){ 1094 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1095 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1096 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1097 // le psm, source cid, mtu, mps, initial credits 1098 channel->local_sig_id = l2cap_next_sig_id(); 1099 channel->credits_incoming = channel->new_credits_incoming; 1100 channel->new_credits_incoming = 0; 1101 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); 1102 break; 1103 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1104 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1105 // TODO: support larger MPS 1106 channel->state = L2CAP_STATE_OPEN; 1107 channel->credits_incoming = channel->new_credits_incoming; 1108 channel->new_credits_incoming = 0; 1109 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); 1110 // notify client 1111 l2cap_emit_le_channel_opened(channel, 0); 1112 break; 1113 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1114 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1115 channel->state = L2CAP_STATE_INVALID; 1116 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1117 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1118 l2cap_stop_rtx(channel); 1119 btstack_linked_list_iterator_remove(&it); 1120 btstack_memory_l2cap_channel_free(channel); 1121 break; 1122 case L2CAP_STATE_OPEN: 1123 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1124 1125 // send credits 1126 if (channel->new_credits_incoming){ 1127 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1128 channel->local_sig_id = l2cap_next_sig_id(); 1129 uint16_t new_credits = channel->new_credits_incoming; 1130 channel->new_credits_incoming = 0; 1131 channel->credits_incoming += new_credits; 1132 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1133 break; 1134 } 1135 1136 // send data 1137 if (!channel->send_sdu_buffer) break; 1138 if (!channel->credits_outgoing) break; 1139 1140 // send part of SDU 1141 hci_reserve_packet_buffer(); 1142 acl_buffer = hci_get_outgoing_packet_buffer(); 1143 l2cap_payload = acl_buffer + 8; 1144 pos = 0; 1145 if (!channel->send_sdu_pos){ 1146 // store SDU len 1147 channel->send_sdu_pos += 2; 1148 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 1149 pos += 2; 1150 } 1151 payload_size = btstack_min(channel->send_sdu_len + 2 - channel->send_sdu_pos, channel->remote_mps - pos); 1152 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 1153 memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len 1154 pos += payload_size; 1155 channel->send_sdu_pos += payload_size; 1156 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 1157 // done 1158 1159 channel->credits_outgoing--; 1160 1161 if (channel->send_sdu_pos >= channel->send_sdu_len + 2){ 1162 channel->send_sdu_buffer = NULL; 1163 // send done event 1164 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 1165 // inform about can send now 1166 l2cap_le_notify_channel_can_send(channel); 1167 } 1168 hci_send_acl_packet_buffer(8 + pos); 1169 break; 1170 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1171 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1172 channel->local_sig_id = l2cap_next_sig_id(); 1173 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1174 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1175 break; 1176 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1177 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1178 channel->state = L2CAP_STATE_INVALID; 1179 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1180 l2cap_le_finialize_channel_close(channel); // -- remove from list 1181 break; 1182 default: 1183 break; 1184 } 1185 } 1186 #endif 1187 1188 #ifdef ENABLE_BLE 1189 // send l2cap con paramter update if necessary 1190 hci_connections_get_iterator(&it); 1191 while(btstack_linked_list_iterator_has_next(&it)){ 1192 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1193 if (connection->address_type != BD_ADDR_TYPE_LE_PUBLIC && connection->address_type != BD_ADDR_TYPE_LE_RANDOM) continue; 1194 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1195 switch (connection->le_con_parameter_update_state){ 1196 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1197 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1198 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, connection->le_con_param_update_identifier, 1199 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1200 break; 1201 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1202 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1203 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1204 break; 1205 case CON_PARAMETER_UPDATE_DENY: 1206 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1207 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1208 break; 1209 default: 1210 break; 1211 } 1212 } 1213 #endif 1214 1215 // log_info("l2cap_run: exit"); 1216 } 1217 1218 #ifdef ENABLE_CLASSIC 1219 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1220 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 1221 log_info("l2cap_handle_connection_complete expected state"); 1222 // success, start l2cap handshake 1223 channel->con_handle = con_handle; 1224 // check remote SSP feature first 1225 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1226 } 1227 } 1228 1229 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1230 1231 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1232 // assumption: outgoing connection 1233 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1234 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1235 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1236 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1237 return; 1238 } 1239 #endif 1240 1241 // fine, go ahead 1242 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1243 } 1244 1245 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1246 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1247 1248 // we have been waiting for remote supported features, if both support SSP, 1249 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)); 1250 if (gap_ssp_supported_on_both_sides(channel->con_handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){ 1251 // request security level 2 1252 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 1253 gap_request_security_level(channel->con_handle, LEVEL_2); 1254 return; 1255 } 1256 1257 l2cap_ready_to_connect(channel); 1258 } 1259 #endif 1260 1261 #ifdef L2CAP_USES_CHANNELS 1262 static l2cap_channel_t * l2cap_create_channel_entry(btstack_packet_handler_t packet_handler, bd_addr_t address, bd_addr_type_t address_type, 1263 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 1264 1265 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 1266 if (!channel) { 1267 return NULL; 1268 } 1269 1270 // Init memory (make valgrind happy) 1271 memset(channel, 0, sizeof(l2cap_channel_t)); 1272 1273 // fill in 1274 channel->packet_handler = packet_handler; 1275 bd_addr_copy(channel->address, address); 1276 channel->address_type = address_type; 1277 channel->psm = psm; 1278 channel->local_mtu = local_mtu; 1279 channel->remote_mtu = L2CAP_MINIMAL_MTU; 1280 channel->required_security_level = security_level; 1281 1282 // 1283 channel->local_cid = l2cap_next_local_cid(); 1284 channel->con_handle = 0; 1285 1286 // set initial state 1287 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 1288 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 1289 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 1290 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 1291 1292 return channel; 1293 } 1294 #endif 1295 1296 #ifdef ENABLE_CLASSIC 1297 1298 /** 1299 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 1300 * @param packet_handler 1301 * @param address 1302 * @param psm 1303 * @param mtu 1304 * @param local_cid 1305 */ 1306 1307 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){ 1308 // limit MTU to the size of our outtgoing HCI buffer 1309 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 1310 1311 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x mtu %u -> local mtu %u", bd_addr_to_str(address), psm, mtu, local_mtu); 1312 1313 l2cap_channel_t * channel = l2cap_create_channel_entry(channel_packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1314 if (!channel) { 1315 return BTSTACK_MEMORY_ALLOC_FAILED; 1316 } 1317 1318 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1319 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1320 #endif 1321 1322 // add to connections list 1323 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1324 1325 // store local_cid 1326 if (out_local_cid){ 1327 *out_local_cid = channel->local_cid; 1328 } 1329 1330 // check if hci connection is already usable 1331 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1332 if (conn){ 1333 log_info("l2cap_create_channel, hci connection already exists"); 1334 l2cap_handle_connection_complete(conn->con_handle, channel); 1335 // check if remote supported fearures are already received 1336 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1337 l2cap_handle_remote_supported_features_received(channel); 1338 } 1339 } 1340 1341 l2cap_run(); 1342 1343 return 0; 1344 } 1345 1346 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1347 1348 static uint8_t l2cap_ertm_validate_local_config(uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, 1349 uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){ 1350 1351 UNUSED(buffer); 1352 UNUSED(size); 1353 1354 uint8_t result = ERROR_CODE_SUCCESS; 1355 if (max_transmit < 1){ 1356 log_error("max_transmit must be >= 1"); 1357 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1358 } 1359 if (retransmission_timeout_ms < 2000){ 1360 log_error("retransmission_timeout_ms must be >= 2000 ms"); 1361 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1362 } 1363 if (monitor_timeout_ms < 12000){ 1364 log_error("monitor_timeout_ms must be >= 12000 ms"); 1365 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1366 } 1367 if (local_mtu < 48){ 1368 log_error("local_mtu must be >= 48"); 1369 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1370 } 1371 if (num_rx_buffers < 1){ 1372 log_error("num_rx_buffers must be >= 1"); 1373 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1374 } 1375 if (num_tx_buffers < 1){ 1376 log_error("num_rx_buffers must be >= 1"); 1377 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1378 } 1379 return result; 1380 } 1381 1382 static void l2cap_ertm_configure_channel(l2cap_channel_t * channel, int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, 1383 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){ 1384 1385 channel->mode = L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION; 1386 channel->ertm_mandatory = ertm_mandatory; 1387 channel->local_max_transmit = max_transmit; 1388 channel->local_retransmission_timeout_ms = retransmission_timeout_ms; 1389 channel->local_monitor_timeout_ms = monitor_timeout_ms; 1390 channel->local_mtu = local_mtu; 1391 channel->num_rx_buffers = num_rx_buffers; 1392 channel->num_tx_buffers = num_tx_buffers; 1393 1394 // align buffer to 16-byte boundary, just in case 1395 int bytes_till_alignment = 16 - (((uintptr_t) buffer) & 0x0f); 1396 buffer += bytes_till_alignment; 1397 size -= bytes_till_alignment; 1398 1399 // setup state buffers 1400 uint32_t pos = 0; 1401 channel->rx_packets_state = (l2cap_ertm_rx_packet_state_t *) &buffer[pos]; 1402 pos += num_rx_buffers * sizeof(l2cap_ertm_rx_packet_state_t); 1403 channel->tx_packets_state = (l2cap_ertm_tx_packet_state_t *) &buffer[pos]; 1404 pos += num_tx_buffers * sizeof(l2cap_ertm_tx_packet_state_t); 1405 1406 // setup reassembly buffer 1407 channel->reassembly_buffer = &buffer[pos]; 1408 pos += local_mtu; 1409 1410 // divide rest of data equally 1411 channel->local_mps = (size - pos) / (num_rx_buffers + num_tx_buffers); 1412 log_info("Local MPS: %u", channel->local_mtu); 1413 channel->rx_packets_data = &buffer[pos]; 1414 pos += num_rx_buffers * channel->local_mtu; 1415 channel->tx_packets_data = &buffer[pos]; 1416 } 1417 1418 uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, 1419 int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, 1420 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){ 1421 1422 log_info("L2CAP_CREATE_CHANNEL addr %s, psm 0x%x, local mtu %u", bd_addr_to_str(address), psm, local_mtu); 1423 1424 // validate local config 1425 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); 1426 if (result) return result; 1427 1428 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1429 if (!channel) { 1430 return BTSTACK_MEMORY_ALLOC_FAILED; 1431 } 1432 1433 // configure ERTM 1434 l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, 1435 local_mtu, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1436 1437 // add to connections list 1438 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1439 1440 // store local_cid 1441 if (out_local_cid){ 1442 *out_local_cid = channel->local_cid; 1443 } 1444 1445 // check if hci connection is already usable 1446 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1447 if (conn){ 1448 log_info("l2cap_create_channel, hci connection already exists"); 1449 l2cap_handle_connection_complete(conn->con_handle, channel); 1450 // check if remote supported fearures are already received 1451 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1452 l2cap_handle_remote_supported_features_received(channel); 1453 } 1454 } 1455 1456 l2cap_run(); 1457 1458 return 0; 1459 } 1460 #endif 1461 1462 void 1463 l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 1464 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 1465 // find channel for local_cid 1466 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1467 if (channel) { 1468 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1469 } 1470 // process 1471 l2cap_run(); 1472 } 1473 1474 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 1475 btstack_linked_list_iterator_t it; 1476 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1477 while (btstack_linked_list_iterator_has_next(&it)){ 1478 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1479 if ( bd_addr_cmp( channel->address, address) != 0) continue; 1480 // channel for this address found 1481 switch (channel->state){ 1482 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 1483 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1484 // failure, forward error code 1485 l2cap_emit_channel_opened(channel, status); 1486 // discard channel 1487 l2cap_stop_rtx(channel); 1488 btstack_linked_list_iterator_remove(&it); 1489 btstack_memory_l2cap_channel_free(channel); 1490 break; 1491 default: 1492 break; 1493 } 1494 } 1495 } 1496 1497 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 1498 btstack_linked_list_iterator_t it; 1499 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1500 while (btstack_linked_list_iterator_has_next(&it)){ 1501 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1502 if ( ! bd_addr_cmp( channel->address, address) ){ 1503 l2cap_handle_connection_complete(handle, channel); 1504 } 1505 } 1506 // process 1507 l2cap_run(); 1508 } 1509 #endif 1510 1511 static void l2cap_notify_channel_can_send(void){ 1512 1513 #ifdef ENABLE_CLASSIC 1514 btstack_linked_list_iterator_t it; 1515 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1516 while (btstack_linked_list_iterator_has_next(&it)){ 1517 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1518 if (!channel->waiting_for_can_send_now) continue; 1519 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1520 channel->waiting_for_can_send_now = 0; 1521 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 1522 } 1523 #endif 1524 1525 int i; 1526 for (i=0;i<L2CAP_FIXED_CHANNEL_TABLE_SIZE;i++){ 1527 if (!fixed_channels[i].callback) continue; 1528 if (!fixed_channels[i].waiting_for_can_send_now) continue; 1529 int can_send = 0; 1530 if (l2cap_fixed_channel_table_index_is_le(i)){ 1531 #ifdef ENABLE_BLE 1532 can_send = hci_can_send_acl_le_packet_now(); 1533 #endif 1534 } else { 1535 #ifdef ENABLE_CLASSIC 1536 can_send = hci_can_send_acl_classic_packet_now(); 1537 #endif 1538 } 1539 if (!can_send) continue; 1540 fixed_channels[i].waiting_for_can_send_now = 0; 1541 l2cap_emit_can_send_now(fixed_channels[i].callback, l2cap_fixed_channel_table_channel_id_for_index(i)); 1542 } 1543 } 1544 1545 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 1546 1547 UNUSED(packet_type); 1548 UNUSED(cid); 1549 UNUSED(size); 1550 1551 bd_addr_t address; 1552 hci_con_handle_t handle; 1553 int hci_con_used; 1554 btstack_linked_list_iterator_t it; 1555 1556 // avoid unused warnings 1557 UNUSED(address); 1558 UNUSED(hci_con_used); 1559 UNUSED(it); 1560 UNUSED(handle); 1561 1562 switch(hci_event_packet_get_type(packet)){ 1563 1564 // Notify channel packet handler if they can send now 1565 case HCI_EVENT_TRANSPORT_PACKET_SENT: 1566 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 1567 l2cap_run(); // try sending signaling packets first 1568 l2cap_notify_channel_can_send(); 1569 break; 1570 1571 case HCI_EVENT_COMMAND_STATUS: 1572 l2cap_run(); // try sending signaling packets first 1573 break; 1574 1575 #ifdef ENABLE_CLASSIC 1576 // handle connection complete events 1577 case HCI_EVENT_CONNECTION_COMPLETE: 1578 reverse_bd_addr(&packet[5], address); 1579 if (packet[2] == 0){ 1580 handle = little_endian_read_16(packet, 3); 1581 l2cap_handle_connection_success_for_addr(address, handle); 1582 } else { 1583 l2cap_handle_connection_failed_for_addr(address, packet[2]); 1584 } 1585 break; 1586 1587 // handle successful create connection cancel command 1588 case HCI_EVENT_COMMAND_COMPLETE: 1589 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 1590 if (packet[5] == 0){ 1591 reverse_bd_addr(&packet[6], address); 1592 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 1593 l2cap_handle_connection_failed_for_addr(address, 0x16); 1594 } 1595 } 1596 l2cap_run(); // try sending signaling packets first 1597 break; 1598 #endif 1599 1600 // handle disconnection complete events 1601 case HCI_EVENT_DISCONNECTION_COMPLETE: 1602 // send l2cap disconnect events for all channels on this handle and free them 1603 #ifdef ENABLE_CLASSIC 1604 handle = little_endian_read_16(packet, 3); 1605 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1606 while (btstack_linked_list_iterator_has_next(&it)){ 1607 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1608 if (channel->con_handle != handle) continue; 1609 l2cap_emit_channel_closed(channel); 1610 l2cap_stop_rtx(channel); 1611 btstack_linked_list_iterator_remove(&it); 1612 btstack_memory_l2cap_channel_free(channel); 1613 } 1614 #endif 1615 #ifdef ENABLE_LE_DATA_CHANNELS 1616 handle = little_endian_read_16(packet, 3); 1617 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1618 while (btstack_linked_list_iterator_has_next(&it)){ 1619 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1620 if (channel->con_handle != handle) continue; 1621 l2cap_emit_channel_closed(channel); 1622 btstack_linked_list_iterator_remove(&it); 1623 btstack_memory_l2cap_channel_free(channel); 1624 } 1625 #endif 1626 break; 1627 1628 // HCI Connection Timeouts 1629 #ifdef ENABLE_CLASSIC 1630 case L2CAP_EVENT_TIMEOUT_CHECK: 1631 handle = little_endian_read_16(packet, 2); 1632 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 1633 if (hci_authentication_active_for_handle(handle)) break; 1634 hci_con_used = 0; 1635 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1636 while (btstack_linked_list_iterator_has_next(&it)){ 1637 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1638 if (channel->con_handle != handle) continue; 1639 hci_con_used = 1; 1640 break; 1641 } 1642 if (hci_con_used) break; 1643 if (!hci_can_send_command_packet_now()) break; 1644 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 1645 break; 1646 1647 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 1648 handle = little_endian_read_16(packet, 3); 1649 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1650 while (btstack_linked_list_iterator_has_next(&it)){ 1651 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1652 if (channel->con_handle != handle) continue; 1653 l2cap_handle_remote_supported_features_received(channel); 1654 break; 1655 } 1656 break; 1657 1658 case GAP_EVENT_SECURITY_LEVEL: 1659 handle = little_endian_read_16(packet, 2); 1660 log_info("l2cap - security level update"); 1661 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1662 while (btstack_linked_list_iterator_has_next(&it)){ 1663 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1664 if (channel->con_handle != handle) continue; 1665 1666 log_info("l2cap - state %u", channel->state); 1667 1668 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 1669 gap_security_level_t required_level = channel->required_security_level; 1670 1671 switch (channel->state){ 1672 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1673 if (actual_level >= required_level){ 1674 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1675 // we need to know if ERTM is supported before sending a config response 1676 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1677 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1678 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 1679 #else 1680 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 1681 l2cap_emit_incoming_connection(channel); 1682 #endif 1683 } else { 1684 channel->reason = 0x0003; // security block 1685 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1686 } 1687 break; 1688 1689 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 1690 if (actual_level >= required_level){ 1691 l2cap_ready_to_connect(channel); 1692 } else { 1693 // disconnnect, authentication not good enough 1694 hci_disconnect_security_block(handle); 1695 } 1696 break; 1697 1698 default: 1699 break; 1700 } 1701 } 1702 break; 1703 #endif 1704 1705 default: 1706 break; 1707 } 1708 1709 l2cap_run(); 1710 } 1711 1712 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 1713 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 1714 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 1715 signaling_responses[signaling_responses_pending].handle = handle; 1716 signaling_responses[signaling_responses_pending].code = code; 1717 signaling_responses[signaling_responses_pending].sig_id = sig_id; 1718 signaling_responses[signaling_responses_pending].cid = cid; 1719 signaling_responses[signaling_responses_pending].data = data; 1720 signaling_responses_pending++; 1721 l2cap_run(); 1722 } 1723 } 1724 1725 #ifdef ENABLE_CLASSIC 1726 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 1727 channel->remote_sig_id = identifier; 1728 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 1729 l2cap_run(); 1730 } 1731 1732 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 1733 1734 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 1735 l2cap_service_t *service = l2cap_get_service(psm); 1736 if (!service) { 1737 // 0x0002 PSM not supported 1738 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 1739 return; 1740 } 1741 1742 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 1743 if (!hci_connection) { 1744 // 1745 log_error("no hci_connection for handle %u", handle); 1746 return; 1747 } 1748 1749 // alloc structure 1750 // log_info("l2cap_handle_connection_request register channel"); 1751 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, hci_connection->address, BD_ADDR_TYPE_CLASSIC, 1752 psm, service->mtu, service->required_security_level); 1753 if (!channel){ 1754 // 0x0004 No resources available 1755 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 1756 return; 1757 } 1758 1759 channel->con_handle = handle; 1760 channel->remote_cid = source_cid; 1761 channel->remote_sig_id = sig_id; 1762 1763 // limit local mtu to max acl packet length - l2cap header 1764 if (channel->local_mtu > l2cap_max_mtu()) { 1765 channel->local_mtu = l2cap_max_mtu(); 1766 } 1767 1768 // set initial state 1769 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 1770 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 1771 1772 // add to connections list 1773 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1774 1775 // assert security requirements 1776 gap_request_security_level(handle, channel->required_security_level); 1777 } 1778 1779 void l2cap_accept_connection(uint16_t local_cid){ 1780 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 1781 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1782 if (!channel) { 1783 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1784 return; 1785 } 1786 1787 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1788 // configure L2CAP Basic mode 1789 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1790 #endif 1791 1792 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1793 1794 // process 1795 l2cap_run(); 1796 } 1797 1798 1799 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1800 uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, 1801 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){ 1802 1803 log_info("L2CAP_ACCEPT_ERTM_CONNECTION local_cid 0x%x", local_cid); 1804 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1805 if (!channel) { 1806 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1807 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1808 } 1809 1810 // validate local config 1811 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); 1812 if (result) return result; 1813 1814 // configure L2CAP ERTM 1815 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); 1816 1817 // continue 1818 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1819 1820 // process 1821 l2cap_run(); 1822 1823 return ERROR_CODE_SUCCESS; 1824 } 1825 1826 uint8_t l2cap_ertm_set_busy(uint16_t local_cid){ 1827 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1828 if (!channel) { 1829 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1830 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1831 } 1832 if (!channel->local_busy){ 1833 channel->local_busy = 1; 1834 channel->send_supervisor_frame_receiver_not_ready = 1; 1835 l2cap_run(); 1836 } 1837 return ERROR_CODE_SUCCESS; 1838 } 1839 1840 uint8_t l2cap_ertm_set_ready(uint16_t local_cid){ 1841 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1842 if (!channel) { 1843 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1844 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1845 } 1846 if (channel->local_busy){ 1847 channel->local_busy = 0; 1848 channel->send_supervisor_frame_receiver_ready_poll = 1; 1849 l2cap_run(); 1850 } 1851 return ERROR_CODE_SUCCESS; 1852 } 1853 1854 static void l2cap_ertm_handle_req_seq(l2cap_channel_t * l2cap_channel, uint8_t req_seq){ 1855 l2cap_ertm_tx_packet_state_t * tx_state; 1856 while (1){ 1857 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 1858 // calc delta 1859 int delta = (req_seq - tx_state->tx_seq) & 0x03f; 1860 if (delta == 0) break; // all packets acknowledged 1861 if (delta > l2cap_channel->remote_tx_window_size) break; 1862 1863 log_info("RR seq %u => packet with tx_seq %u done", req_seq, tx_state->tx_seq); 1864 1865 // stop retransmission timer 1866 btstack_run_loop_remove_timer(&tx_state->retransmission_timer); 1867 l2cap_channel->tx_read_index++; 1868 if (l2cap_channel->tx_read_index >= l2cap_channel->num_rx_buffers){ 1869 l2cap_channel->tx_read_index = 0; 1870 } 1871 1872 // no unack packages 1873 if (l2cap_channel->tx_read_index == l2cap_channel->tx_write_index) break; 1874 } 1875 } 1876 1877 static l2cap_ertm_tx_packet_state_t * l2cap_ertm_get_tx_state(l2cap_channel_t * l2cap_channel, uint8_t tx_seq){ 1878 int i; 1879 for (i=0;i<l2cap_channel->num_tx_buffers;i++){ 1880 l2cap_ertm_tx_packet_state_t * tx_state = &l2cap_channel->tx_packets_state[i]; 1881 if (tx_state->tx_seq == tx_seq) return tx_state; 1882 } 1883 return NULL; 1884 } 1885 #endif 1886 1887 1888 void l2cap_decline_connection(uint16_t local_cid){ 1889 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 1890 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1891 if (!channel) { 1892 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1893 return; 1894 } 1895 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1896 channel->reason = 0x04; // no resources available 1897 l2cap_run(); 1898 } 1899 1900 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 1901 1902 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1903 1904 uint16_t flags = little_endian_read_16(command, 6); 1905 if (flags & 1) { 1906 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1907 } 1908 1909 // accept the other's configuration options 1910 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 1911 uint16_t pos = 8; 1912 while (pos < end_pos){ 1913 uint8_t option_hint = command[pos] >> 7; 1914 uint8_t option_type = command[pos] & 0x7f; 1915 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 1916 pos++; 1917 uint8_t length = command[pos++]; 1918 // MTU { type(8): 1, len(8):2, MTU(16) } 1919 if (option_type == 1 && length == 2){ 1920 channel->remote_mtu = little_endian_read_16(command, pos); 1921 log_info("Remote MTU %u", channel->remote_mtu); 1922 if (channel->remote_mtu > l2cap_max_mtu()){ 1923 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 1924 channel->remote_mtu = l2cap_max_mtu(); 1925 } 1926 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1927 } 1928 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 1929 if (option_type == 2 && length == 2){ 1930 channel->flush_timeout = little_endian_read_16(command, pos); 1931 log_info("Flush timeout: %u ms", channel->flush_timeout); 1932 } 1933 1934 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1935 // Retransmission and Flow Control Option 1936 if (option_type == 4 && length == 9){ 1937 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 1938 switch(channel->mode){ 1939 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 1940 // Store remote config 1941 channel->remote_tx_window_size = command[pos+1]; 1942 channel->remote_max_transmit = command[pos+2]; 1943 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 1944 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 1945 channel->remote_mps = little_endian_read_16(command, pos + 7); 1946 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 1947 channel->remote_tx_window_size, 1948 channel->remote_max_transmit, 1949 channel->remote_retransmission_timeout_ms, 1950 channel->remote_monitor_timeout_ms, 1951 channel->remote_mps); 1952 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 1953 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1954 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1955 } else { 1956 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1957 } 1958 break; 1959 case L2CAP_CHANNEL_MODE_BASIC: 1960 switch (mode){ 1961 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 1962 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 1963 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 1964 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1965 } 1966 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 1967 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1968 break; 1969 default: // case L2CAP_CHANNEL_MODE_BASIC: 1970 // TODO store and evaluate configuration 1971 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1972 break; 1973 } 1974 break; 1975 default: 1976 break; 1977 } 1978 } 1979 #endif 1980 // check for unknown options 1981 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 1982 log_info("l2cap cid %u, unknown options", channel->local_cid); 1983 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 1984 } 1985 pos += length; 1986 } 1987 } 1988 1989 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 1990 log_info("l2cap_signaling_handle_configure_response"); 1991 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1992 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 1993 uint16_t pos = 10; 1994 while (pos < end_pos){ 1995 uint8_t option_hint = command[pos] >> 7; 1996 uint8_t option_type = command[pos] & 0x7f; 1997 log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 1998 pos++; 1999 uint8_t length = command[pos++]; 2000 2001 // Retransmission and Flow Control Option 2002 if (option_type == 4 && length == 9){ 2003 switch (channel->mode){ 2004 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2005 if (channel->ertm_mandatory){ 2006 // ?? 2007 } else { 2008 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2009 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2010 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2011 } 2012 } 2013 break; 2014 case L2CAP_CHANNEL_MODE_BASIC: 2015 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2016 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2017 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2018 } 2019 break; 2020 default: 2021 break; 2022 } 2023 } 2024 2025 // check for unknown options 2026 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 2027 log_info("l2cap cid %u, unknown options", channel->local_cid); 2028 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2029 } 2030 2031 pos += length; 2032 } 2033 #endif 2034 } 2035 2036 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2037 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2038 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2039 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2040 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2041 if (channel->state == L2CAP_STATE_OPEN) return 0; 2042 return 1; 2043 } 2044 2045 2046 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2047 2048 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2049 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2050 uint16_t result = 0; 2051 2052 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2053 2054 // handle DISCONNECT REQUESTS seperately 2055 if (code == DISCONNECTION_REQUEST){ 2056 switch (channel->state){ 2057 case L2CAP_STATE_CONFIG: 2058 case L2CAP_STATE_OPEN: 2059 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2060 case L2CAP_STATE_WAIT_DISCONNECT: 2061 l2cap_handle_disconnect_request(channel, identifier); 2062 break; 2063 2064 default: 2065 // ignore in other states 2066 break; 2067 } 2068 return; 2069 } 2070 2071 // @STATEMACHINE(l2cap) 2072 switch (channel->state) { 2073 2074 case L2CAP_STATE_WAIT_CONNECT_RSP: 2075 switch (code){ 2076 case CONNECTION_RESPONSE: 2077 l2cap_stop_rtx(channel); 2078 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2079 switch (result) { 2080 case 0: 2081 // successful connection 2082 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2083 channel->state = L2CAP_STATE_CONFIG; 2084 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2085 break; 2086 case 1: 2087 // connection pending. get some coffee, but start the ERTX 2088 l2cap_start_ertx(channel); 2089 break; 2090 default: 2091 // channel closed 2092 channel->state = L2CAP_STATE_CLOSED; 2093 // map l2cap connection response result to BTstack status enumeration 2094 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2095 2096 // drop link key if security block 2097 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2098 gap_drop_link_key_for_bd_addr(channel->address); 2099 } 2100 2101 // discard channel 2102 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2103 btstack_memory_l2cap_channel_free(channel); 2104 break; 2105 } 2106 break; 2107 2108 default: 2109 //@TODO: implement other signaling packets 2110 break; 2111 } 2112 break; 2113 2114 case L2CAP_STATE_CONFIG: 2115 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2116 switch (code) { 2117 case CONFIGURE_REQUEST: 2118 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 2119 l2cap_signaling_handle_configure_request(channel, command); 2120 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 2121 // only done if continuation not set 2122 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 2123 } 2124 break; 2125 case CONFIGURE_RESPONSE: 2126 l2cap_stop_rtx(channel); 2127 l2cap_signaling_handle_configure_response(channel, result, command); 2128 switch (result){ 2129 case 0: // success 2130 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 2131 break; 2132 case 4: // pending 2133 l2cap_start_ertx(channel); 2134 break; 2135 default: 2136 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2137 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 2138 // remote does not offer ertm but it's required 2139 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2140 break; 2141 } 2142 #endif 2143 // retry on negative result 2144 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2145 break; 2146 } 2147 break; 2148 default: 2149 break; 2150 } 2151 if (l2cap_channel_ready_for_open(channel)){ 2152 // for open: 2153 channel->state = L2CAP_STATE_OPEN; 2154 l2cap_emit_channel_opened(channel, 0); 2155 } 2156 break; 2157 2158 case L2CAP_STATE_WAIT_DISCONNECT: 2159 switch (code) { 2160 case DISCONNECTION_RESPONSE: 2161 l2cap_finialize_channel_close(channel); 2162 break; 2163 default: 2164 //@TODO: implement other signaling packets 2165 break; 2166 } 2167 break; 2168 2169 case L2CAP_STATE_CLOSED: 2170 // @TODO handle incoming requests 2171 break; 2172 2173 case L2CAP_STATE_OPEN: 2174 //@TODO: implement other signaling packets, e.g. re-configure 2175 break; 2176 default: 2177 break; 2178 } 2179 // log_info("new state %u", channel->state); 2180 } 2181 2182 2183 static void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){ 2184 2185 btstack_linked_list_iterator_t it; 2186 2187 // get code, signalind identifier and command len 2188 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2189 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2190 2191 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 2192 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_RESPONSE){ 2193 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2194 return; 2195 } 2196 2197 // general commands without an assigned channel 2198 switch(code) { 2199 2200 case CONNECTION_REQUEST: { 2201 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2202 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2203 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 2204 return; 2205 } 2206 2207 case ECHO_REQUEST: 2208 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 2209 return; 2210 2211 case INFORMATION_REQUEST: { 2212 uint16_t infoType = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2213 l2cap_register_signaling_response(handle, code, sig_id, 0, infoType); 2214 return; 2215 } 2216 2217 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2218 case INFORMATION_RESPONSE: { 2219 hci_connection_t * connection = hci_connection_for_handle(handle); 2220 if (!connection) return; 2221 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2222 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2223 if (result != 0) return; 2224 if (info_type != 0x02) return; 2225 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2226 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2227 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2228 // trigger connection request 2229 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2230 while (btstack_linked_list_iterator_has_next(&it)){ 2231 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2232 if (channel->con_handle != handle) continue; 2233 // bail if ERTM was requested but is not supported 2234 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2235 if (channel->ertm_mandatory){ 2236 // channel closed 2237 channel->state = L2CAP_STATE_CLOSED; 2238 // map l2cap connection response result to BTstack status enumeration 2239 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTD); 2240 // discard channel 2241 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2242 btstack_memory_l2cap_channel_free(channel); 2243 continue; 2244 } else { 2245 // fallback to Basic mode 2246 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2247 } 2248 } 2249 // start connecting 2250 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2251 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 2252 } 2253 // respond to connection request 2254 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2255 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2256 l2cap_emit_incoming_connection(channel); 2257 } 2258 } 2259 return; 2260 } 2261 #endif 2262 2263 default: 2264 break; 2265 } 2266 2267 2268 // Get potential destination CID 2269 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2270 2271 // Find channel for this sig_id and connection handle 2272 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2273 while (btstack_linked_list_iterator_has_next(&it)){ 2274 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2275 if (channel->con_handle != handle) continue; 2276 if (code & 1) { 2277 // match odd commands (responses) by previous signaling identifier 2278 if (channel->local_sig_id == sig_id) { 2279 l2cap_signaling_handler_channel(channel, command); 2280 break; 2281 } 2282 } else { 2283 // match even commands (requests) by local channel id 2284 if (channel->local_cid == dest_cid) { 2285 l2cap_signaling_handler_channel(channel, command); 2286 break; 2287 } 2288 } 2289 } 2290 } 2291 #endif 2292 2293 #ifdef ENABLE_BLE 2294 2295 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 2296 uint8_t event[6]; 2297 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 2298 event[1] = 4; 2299 little_endian_store_16(event, 2, con_handle); 2300 little_endian_store_16(event, 4, result); 2301 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2302 if (!l2cap_event_packet_handler) return; 2303 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 2304 } 2305 2306 // @returns valid 2307 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 2308 hci_connection_t * connection; 2309 uint16_t result; 2310 uint8_t event[10]; 2311 2312 #ifdef ENABLE_LE_DATA_CHANNELS 2313 btstack_linked_list_iterator_t it; 2314 l2cap_channel_t * channel; 2315 uint16_t local_cid; 2316 uint16_t le_psm; 2317 uint16_t new_credits; 2318 uint16_t credits_before; 2319 l2cap_service_t * service; 2320 uint16_t source_cid; 2321 #endif 2322 2323 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2324 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u", code, sig_id); 2325 2326 switch (code){ 2327 2328 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 2329 result = little_endian_read_16(command, 4); 2330 l2cap_emit_connection_parameter_update_response(handle, result); 2331 break; 2332 2333 case CONNECTION_PARAMETER_UPDATE_REQUEST: 2334 connection = hci_connection_for_handle(handle); 2335 if (connection){ 2336 if (connection->role != HCI_ROLE_MASTER){ 2337 // reject command without notifying upper layer when not in master role 2338 return 0; 2339 } 2340 int update_parameter = 1; 2341 le_connection_parameter_range_t existing_range; 2342 gap_get_connection_parameter_range(&existing_range); 2343 uint16_t le_conn_interval_min = little_endian_read_16(command,8); 2344 uint16_t le_conn_interval_max = little_endian_read_16(command,10); 2345 uint16_t le_conn_latency = little_endian_read_16(command,12); 2346 uint16_t le_supervision_timeout = little_endian_read_16(command,14); 2347 2348 if (le_conn_interval_min < existing_range.le_conn_interval_min) update_parameter = 0; 2349 if (le_conn_interval_max > existing_range.le_conn_interval_max) update_parameter = 0; 2350 2351 if (le_conn_latency < existing_range.le_conn_latency_min) update_parameter = 0; 2352 if (le_conn_latency > existing_range.le_conn_latency_max) update_parameter = 0; 2353 2354 if (le_supervision_timeout < existing_range.le_supervision_timeout_min) update_parameter = 0; 2355 if (le_supervision_timeout > existing_range.le_supervision_timeout_max) update_parameter = 0; 2356 2357 if (update_parameter){ 2358 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 2359 connection->le_conn_interval_min = le_conn_interval_min; 2360 connection->le_conn_interval_max = le_conn_interval_max; 2361 connection->le_conn_latency = le_conn_latency; 2362 connection->le_supervision_timeout = le_supervision_timeout; 2363 } else { 2364 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 2365 } 2366 connection->le_con_param_update_identifier = sig_id; 2367 } 2368 2369 if (!l2cap_event_packet_handler) break; 2370 2371 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 2372 event[1] = 8; 2373 memcpy(&event[2], &command[4], 8); 2374 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2375 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2376 break; 2377 2378 #ifdef ENABLE_LE_DATA_CHANNELS 2379 2380 case COMMAND_REJECT: 2381 // Find channel for this sig_id and connection handle 2382 channel = NULL; 2383 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2384 while (btstack_linked_list_iterator_has_next(&it)){ 2385 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2386 if (a_channel->con_handle != handle) continue; 2387 if (a_channel->local_sig_id != sig_id) continue; 2388 channel = a_channel; 2389 break; 2390 } 2391 if (!channel) break; 2392 2393 // if received while waiting for le connection response, assume legacy device 2394 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 2395 channel->state = L2CAP_STATE_CLOSED; 2396 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 2397 l2cap_emit_le_channel_opened(channel, 0x0002); 2398 2399 // discard channel 2400 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2401 btstack_memory_l2cap_channel_free(channel); 2402 break; 2403 } 2404 break; 2405 2406 case LE_CREDIT_BASED_CONNECTION_REQUEST: 2407 2408 // get hci connection, bail if not found (must not happen) 2409 connection = hci_connection_for_handle(handle); 2410 if (!connection) return 0; 2411 2412 // check if service registered 2413 le_psm = little_endian_read_16(command, 4); 2414 service = l2cap_le_get_service(le_psm); 2415 source_cid = little_endian_read_16(command, 6); 2416 2417 if (service){ 2418 if (source_cid < 0x40){ 2419 // 0x0009 Connection refused - Invalid Source CID 2420 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 2421 return 1; 2422 } 2423 2424 // go through list of channels for this ACL connection and check if we get a match 2425 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2426 while (btstack_linked_list_iterator_has_next(&it)){ 2427 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2428 if (a_channel->con_handle != handle) continue; 2429 if (a_channel->remote_cid != source_cid) continue; 2430 // 0x000a Connection refused - Source CID already allocated 2431 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 2432 return 1; 2433 } 2434 2435 // security: check encryption 2436 if (service->required_security_level >= LEVEL_2){ 2437 if (sm_encryption_key_size(handle) == 0){ 2438 // 0x0008 Connection refused - insufficient encryption 2439 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 2440 return 1; 2441 } 2442 // anything less than 16 byte key size is insufficient 2443 if (sm_encryption_key_size(handle) < 16){ 2444 // 0x0007 Connection refused – insufficient encryption key size 2445 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 2446 return 1; 2447 } 2448 } 2449 2450 // security: check authencation 2451 if (service->required_security_level >= LEVEL_3){ 2452 if (!sm_authenticated(handle)){ 2453 // 0x0005 Connection refused – insufficient authentication 2454 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 2455 return 1; 2456 } 2457 } 2458 2459 // security: check authorization 2460 if (service->required_security_level >= LEVEL_4){ 2461 if (sm_authorization_state(handle) != AUTHORIZATION_GRANTED){ 2462 // 0x0006 Connection refused – insufficient authorization 2463 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 2464 return 1; 2465 } 2466 } 2467 2468 // allocate channel 2469 channel = l2cap_create_channel_entry(service->packet_handler, connection->address, 2470 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 2471 if (!channel){ 2472 // 0x0004 Connection refused – no resources available 2473 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2474 return 1; 2475 } 2476 2477 channel->con_handle = handle; 2478 channel->remote_cid = source_cid; 2479 channel->remote_sig_id = sig_id; 2480 channel->remote_mtu = little_endian_read_16(command, 8); 2481 channel->remote_mps = little_endian_read_16(command, 10); 2482 channel->credits_outgoing = little_endian_read_16(command, 12); 2483 2484 // set initial state 2485 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2486 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 2487 2488 // add to connections list 2489 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2490 2491 // post connection request event 2492 l2cap_emit_le_incoming_connection(channel); 2493 2494 } else { 2495 // Connection refused – LE_PSM not supported 2496 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2497 } 2498 break; 2499 2500 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 2501 // Find channel for this sig_id and connection handle 2502 channel = NULL; 2503 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2504 while (btstack_linked_list_iterator_has_next(&it)){ 2505 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2506 if (a_channel->con_handle != handle) continue; 2507 if (a_channel->local_sig_id != sig_id) continue; 2508 channel = a_channel; 2509 break; 2510 } 2511 if (!channel) break; 2512 2513 // cid + 0 2514 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 2515 if (result){ 2516 channel->state = L2CAP_STATE_CLOSED; 2517 // map l2cap connection response result to BTstack status enumeration 2518 l2cap_emit_le_channel_opened(channel, result); 2519 2520 // discard channel 2521 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2522 btstack_memory_l2cap_channel_free(channel); 2523 break; 2524 } 2525 2526 // success 2527 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2528 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2529 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 2530 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 2531 channel->state = L2CAP_STATE_OPEN; 2532 l2cap_emit_le_channel_opened(channel, result); 2533 break; 2534 2535 case LE_FLOW_CONTROL_CREDIT: 2536 // find channel 2537 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2538 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2539 if (!channel) { 2540 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2541 break; 2542 } 2543 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2544 credits_before = channel->credits_outgoing; 2545 channel->credits_outgoing += new_credits; 2546 // check for credit overrun 2547 if (credits_before > channel->credits_outgoing){ 2548 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 2549 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2550 break; 2551 } 2552 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 2553 break; 2554 2555 case DISCONNECTION_REQUEST: 2556 // find channel 2557 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2558 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2559 if (!channel) { 2560 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2561 break; 2562 } 2563 channel->remote_sig_id = sig_id; 2564 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2565 break; 2566 2567 #endif 2568 2569 case DISCONNECTION_RESPONSE: 2570 break; 2571 2572 default: 2573 // command unknown -> reject command 2574 return 0; 2575 } 2576 return 1; 2577 } 2578 #endif 2579 2580 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size ){ 2581 UNUSED(packet_type); 2582 UNUSED(channel); 2583 2584 l2cap_channel_t * l2cap_channel; 2585 UNUSED(l2cap_channel); 2586 2587 // Get Channel ID 2588 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 2589 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 2590 2591 switch (channel_id) { 2592 2593 #ifdef ENABLE_CLASSIC 2594 case L2CAP_CID_SIGNALING: { 2595 uint16_t command_offset = 8; 2596 while (command_offset < size) { 2597 // handle signaling commands 2598 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 2599 2600 // increment command_offset 2601 command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2602 } 2603 break; 2604 } 2605 #endif 2606 2607 #ifdef ENABLE_BLE 2608 case L2CAP_CID_SIGNALING_LE: { 2609 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 2610 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 2611 if (!valid){ 2612 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2613 } 2614 break; 2615 } 2616 #endif 2617 2618 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 2619 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback) { 2620 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2621 } 2622 break; 2623 2624 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 2625 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback) { 2626 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2627 } 2628 break; 2629 2630 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 2631 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback) { 2632 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2633 } 2634 break; 2635 2636 default: 2637 #ifdef ENABLE_CLASSIC 2638 // Find channel for this channel_id and connection handle 2639 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 2640 if (l2cap_channel) { 2641 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2642 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2643 2644 // verify FCS 2645 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 2646 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 2647 log_info("Packet FCS 0x%04x, calculated FCS 0x%04x", fcs_packet, fcs_calculated); 2648 if (fcs_calculated != fcs_packet){ 2649 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 2650 // TODO: trigger retransmission or something like that 2651 break; 2652 } 2653 2654 // switch on packet type 2655 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2656 uint8_t req_seq = (control >> 8) & 0x3f; 2657 int final = (control >> 7) & 0x01; 2658 if (control & 1){ 2659 // S-Frame 2660 int poll = (control >> 4) & 0x01; 2661 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 2662 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 2663 l2cap_ertm_tx_packet_state_t * tx_state; 2664 switch (s){ 2665 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 2666 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 2667 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2668 if (poll && final){ 2669 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 2670 log_error("P=F=1 in S-Frame"); 2671 break; 2672 } 2673 if (poll){ 2674 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 2675 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 2676 } 2677 if (final){ 2678 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 2679 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 2680 } 2681 break; 2682 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 2683 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 2684 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2685 // rsetart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_handle_req_seq) 2686 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 2687 break; 2688 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 2689 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 2690 break; 2691 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 2692 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 2693 if (poll){ 2694 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2695 } 2696 // find requested i-frame 2697 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 2698 if (tx_state){ 2699 log_info("Retransmission for tx_seq %u requested", req_seq); 2700 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 2701 tx_state->retransmission_requested = 1; 2702 l2cap_channel->srej_active = 1; 2703 } 2704 break; 2705 default: 2706 break; 2707 } 2708 break; 2709 } else { 2710 // I-Frame 2711 // get control 2712 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 2713 uint8_t tx_seq = (control >> 1) & 0x3f; 2714 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 2715 log_info("SAR: pos %u", l2cap_channel->rx_packets_state->pos); 2716 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 2717 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2718 if (final){ 2719 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 2720 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 2721 } 2722 // check ordering 2723 if (l2cap_channel->expected_tx_seq == tx_seq){ 2724 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 2725 l2cap_channel->req_seq = (tx_seq+1) & 0x3f; 2726 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 2727 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 2728 uint16_t sdu_length; 2729 uint16_t segment_length; 2730 uint16_t payload_offset; 2731 switch (sar){ 2732 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 2733 payload_offset = COMPLETE_L2CAP_HEADER+2; 2734 segment_length = payload_offset-2; 2735 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER+2], segment_length); 2736 break; 2737 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 2738 // TODO: use current packet 2739 // TODO: check if reassembly started 2740 // TODO: check len against local mtu 2741 sdu_length = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER+2); 2742 payload_offset = COMPLETE_L2CAP_HEADER+4; 2743 segment_length = size - payload_offset-2; 2744 memcpy(&l2cap_channel->rx_packets_data[0], &packet[payload_offset], segment_length); 2745 l2cap_channel->rx_packets_state->sdu_length = sdu_length; 2746 l2cap_channel->rx_packets_state->pos = segment_length; 2747 break; 2748 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 2749 payload_offset = COMPLETE_L2CAP_HEADER+2; 2750 segment_length = size - payload_offset-2; 2751 memcpy(&l2cap_channel->rx_packets_data[l2cap_channel->rx_packets_state->pos], &packet[payload_offset], segment_length); 2752 l2cap_channel->rx_packets_state->pos += segment_length; 2753 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->rx_packets_data, l2cap_channel->rx_packets_state[0].pos); 2754 break; 2755 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 2756 payload_offset = COMPLETE_L2CAP_HEADER+2; 2757 segment_length = size - payload_offset-2; 2758 memcpy(&l2cap_channel->rx_packets_data[l2cap_channel->rx_packets_state->pos], &packet[payload_offset], segment_length); 2759 l2cap_channel->rx_packets_state->pos += segment_length; 2760 break; 2761 } 2762 } else { 2763 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 2764 if (delta < 2){ 2765 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 2766 l2cap_channel->send_supervisor_frame_selective_reject = 1; 2767 } else { 2768 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 2769 l2cap_channel->send_supervisor_frame_reject = 1; 2770 } 2771 } 2772 } 2773 break; 2774 } 2775 #endif 2776 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2777 } 2778 #endif 2779 #ifdef ENABLE_LE_DATA_CHANNELS 2780 l2cap_channel = l2cap_le_get_channel_for_local_cid(channel_id); 2781 if (l2cap_channel) { 2782 // credit counting 2783 if (l2cap_channel->credits_incoming == 0){ 2784 log_error("LE Data Channel packet received but no incoming credits"); 2785 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2786 break; 2787 } 2788 l2cap_channel->credits_incoming--; 2789 2790 // automatic credits 2791 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 2792 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 2793 } 2794 2795 // first fragment 2796 uint16_t pos = 0; 2797 if (!l2cap_channel->receive_sdu_len){ 2798 l2cap_channel->receive_sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2799 l2cap_channel->receive_sdu_pos = 0; 2800 pos += 2; 2801 size -= 2; 2802 } 2803 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], size-COMPLETE_L2CAP_HEADER); 2804 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 2805 // done? 2806 log_info("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 2807 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 2808 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 2809 l2cap_channel->receive_sdu_len = 0; 2810 } 2811 } else { 2812 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 2813 } 2814 #endif 2815 break; 2816 } 2817 2818 l2cap_run(); 2819 } 2820 2821 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 2822 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 2823 int index = l2cap_fixed_channel_table_index_for_channel_id(channel_id); 2824 if (index < 0) return; 2825 fixed_channels[index].callback = the_packet_handler; 2826 } 2827 2828 #ifdef ENABLE_CLASSIC 2829 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 2830 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 2831 channel->state = L2CAP_STATE_CLOSED; 2832 l2cap_emit_channel_closed(channel); 2833 // discard channel 2834 l2cap_stop_rtx(channel); 2835 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2836 btstack_memory_l2cap_channel_free(channel); 2837 } 2838 2839 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 2840 btstack_linked_list_iterator_t it; 2841 btstack_linked_list_iterator_init(&it, services); 2842 while (btstack_linked_list_iterator_has_next(&it)){ 2843 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 2844 if ( service->psm == psm){ 2845 return service; 2846 }; 2847 } 2848 return NULL; 2849 } 2850 2851 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 2852 return l2cap_get_service_internal(&l2cap_services, psm); 2853 } 2854 2855 2856 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 2857 2858 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 2859 2860 // check for alread registered psm 2861 l2cap_service_t *service = l2cap_get_service(psm); 2862 if (service) { 2863 log_error("l2cap_register_service: PSM %u already registered", psm); 2864 return L2CAP_SERVICE_ALREADY_REGISTERED; 2865 } 2866 2867 // alloc structure 2868 service = btstack_memory_l2cap_service_get(); 2869 if (!service) { 2870 log_error("l2cap_register_service: no memory for l2cap_service_t"); 2871 return BTSTACK_MEMORY_ALLOC_FAILED; 2872 } 2873 2874 // fill in 2875 service->psm = psm; 2876 service->mtu = mtu; 2877 service->packet_handler = service_packet_handler; 2878 service->required_security_level = security_level; 2879 2880 // add to services list 2881 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 2882 2883 // enable page scan 2884 gap_connectable_control(1); 2885 2886 return 0; 2887 } 2888 2889 uint8_t l2cap_unregister_service(uint16_t psm){ 2890 2891 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 2892 2893 l2cap_service_t *service = l2cap_get_service(psm); 2894 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 2895 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 2896 btstack_memory_l2cap_service_free(service); 2897 2898 // disable page scan when no services registered 2899 if (btstack_linked_list_empty(&l2cap_services)) { 2900 gap_connectable_control(0); 2901 } 2902 return 0; 2903 } 2904 #endif 2905 2906 2907 #ifdef ENABLE_LE_DATA_CHANNELS 2908 2909 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 2910 if (!channel->waiting_for_can_send_now) return; 2911 if (channel->send_sdu_buffer) return; 2912 channel->waiting_for_can_send_now = 0; 2913 log_info("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 2914 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 2915 } 2916 2917 // 1BH2222 2918 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 2919 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", 2920 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 2921 uint8_t event[19]; 2922 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 2923 event[1] = sizeof(event) - 2; 2924 event[2] = channel->address_type; 2925 reverse_bd_addr(channel->address, &event[3]); 2926 little_endian_store_16(event, 9, channel->con_handle); 2927 little_endian_store_16(event, 11, channel->psm); 2928 little_endian_store_16(event, 13, channel->local_cid); 2929 little_endian_store_16(event, 15, channel->remote_cid); 2930 little_endian_store_16(event, 17, channel->remote_mtu); 2931 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2932 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 2933 } 2934 // 11BH22222 2935 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 2936 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", 2937 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 2938 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 2939 uint8_t event[23]; 2940 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 2941 event[1] = sizeof(event) - 2; 2942 event[2] = status; 2943 event[3] = channel->address_type; 2944 reverse_bd_addr(channel->address, &event[4]); 2945 little_endian_store_16(event, 10, channel->con_handle); 2946 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 2947 little_endian_store_16(event, 13, channel->psm); 2948 little_endian_store_16(event, 15, channel->local_cid); 2949 little_endian_store_16(event, 17, channel->remote_cid); 2950 little_endian_store_16(event, 19, channel->local_mtu); 2951 little_endian_store_16(event, 21, channel->remote_mtu); 2952 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2953 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 2954 } 2955 2956 static l2cap_channel_t * l2cap_le_get_channel_for_local_cid(uint16_t local_cid){ 2957 btstack_linked_list_iterator_t it; 2958 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2959 while (btstack_linked_list_iterator_has_next(&it)){ 2960 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2961 if ( channel->local_cid == local_cid) { 2962 return channel; 2963 } 2964 } 2965 return NULL; 2966 } 2967 2968 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 2969 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 2970 channel->state = L2CAP_STATE_CLOSED; 2971 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 2972 // discard channel 2973 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2974 btstack_memory_l2cap_channel_free(channel); 2975 } 2976 2977 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 2978 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 2979 } 2980 2981 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 2982 2983 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 2984 2985 // check for alread registered psm 2986 l2cap_service_t *service = l2cap_le_get_service(psm); 2987 if (service) { 2988 return L2CAP_SERVICE_ALREADY_REGISTERED; 2989 } 2990 2991 // alloc structure 2992 service = btstack_memory_l2cap_service_get(); 2993 if (!service) { 2994 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 2995 return BTSTACK_MEMORY_ALLOC_FAILED; 2996 } 2997 2998 // fill in 2999 service->psm = psm; 3000 service->mtu = 0; 3001 service->packet_handler = packet_handler; 3002 service->required_security_level = security_level; 3003 3004 // add to services list 3005 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 3006 3007 // done 3008 return 0; 3009 } 3010 3011 uint8_t l2cap_le_unregister_service(uint16_t psm) { 3012 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 3013 l2cap_service_t *service = l2cap_le_get_service(psm); 3014 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3015 3016 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 3017 btstack_memory_l2cap_service_free(service); 3018 return 0; 3019 } 3020 3021 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 3022 // get channel 3023 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3024 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3025 3026 // validate state 3027 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3028 return ERROR_CODE_COMMAND_DISALLOWED; 3029 } 3030 3031 // set state accept connection 3032 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 3033 channel->receive_sdu_buffer = receive_sdu_buffer; 3034 channel->local_mtu = mtu; 3035 channel->new_credits_incoming = initial_credits; 3036 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3037 3038 // test 3039 // channel->new_credits_incoming = 1; 3040 3041 // go 3042 l2cap_run(); 3043 return 0; 3044 } 3045 3046 /** 3047 * @brief Deny incoming LE Data Channel connection due to resource constraints 3048 * @param local_cid L2CAP LE Data Channel Identifier 3049 */ 3050 3051 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 3052 // get channel 3053 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3054 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3055 3056 // validate state 3057 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3058 return ERROR_CODE_COMMAND_DISALLOWED; 3059 } 3060 3061 // set state decline connection 3062 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 3063 channel->reason = 0x04; // no resources available 3064 l2cap_run(); 3065 return 0; 3066 } 3067 3068 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 3069 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 3070 uint16_t * out_local_cid) { 3071 3072 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 3073 3074 3075 hci_connection_t * connection = hci_connection_for_handle(con_handle); 3076 if (!connection) { 3077 log_error("no hci_connection for handle 0x%04x", con_handle); 3078 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 3079 } 3080 3081 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, connection->address, connection->address_type, psm, mtu, security_level); 3082 if (!channel) { 3083 return BTSTACK_MEMORY_ALLOC_FAILED; 3084 } 3085 log_info("l2cap_le_create_channel %p", channel); 3086 3087 // store local_cid 3088 if (out_local_cid){ 3089 *out_local_cid = channel->local_cid; 3090 } 3091 3092 // provide buffer 3093 channel->con_handle = con_handle; 3094 channel->receive_sdu_buffer = receive_sdu_buffer; 3095 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 3096 channel->new_credits_incoming = initial_credits; 3097 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3098 3099 // add to connections list 3100 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 3101 3102 // go 3103 l2cap_run(); 3104 return 0; 3105 } 3106 3107 /** 3108 * @brief Provide credtis for LE Data Channel 3109 * @param local_cid L2CAP LE Data Channel Identifier 3110 * @param credits Number additional credits for peer 3111 */ 3112 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 3113 3114 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3115 if (!channel) { 3116 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3117 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3118 } 3119 3120 // check state 3121 if (channel->state != L2CAP_STATE_OPEN){ 3122 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 3123 } 3124 3125 // assert incoming credits + credits <= 0xffff 3126 uint32_t total_credits = channel->credits_incoming; 3127 total_credits += channel->new_credits_incoming; 3128 total_credits += credits; 3129 if (total_credits > 0xffff){ 3130 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 3131 channel->new_credits_incoming, credits); 3132 } 3133 3134 // set credits_granted 3135 channel->new_credits_incoming += credits; 3136 3137 // go 3138 l2cap_run(); 3139 return 0; 3140 } 3141 3142 /** 3143 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 3144 * @param local_cid L2CAP LE Data Channel Identifier 3145 */ 3146 int l2cap_le_can_send_now(uint16_t local_cid){ 3147 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3148 if (!channel) { 3149 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3150 return 0; 3151 } 3152 3153 // check state 3154 if (channel->state != L2CAP_STATE_OPEN) return 0; 3155 3156 // check queue 3157 if (channel->send_sdu_buffer) return 0; 3158 3159 // fine, go ahead 3160 return 1; 3161 } 3162 3163 /** 3164 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 3165 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 3166 * so packet handler should be ready to handle it 3167 * @param local_cid L2CAP LE Data Channel Identifier 3168 */ 3169 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 3170 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3171 if (!channel) { 3172 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 3173 return 0; 3174 } 3175 channel->waiting_for_can_send_now = 1; 3176 l2cap_le_notify_channel_can_send(channel); 3177 return 0; 3178 } 3179 3180 /** 3181 * @brief Send data via LE Data Channel 3182 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 3183 * @param local_cid L2CAP LE Data Channel Identifier 3184 * @param data data to send 3185 * @param size data size 3186 */ 3187 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 3188 3189 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3190 if (!channel) { 3191 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3192 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3193 } 3194 3195 if (len > channel->remote_mtu){ 3196 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 3197 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 3198 } 3199 3200 if (channel->send_sdu_buffer){ 3201 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 3202 return BTSTACK_ACL_BUFFERS_FULL; 3203 } 3204 3205 channel->send_sdu_buffer = data; 3206 channel->send_sdu_len = len; 3207 channel->send_sdu_pos = 0; 3208 3209 l2cap_run(); 3210 return 0; 3211 } 3212 3213 /** 3214 * @brief Disconnect from LE Data Channel 3215 * @param local_cid L2CAP LE Data Channel Identifier 3216 */ 3217 uint8_t l2cap_le_disconnect(uint16_t local_cid) 3218 { 3219 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3220 if (!channel) { 3221 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3222 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3223 } 3224 3225 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3226 l2cap_run(); 3227 return 0; 3228 } 3229 3230 #endif 3231