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