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