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