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