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