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