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