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