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