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