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