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 #ifdef ENABLE_CLASSIC 214 l2cap_channels = NULL; 215 l2cap_services = NULL; 216 require_security_level2_for_outgoing_sdp = 0; 217 #endif 218 219 #ifdef ENABLE_LE_DATA_CHANNELS 220 l2cap_le_services = NULL; 221 l2cap_le_channels = NULL; 222 #endif 223 224 #ifdef ENABLE_BLE 225 l2cap_event_packet_handler = NULL; 226 #endif 227 memset(fixed_channels, 0, sizeof(fixed_channels)); 228 229 // 230 // register callback with HCI 231 // 232 hci_event_callback_registration.callback = &l2cap_hci_event_handler; 233 hci_add_event_handler(&hci_event_callback_registration); 234 235 hci_register_acl_packet_handler(&l2cap_acl_handler); 236 237 #ifdef ENABLE_CLASSIC 238 gap_connectable_control(0); // no services yet 239 #endif 240 } 241 242 void l2cap_register_packet_handler(void (*handler)(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 243 #ifdef ENABLE_BLE 244 l2cap_event_packet_handler = handler; 245 #else 246 UNUSED(handler); 247 #endif 248 } 249 250 void l2cap_request_can_send_fix_channel_now_event(hci_con_handle_t con_handle, uint16_t channel_id){ 251 UNUSED(con_handle); 252 253 int index = l2cap_fixed_channel_table_index_for_channel_id(channel_id); 254 if (index < 0) return; 255 fixed_channels[index].waiting_for_can_send_now = 1; 256 l2cap_notify_channel_can_send(); 257 } 258 259 int l2cap_can_send_fixed_channel_packet_now(hci_con_handle_t con_handle, uint16_t channel_id){ 260 UNUSED(channel_id); 261 262 return hci_can_send_acl_packet_now(con_handle); 263 } 264 265 uint8_t *l2cap_get_outgoing_buffer(void){ 266 return hci_get_outgoing_packet_buffer() + COMPLETE_L2CAP_HEADER; // 8 bytes 267 } 268 269 int l2cap_reserve_packet_buffer(void){ 270 return hci_reserve_packet_buffer(); 271 } 272 273 void l2cap_release_packet_buffer(void){ 274 hci_release_packet_buffer(); 275 } 276 277 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){ 278 // 0 - Connection handle : PB=pb : BC=00 279 little_endian_store_16(acl_buffer, 0, con_handle | (packet_boundary << 12) | (0 << 14)); 280 // 2 - ACL length 281 little_endian_store_16(acl_buffer, 2, len + 4); 282 // 4 - L2CAP packet length 283 little_endian_store_16(acl_buffer, 4, len + 0); 284 // 6 - L2CAP channel DEST 285 little_endian_store_16(acl_buffer, 6, remote_cid); 286 } 287 288 // assumption - only on LE connections 289 int l2cap_send_prepared_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint16_t len){ 290 291 if (!hci_is_packet_buffer_reserved()){ 292 log_error("l2cap_send_prepared_connectionless called without reserving packet first"); 293 return BTSTACK_ACL_BUFFERS_FULL; 294 } 295 296 if (!hci_can_send_prepared_acl_packet_now(con_handle)){ 297 log_info("l2cap_send_prepared_connectionless handle 0x%02x, cid 0x%02x, cannot send", con_handle, cid); 298 return BTSTACK_ACL_BUFFERS_FULL; 299 } 300 301 log_debug("l2cap_send_prepared_connectionless handle %u, cid 0x%02x", con_handle, cid); 302 303 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 304 l2cap_setup_header(acl_buffer, con_handle, 0, cid, len); 305 // send 306 return hci_send_acl_packet_buffer(len+8); 307 } 308 309 // assumption - only on LE connections 310 int l2cap_send_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint8_t *data, uint16_t len){ 311 312 if (!hci_can_send_acl_packet_now(con_handle)){ 313 log_info("l2cap_send cid 0x%02x, cannot send", cid); 314 return BTSTACK_ACL_BUFFERS_FULL; 315 } 316 317 hci_reserve_packet_buffer(); 318 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 319 320 memcpy(&acl_buffer[8], data, len); 321 322 return l2cap_send_prepared_connectionless(con_handle, cid, len); 323 } 324 325 static void l2cap_emit_can_send_now(btstack_packet_handler_t packet_handler, uint16_t channel) { 326 log_debug("L2CAP_EVENT_CHANNEL_CAN_SEND_NOW local_cid 0x%x", channel); 327 uint8_t event[4]; 328 event[0] = L2CAP_EVENT_CAN_SEND_NOW; 329 event[1] = sizeof(event) - 2; 330 little_endian_store_16(event, 2, channel); 331 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 332 packet_handler(HCI_EVENT_PACKET, channel, event, sizeof(event)); 333 } 334 335 #ifdef L2CAP_USES_CHANNELS 336 static void l2cap_dispatch_to_channel(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){ 337 (* (channel->packet_handler))(type, channel->local_cid, data, size); 338 } 339 340 static void l2cap_emit_simple_event_with_cid(l2cap_channel_t * channel, uint8_t event_code){ 341 uint8_t event[4]; 342 event[0] = event_code; 343 event[1] = sizeof(event) - 2; 344 little_endian_store_16(event, 2, channel->local_cid); 345 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 346 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 347 } 348 #endif 349 350 #ifdef ENABLE_CLASSIC 351 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 352 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", 353 status, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 354 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu, channel->flush_timeout); 355 uint8_t event[24]; 356 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 357 event[1] = sizeof(event) - 2; 358 event[2] = status; 359 reverse_bd_addr(channel->address, &event[3]); 360 little_endian_store_16(event, 9, channel->con_handle); 361 little_endian_store_16(event, 11, channel->psm); 362 little_endian_store_16(event, 13, channel->local_cid); 363 little_endian_store_16(event, 15, channel->remote_cid); 364 little_endian_store_16(event, 17, channel->local_mtu); 365 little_endian_store_16(event, 19, channel->remote_mtu); 366 little_endian_store_16(event, 21, channel->flush_timeout); 367 event[23] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 368 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 369 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 370 } 371 372 static void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 373 log_info("L2CAP_EVENT_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 374 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 375 } 376 377 static void l2cap_emit_incoming_connection(l2cap_channel_t *channel) { 378 log_info("L2CAP_EVENT_INCOMING_CONNECTION addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x", 379 bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid); 380 uint8_t event[16]; 381 event[0] = L2CAP_EVENT_INCOMING_CONNECTION; 382 event[1] = sizeof(event) - 2; 383 reverse_bd_addr(channel->address, &event[2]); 384 little_endian_store_16(event, 8, channel->con_handle); 385 little_endian_store_16(event, 10, channel->psm); 386 little_endian_store_16(event, 12, channel->local_cid); 387 little_endian_store_16(event, 14, channel->remote_cid); 388 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 389 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 390 } 391 392 static l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){ 393 btstack_linked_list_iterator_t it; 394 btstack_linked_list_iterator_init(&it, &l2cap_channels); 395 while (btstack_linked_list_iterator_has_next(&it)){ 396 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 397 if ( channel->local_cid == local_cid) { 398 return channel; 399 } 400 } 401 return NULL; 402 } 403 404 /// 405 406 void l2cap_request_can_send_now_event(uint16_t local_cid){ 407 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 408 if (!channel) return; 409 channel->waiting_for_can_send_now = 1; 410 l2cap_notify_channel_can_send(); 411 } 412 413 int l2cap_can_send_packet_now(uint16_t local_cid){ 414 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 415 if (!channel) return 0; 416 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 417 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 418 // get num free tx buffers 419 int num_tx_buffers_used = channel->tx_write_index - channel->tx_read_index; 420 if (num_tx_buffers_used < 0){ 421 num_tx_buffers_used += channel->num_tx_buffers; 422 } 423 int num_free_tx_buffers = channel->num_tx_buffers - num_tx_buffers_used; 424 // calculate num tx buffers for remote MTU 425 int num_tx_buffers_for_max_remote_mtu; 426 if (channel->remote_mtu <= channel->remote_mps){ 427 // MTU fits into single packet 428 num_tx_buffers_for_max_remote_mtu = 1; 429 } else { 430 // include SDU Length 431 num_tx_buffers_for_max_remote_mtu = (channel->remote_mtu + 2 + (channel->remote_mps - 1)) / channel->remote_mps; 432 } 433 return num_tx_buffers_for_max_remote_mtu <= num_free_tx_buffers; 434 } 435 #endif 436 return hci_can_send_acl_packet_now(channel->con_handle); 437 } 438 439 int l2cap_can_send_prepared_packet_now(uint16_t local_cid){ 440 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 441 if (!channel) return 0; 442 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 443 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 444 return 0; 445 } 446 #endif 447 return hci_can_send_prepared_acl_packet_now(channel->con_handle); 448 } 449 450 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){ 451 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 452 if (channel) { 453 return channel->remote_mtu; 454 } 455 return 0; 456 } 457 458 static l2cap_channel_t * l2cap_channel_for_rtx_timer(btstack_timer_source_t * ts){ 459 btstack_linked_list_iterator_t it; 460 btstack_linked_list_iterator_init(&it, &l2cap_channels); 461 while (btstack_linked_list_iterator_has_next(&it)){ 462 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 463 if ( &channel->rtx == ts) { 464 return channel; 465 } 466 } 467 return NULL; 468 } 469 470 static void l2cap_rtx_timeout(btstack_timer_source_t * ts){ 471 l2cap_channel_t * channel = l2cap_channel_for_rtx_timer(ts); 472 if (!channel) return; 473 474 log_info("l2cap_rtx_timeout for local cid 0x%02x", channel->local_cid); 475 476 // "When terminating the channel, it is not necessary to send a L2CAP_DisconnectReq 477 // and enter WAIT_DISCONNECT state. Channels can be transitioned directly to the CLOSED state." 478 // notify client 479 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_RTX_TIMEOUT); 480 481 // discard channel 482 // no need to stop timer here, it is removed from list during timer callback 483 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 484 btstack_memory_l2cap_channel_free(channel); 485 } 486 487 static void l2cap_stop_rtx(l2cap_channel_t * channel){ 488 log_info("l2cap_stop_rtx for local cid 0x%02x", channel->local_cid); 489 btstack_run_loop_remove_timer(&channel->rtx); 490 } 491 492 static void l2cap_start_rtx(l2cap_channel_t * channel){ 493 l2cap_stop_rtx(channel); 494 log_info("l2cap_start_rtx for local cid 0x%02x", channel->local_cid); 495 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 496 btstack_run_loop_set_timer(&channel->rtx, L2CAP_RTX_TIMEOUT_MS); 497 btstack_run_loop_add_timer(&channel->rtx); 498 } 499 500 static void l2cap_start_ertx(l2cap_channel_t * channel){ 501 log_info("l2cap_start_ertx for local cid 0x%02x", channel->local_cid); 502 l2cap_stop_rtx(channel); 503 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 504 btstack_run_loop_set_timer(&channel->rtx, L2CAP_ERTX_TIMEOUT_MS); 505 btstack_run_loop_add_timer(&channel->rtx); 506 } 507 508 void l2cap_require_security_level_2_for_outgoing_sdp(void){ 509 require_security_level2_for_outgoing_sdp = 1; 510 } 511 512 static int l2cap_security_level_0_allowed_for_PSM(uint16_t psm){ 513 return (psm == BLUETOOTH_PROTOCOL_SDP) && (!require_security_level2_for_outgoing_sdp); 514 } 515 516 static int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 517 if (!hci_can_send_acl_packet_now(handle)){ 518 log_info("l2cap_send_signaling_packet, cannot send"); 519 return BTSTACK_ACL_BUFFERS_FULL; 520 } 521 522 // log_info("l2cap_send_signaling_packet type %u", cmd); 523 hci_reserve_packet_buffer(); 524 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 525 va_list argptr; 526 va_start(argptr, identifier); 527 uint16_t len = l2cap_create_signaling_classic(acl_buffer, handle, cmd, identifier, argptr); 528 va_end(argptr); 529 // log_info("l2cap_send_signaling_packet con %u!", handle); 530 return hci_send_acl_packet_buffer(len); 531 } 532 533 // assumption - only on Classic connections 534 int l2cap_send_prepared(uint16_t local_cid, uint16_t len){ 535 536 if (!hci_is_packet_buffer_reserved()){ 537 log_error("l2cap_send_prepared called without reserving packet first"); 538 return BTSTACK_ACL_BUFFERS_FULL; 539 } 540 541 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 542 if (!channel) { 543 log_error("l2cap_send_prepared no channel for cid 0x%02x", local_cid); 544 return -1; // TODO: define error 545 } 546 547 if (!hci_can_send_prepared_acl_packet_now(channel->con_handle)){ 548 log_info("l2cap_send_prepared cid 0x%02x, cannot send", local_cid); 549 return BTSTACK_ACL_BUFFERS_FULL; 550 } 551 552 log_debug("l2cap_send_prepared cid 0x%02x, handle %u, 1 credit used", local_cid, channel->con_handle); 553 554 int fcs_size = 0; 555 556 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 557 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 558 fcs_size = 2; 559 } 560 #endif 561 562 // set non-flushable packet boundary flag if supported on Controller 563 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 564 uint8_t packet_boundary_flag = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 565 l2cap_setup_header(acl_buffer, channel->con_handle, packet_boundary_flag, channel->remote_cid, len + fcs_size); 566 567 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 568 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 569 // calculate FCS over l2cap data 570 uint16_t fcs = crc16_calc(acl_buffer + 4, 4 + len); 571 log_info("I-Frame: fcs 0x%04x", fcs); 572 little_endian_store_16(acl_buffer, 8 + len, fcs); 573 } 574 #endif 575 576 // send 577 return hci_send_acl_packet_buffer(len+8+fcs_size); 578 } 579 580 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 581 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){ 582 return (((uint16_t) sar) << 14) | (req_seq << 8) | (final << 7) | (tx_seq << 1) | 0; 583 } 584 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){ 585 return (req_seq << 8) | (final << 7) | (poll << 4) | (((int) supervisory_function) << 2) | 1; 586 } 587 static int l2cap_next_ertm_seq_nr(int seq_nr){ 588 return (seq_nr + 1) & 0x3f; 589 } 590 static void l2cap_ertm_next_tx_write_index(l2cap_channel_t * channel){ 591 channel->tx_write_index++; 592 if (channel->tx_write_index < channel->num_tx_buffers) return; 593 channel->tx_write_index = 0; 594 } 595 static void l2cap_ertm_monitor_timeout_callback(btstack_timer_source_t * ts){ 596 log_info("l2cap_ertm_monitor_timeout_callback"); 597 l2cap_channel_t * l2cap_channel = (l2cap_channel_t *) btstack_run_loop_get_timer_context(ts); 598 599 // TODO: we assume that it's the oldest packet 600 l2cap_ertm_tx_packet_state_t * tx_state; 601 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 602 603 // check retry count 604 if (tx_state->retry_count < l2cap_channel->remote_max_transmit){ 605 // increment retry count 606 tx_state->retry_count++; 607 608 // start monitor timer 609 btstack_run_loop_set_timer_handler(&tx_state->monitor_timer, &l2cap_ertm_monitor_timeout_callback); 610 btstack_run_loop_set_timer_context(&tx_state->monitor_timer, l2cap_channel); 611 btstack_run_loop_set_timer(&tx_state->monitor_timer, l2cap_channel->local_monitor_timeout_ms); 612 btstack_run_loop_add_timer(&tx_state->monitor_timer); 613 614 // send RR/P=1 615 l2cap_channel->send_supervisor_frame_receiver_ready_poll = 1; 616 } else { 617 log_info("Monitor timer expired & retry count >= max transmit -> disconnect"); 618 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 619 } 620 l2cap_run(); 621 } 622 static void l2cap_ertm_retransmission_timeout_callback(btstack_timer_source_t * ts){ 623 log_info("l2cap_ertm_retransmission_timeout_callback"); 624 l2cap_channel_t * l2cap_channel = (l2cap_channel_t *) btstack_run_loop_get_timer_context(ts); 625 626 // TODO: we assume that it's the oldest packet 627 l2cap_ertm_tx_packet_state_t * tx_state; 628 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 629 630 // set retry count = 1 631 tx_state->retry_count = 1; 632 633 // start monitor timer 634 btstack_run_loop_set_timer_handler(&tx_state->monitor_timer, &l2cap_ertm_monitor_timeout_callback); 635 btstack_run_loop_set_timer_context(&tx_state->monitor_timer, l2cap_channel); 636 btstack_run_loop_set_timer(&tx_state->monitor_timer, l2cap_channel->local_monitor_timeout_ms); 637 btstack_run_loop_add_timer(&tx_state->monitor_timer); 638 639 // send RR/P=1 640 l2cap_channel->send_supervisor_frame_receiver_ready_poll = 1; 641 l2cap_run(); 642 } 643 static int l2cap_ertm_send_information_frame(l2cap_channel_t * channel, int index, int final){ 644 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index]; 645 hci_reserve_packet_buffer(); 646 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 647 uint16_t control = l2cap_encanced_control_field_for_information_frame(tx_state->tx_seq, final, channel->req_seq, tx_state->sar); 648 log_info("I-Frame: control 0x%04x", control); 649 little_endian_store_16(acl_buffer, 8, control); 650 memcpy(&acl_buffer[8+2], &channel->tx_packets_data[index * channel->local_mtu], tx_state->len); 651 // send 652 return l2cap_send_prepared(channel->local_cid, 2 + tx_state->len); 653 } 654 655 static void l2cap_ertm_store_fragment(l2cap_channel_t * channel, l2cap_segmentation_and_reassembly_t sar, uint16_t sdu_length, uint8_t * data, uint16_t len){ 656 // get next index for storing packets 657 int index = channel->tx_write_index; 658 659 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index]; 660 tx_state->tx_seq = channel->next_tx_seq; 661 tx_state->len = len; 662 tx_state->sar = sar; 663 tx_state->retry_count = 0; 664 665 uint8_t * tx_packet = &channel->tx_packets_data[index * channel->local_mtu]; 666 int pos = 0; 667 if (sar == L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU){ 668 little_endian_store_16(tx_packet, 0, sdu_length); 669 pos += 2; 670 } 671 memcpy(&tx_packet[pos], data, len); 672 673 // update 674 channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq); 675 l2cap_ertm_next_tx_write_index(channel); 676 677 // set retransmission timer 678 btstack_run_loop_set_timer_handler(&tx_state->retransmission_timer, &l2cap_ertm_retransmission_timeout_callback); 679 btstack_run_loop_set_timer_context(&tx_state->retransmission_timer, channel); 680 btstack_run_loop_set_timer(&tx_state->retransmission_timer, channel->local_retransmission_timeout_ms); 681 btstack_run_loop_add_timer(&tx_state->retransmission_timer); 682 } 683 684 static int l2cap_ertm_send(l2cap_channel_t * channel, uint8_t * data, uint16_t len){ 685 if (len > channel->remote_mtu){ 686 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", channel->local_cid); 687 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 688 } 689 690 // check if it needs to get fragmented 691 if (len > channel->remote_mps){ 692 // fragmentation needed. 693 l2cap_segmentation_and_reassembly_t sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU; 694 int chunk_len; 695 while (len){ 696 switch (sar){ 697 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 698 chunk_len = channel->remote_mps - 2; // sdu_length 699 l2cap_ertm_store_fragment(channel, sar, len, data, chunk_len); 700 len -= chunk_len; 701 sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU; 702 break; 703 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 704 chunk_len = channel->remote_mps; 705 if (chunk_len >= len){ 706 sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU; 707 chunk_len = len; 708 } 709 l2cap_ertm_store_fragment(channel, sar, len, data, chunk_len); 710 len -= chunk_len; 711 break; 712 default: 713 break; 714 } 715 } 716 717 } else { 718 l2cap_ertm_store_fragment(channel, L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU, 0, data, len); 719 } 720 721 // try to send 722 l2cap_run(); 723 return 0; 724 } 725 #endif 726 727 // assumption - only on Classic connections 728 int l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){ 729 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 730 if (!channel) { 731 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 732 return -1; // TODO: define error 733 } 734 735 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 736 // send in ERTM 737 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 738 return l2cap_ertm_send(channel, data, len); 739 } 740 #endif 741 742 if (len > channel->remote_mtu){ 743 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 744 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 745 } 746 747 if (!hci_can_send_acl_packet_now(channel->con_handle)){ 748 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 749 return BTSTACK_ACL_BUFFERS_FULL; 750 } 751 752 hci_reserve_packet_buffer(); 753 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 754 memcpy(&acl_buffer[8], data, len); 755 return l2cap_send_prepared(local_cid, len); 756 } 757 758 int l2cap_send_echo_request(hci_con_handle_t con_handle, uint8_t *data, uint16_t len){ 759 return l2cap_send_signaling_packet(con_handle, ECHO_REQUEST, 0x77, len, data); 760 } 761 762 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 763 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var | flag); 764 } 765 766 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 767 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var & ~flag); 768 } 769 #endif 770 771 772 #ifdef ENABLE_BLE 773 static int l2cap_send_le_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 774 775 if (!hci_can_send_acl_packet_now(handle)){ 776 log_info("l2cap_send_le_signaling_packet, cannot send"); 777 return BTSTACK_ACL_BUFFERS_FULL; 778 } 779 780 // log_info("l2cap_send_le_signaling_packet type %u", cmd); 781 hci_reserve_packet_buffer(); 782 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 783 va_list argptr; 784 va_start(argptr, identifier); 785 uint16_t len = l2cap_create_signaling_le(acl_buffer, handle, cmd, identifier, argptr); 786 va_end(argptr); 787 // log_info("l2cap_send_le_signaling_packet con %u!", handle); 788 return hci_send_acl_packet_buffer(len); 789 } 790 #endif 791 792 uint16_t l2cap_max_mtu(void){ 793 return HCI_ACL_PAYLOAD_SIZE - L2CAP_HEADER_SIZE; 794 } 795 796 uint16_t l2cap_max_le_mtu(void){ 797 return l2cap_max_mtu(); 798 } 799 800 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 801 static uint16_t l2cap_setup_options_ertm(l2cap_channel_t * channel, uint8_t * config_options){ 802 config_options[0] = 0x04; // RETRANSMISSION AND FLOW CONTROL OPTION 803 config_options[1] = 9; // length 804 config_options[2] = (uint8_t) channel->mode; 805 config_options[3] = channel->num_rx_buffers; // == TxWindows size 806 config_options[4] = channel->local_max_transmit; 807 little_endian_store_16( config_options, 5, channel->local_retransmission_timeout_ms); 808 little_endian_store_16( config_options, 7, channel->local_monitor_timeout_ms); 809 little_endian_store_16( config_options, 9, channel->local_mps); 810 return 11; 811 } 812 static int l2cap_ertm_send_supervisor_frame(l2cap_channel_t * channel, uint16_t control){ 813 hci_reserve_packet_buffer(); 814 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 815 log_info("S-Frame: control 0x%04x", control); 816 little_endian_store_16(acl_buffer, 8, control); 817 return l2cap_send_prepared(channel->local_cid, 2); 818 } 819 static int l2cap_ertm_num_unacknowledged_tx_packets(l2cap_channel_t * channel){ 820 int unacknowledged_packets = channel->tx_send_index - channel->tx_read_index; 821 if (unacknowledged_packets < 0){ 822 unacknowledged_packets += channel->num_tx_buffers; 823 } 824 return unacknowledged_packets; 825 } 826 #endif 827 828 #ifdef ENABLE_CLASSIC 829 830 static uint16_t l2cap_setup_options_mtu(l2cap_channel_t * channel, uint8_t * config_options){ 831 config_options[0] = 1; // MTU 832 config_options[1] = 2; // len param 833 little_endian_store_16( (uint8_t*)&config_options, 2, channel->local_mtu); 834 return 4; 835 } 836 837 static uint16_t l2cap_setup_options(l2cap_channel_t * channel, uint8_t * config_options){ 838 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 839 // use ERTM options if supported 840 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 841 if ((connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_DONE) && (connection->l2cap_state.extended_feature_mask & 0x08)){ 842 return l2cap_setup_options_ertm(channel, config_options); 843 844 } 845 #endif 846 return l2cap_setup_options_mtu(channel, config_options); 847 } 848 849 static uint32_t l2cap_extended_features_mask(void){ 850 // extended features request supported, features: fixed channels, unicast connectionless data reception 851 uint32_t features = 0x280; 852 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 853 features |= 0x0008; 854 #endif 855 return features; 856 } 857 #endif 858 859 // MARK: L2CAP_RUN 860 // process outstanding signaling tasks 861 static void l2cap_run(void){ 862 863 // log_info("l2cap_run: entered"); 864 865 // check pending signaling responses 866 while (signaling_responses_pending){ 867 868 hci_con_handle_t handle = signaling_responses[0].handle; 869 870 if (!hci_can_send_acl_packet_now(handle)) break; 871 872 uint8_t sig_id = signaling_responses[0].sig_id; 873 uint8_t response_code = signaling_responses[0].code; 874 uint16_t infoType = signaling_responses[0].data; // INFORMATION_REQUEST 875 uint16_t result = signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT 876 #ifdef ENABLE_CLASSIC 877 uint16_t source_cid = signaling_responses[0].cid; // CONNECTION_REQUEST 878 #endif 879 UNUSED(infoType); 880 881 // remove first item before sending (to avoid sending response mutliple times) 882 signaling_responses_pending--; 883 int i; 884 for (i=0; i < signaling_responses_pending; i++){ 885 memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t)); 886 } 887 888 switch (response_code){ 889 #ifdef ENABLE_CLASSIC 890 case CONNECTION_REQUEST: 891 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 892 // also disconnect if result is 0x0003 - security blocked 893 if (result == 0x0003){ 894 hci_disconnect_security_block(handle); 895 } 896 break; 897 case ECHO_REQUEST: 898 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 899 break; 900 case INFORMATION_REQUEST: 901 switch (infoType){ 902 case 1: { // Connectionless MTU 903 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 904 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(connectionless_mtu), &connectionless_mtu); 905 } 906 break; 907 case 2: { // Extended Features Supported 908 uint32_t features = l2cap_extended_features_mask(); 909 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(features), &features); 910 } 911 break; 912 case 3: { // Fixed Channels Supported 913 uint8_t map[8]; 914 memset(map, 0, 8); 915 map[0] = 0x06; // L2CAP Signaling Channel (0x02) + Connectionless reception (0x04) 916 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(map), &map); 917 } 918 break; 919 default: 920 // all other types are not supported 921 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL); 922 break; 923 } 924 break; 925 case COMMAND_REJECT: 926 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 927 break; 928 #endif 929 #ifdef ENABLE_BLE 930 case LE_CREDIT_BASED_CONNECTION_REQUEST: 931 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 932 break; 933 case COMMAND_REJECT_LE: 934 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 935 break; 936 #endif 937 default: 938 // should not happen 939 break; 940 } 941 } 942 943 btstack_linked_list_iterator_t it; 944 UNUSED(it); 945 946 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 947 // send l2cap information request if neccessary 948 hci_connections_get_iterator(&it); 949 while(btstack_linked_list_iterator_has_next(&it)){ 950 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 951 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST){ 952 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE; 953 // send information request for extended features 954 uint8_t sig_id = l2cap_next_sig_id(); 955 uint8_t info_type = 2; 956 l2cap_send_signaling_packet(connection->con_handle, INFORMATION_REQUEST, sig_id, info_type); 957 return; 958 } 959 } 960 #endif 961 962 #ifdef ENABLE_CLASSIC 963 uint8_t config_options[10]; 964 btstack_linked_list_iterator_init(&it, &l2cap_channels); 965 while (btstack_linked_list_iterator_has_next(&it)){ 966 967 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 968 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 969 switch (channel->state){ 970 971 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 972 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 973 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 974 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 975 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 976 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0); 977 } 978 break; 979 980 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 981 if (!hci_can_send_command_packet_now()) break; 982 // send connection request - set state first 983 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 984 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 985 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1); 986 break; 987 988 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 989 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 990 channel->state = L2CAP_STATE_INVALID; 991 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0); 992 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 993 l2cap_stop_rtx(channel); 994 btstack_linked_list_iterator_remove(&it); 995 btstack_memory_l2cap_channel_free(channel); 996 break; 997 998 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 999 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1000 channel->state = L2CAP_STATE_CONFIG; 1001 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1002 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 1003 break; 1004 1005 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 1006 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1007 // success, start l2cap handshake 1008 channel->local_sig_id = l2cap_next_sig_id(); 1009 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 1010 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 1011 l2cap_start_rtx(channel); 1012 break; 1013 1014 case L2CAP_STATE_CONFIG: 1015 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1016 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 1017 uint16_t flags = 0; 1018 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1019 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 1020 flags = 1; 1021 } else { 1022 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1023 } 1024 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 1025 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1026 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL); 1027 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1028 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 1029 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1030 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1031 uint16_t options_size = l2cap_setup_options(channel, config_options); 1032 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); 1033 #endif 1034 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 1035 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1036 uint16_t options_size = l2cap_setup_options(channel, config_options); 1037 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); 1038 } else { 1039 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 1040 } 1041 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1042 } 1043 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 1044 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1045 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 1046 channel->local_sig_id = l2cap_next_sig_id(); 1047 uint16_t options_size = l2cap_setup_options(channel, config_options); 1048 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, options_size, &config_options); 1049 l2cap_start_rtx(channel); 1050 } 1051 if (l2cap_channel_ready_for_open(channel)){ 1052 channel->state = L2CAP_STATE_OPEN; 1053 l2cap_emit_channel_opened(channel, 0); // success 1054 } 1055 break; 1056 1057 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1058 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1059 channel->state = L2CAP_STATE_INVALID; 1060 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1061 // 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 :) 1062 l2cap_finialize_channel_close(channel); // -- remove from list 1063 break; 1064 1065 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1066 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1067 channel->local_sig_id = l2cap_next_sig_id(); 1068 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1069 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1070 break; 1071 default: 1072 break; 1073 } 1074 1075 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1076 // send s-frame to acknowledge received packets 1077 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1078 1079 if (channel->tx_send_index != channel->tx_write_index){ 1080 int unacknowledged_packets = l2cap_ertm_num_unacknowledged_tx_packets(channel); 1081 // check remote tx window 1082 log_info("unacknowledged_packets %u, remote tx window size %u", unacknowledged_packets, channel->remote_tx_window_size); 1083 if (unacknowledged_packets < channel->remote_tx_window_size){ 1084 int index = channel->tx_send_index; 1085 channel->tx_send_index++; 1086 if (channel->tx_send_index >= channel->num_tx_buffers){ 1087 channel->tx_send_index = 0; 1088 } 1089 l2cap_ertm_send_information_frame(channel, index, 0); // final = 0 1090 continue; 1091 } 1092 } 1093 1094 if (channel->send_supervisor_frame_receiver_ready){ 1095 channel->send_supervisor_frame_receiver_ready = 0; 1096 log_info("Send S-Frame: RR %u, final %u", channel->req_seq, channel->set_final_bit_after_packet_with_poll_bit_set); 1097 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 0, channel->set_final_bit_after_packet_with_poll_bit_set, channel->req_seq); 1098 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1099 l2cap_ertm_send_supervisor_frame(channel, control); 1100 continue; 1101 } 1102 if (channel->send_supervisor_frame_receiver_ready_poll){ 1103 channel->send_supervisor_frame_receiver_ready_poll = 0; 1104 log_info("Send S-Frame: RR %u with poll=1 ", channel->req_seq); 1105 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 1, 0, channel->req_seq); 1106 l2cap_ertm_send_supervisor_frame(channel, control); 1107 continue; 1108 } 1109 if (channel->send_supervisor_frame_receiver_not_ready){ 1110 channel->send_supervisor_frame_receiver_not_ready = 0; 1111 log_info("Send S-Frame: RNR %u", channel->req_seq); 1112 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 1113 l2cap_ertm_send_supervisor_frame(channel, control); 1114 continue; 1115 } 1116 if (channel->send_supervisor_frame_reject){ 1117 channel->send_supervisor_frame_reject = 0; 1118 log_info("Send S-Frame: REJ %u", channel->req_seq); 1119 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT, 0, 0, channel->req_seq); 1120 l2cap_ertm_send_supervisor_frame(channel, control); 1121 continue; 1122 } 1123 if (channel->send_supervisor_frame_selective_reject){ 1124 channel->send_supervisor_frame_selective_reject = 0; 1125 log_info("Send S-Frame: SREJ %u", channel->expected_tx_seq); 1126 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT, 0, channel->set_final_bit_after_packet_with_poll_bit_set, channel->expected_tx_seq); 1127 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1128 l2cap_ertm_send_supervisor_frame(channel, control); 1129 continue; 1130 } 1131 1132 if (channel->srej_active){ 1133 int i; 1134 for (i=0;i<channel->num_tx_buffers;i++){ 1135 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[i]; 1136 if (tx_state->retransmission_requested) { 1137 tx_state->retransmission_requested = 0; 1138 uint8_t final = channel->set_final_bit_after_packet_with_poll_bit_set; 1139 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1140 l2cap_ertm_send_information_frame(channel, i, final); 1141 break; 1142 } 1143 } 1144 if (i == channel->num_tx_buffers){ 1145 // no retransmission request found 1146 channel->srej_active = 0; 1147 } else { 1148 // packet was sent 1149 continue; 1150 } 1151 } 1152 #endif 1153 1154 } 1155 #endif 1156 1157 #ifdef ENABLE_LE_DATA_CHANNELS 1158 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1159 while (btstack_linked_list_iterator_has_next(&it)){ 1160 uint8_t * acl_buffer; 1161 uint8_t * l2cap_payload; 1162 uint16_t pos; 1163 uint16_t payload_size; 1164 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1165 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1166 switch (channel->state){ 1167 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1168 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1169 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1170 // le psm, source cid, mtu, mps, initial credits 1171 channel->local_sig_id = l2cap_next_sig_id(); 1172 channel->credits_incoming = channel->new_credits_incoming; 1173 channel->new_credits_incoming = 0; 1174 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); 1175 break; 1176 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1177 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1178 // TODO: support larger MPS 1179 channel->state = L2CAP_STATE_OPEN; 1180 channel->credits_incoming = channel->new_credits_incoming; 1181 channel->new_credits_incoming = 0; 1182 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); 1183 // notify client 1184 l2cap_emit_le_channel_opened(channel, 0); 1185 break; 1186 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1187 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1188 channel->state = L2CAP_STATE_INVALID; 1189 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1190 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1191 l2cap_stop_rtx(channel); 1192 btstack_linked_list_iterator_remove(&it); 1193 btstack_memory_l2cap_channel_free(channel); 1194 break; 1195 case L2CAP_STATE_OPEN: 1196 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1197 1198 // send credits 1199 if (channel->new_credits_incoming){ 1200 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1201 channel->local_sig_id = l2cap_next_sig_id(); 1202 uint16_t new_credits = channel->new_credits_incoming; 1203 channel->new_credits_incoming = 0; 1204 channel->credits_incoming += new_credits; 1205 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1206 break; 1207 } 1208 1209 // send data 1210 if (!channel->send_sdu_buffer) break; 1211 if (!channel->credits_outgoing) break; 1212 1213 // send part of SDU 1214 hci_reserve_packet_buffer(); 1215 acl_buffer = hci_get_outgoing_packet_buffer(); 1216 l2cap_payload = acl_buffer + 8; 1217 pos = 0; 1218 if (!channel->send_sdu_pos){ 1219 // store SDU len 1220 channel->send_sdu_pos += 2; 1221 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 1222 pos += 2; 1223 } 1224 payload_size = btstack_min(channel->send_sdu_len + 2 - channel->send_sdu_pos, channel->remote_mps - pos); 1225 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 1226 memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len 1227 pos += payload_size; 1228 channel->send_sdu_pos += payload_size; 1229 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 1230 // done 1231 1232 channel->credits_outgoing--; 1233 1234 if (channel->send_sdu_pos >= channel->send_sdu_len + 2){ 1235 channel->send_sdu_buffer = NULL; 1236 // send done event 1237 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 1238 // inform about can send now 1239 l2cap_le_notify_channel_can_send(channel); 1240 } 1241 hci_send_acl_packet_buffer(8 + pos); 1242 break; 1243 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1244 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1245 channel->local_sig_id = l2cap_next_sig_id(); 1246 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1247 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1248 break; 1249 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1250 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1251 channel->state = L2CAP_STATE_INVALID; 1252 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1253 l2cap_le_finialize_channel_close(channel); // -- remove from list 1254 break; 1255 default: 1256 break; 1257 } 1258 } 1259 #endif 1260 1261 #ifdef ENABLE_BLE 1262 // send l2cap con paramter update if necessary 1263 hci_connections_get_iterator(&it); 1264 while(btstack_linked_list_iterator_has_next(&it)){ 1265 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1266 if (connection->address_type != BD_ADDR_TYPE_LE_PUBLIC && connection->address_type != BD_ADDR_TYPE_LE_RANDOM) continue; 1267 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1268 switch (connection->le_con_parameter_update_state){ 1269 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1270 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1271 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, connection->le_con_param_update_identifier, 1272 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1273 break; 1274 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1275 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1276 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1277 break; 1278 case CON_PARAMETER_UPDATE_DENY: 1279 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1280 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1281 break; 1282 default: 1283 break; 1284 } 1285 } 1286 #endif 1287 1288 // log_info("l2cap_run: exit"); 1289 } 1290 1291 #ifdef ENABLE_CLASSIC 1292 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1293 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 1294 log_info("l2cap_handle_connection_complete expected state"); 1295 // success, start l2cap handshake 1296 channel->con_handle = con_handle; 1297 // check remote SSP feature first 1298 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1299 } 1300 } 1301 1302 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1303 1304 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1305 // assumption: outgoing connection 1306 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1307 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1308 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1309 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1310 return; 1311 } 1312 #endif 1313 1314 // fine, go ahead 1315 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1316 } 1317 1318 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1319 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1320 1321 // we have been waiting for remote supported features, if both support SSP, 1322 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)); 1323 if (gap_ssp_supported_on_both_sides(channel->con_handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){ 1324 // request security level 2 1325 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 1326 gap_request_security_level(channel->con_handle, LEVEL_2); 1327 return; 1328 } 1329 1330 l2cap_ready_to_connect(channel); 1331 } 1332 #endif 1333 1334 #ifdef L2CAP_USES_CHANNELS 1335 static l2cap_channel_t * l2cap_create_channel_entry(btstack_packet_handler_t packet_handler, bd_addr_t address, bd_addr_type_t address_type, 1336 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 1337 1338 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 1339 if (!channel) { 1340 return NULL; 1341 } 1342 1343 // Init memory (make valgrind happy) 1344 memset(channel, 0, sizeof(l2cap_channel_t)); 1345 1346 // fill in 1347 channel->packet_handler = packet_handler; 1348 bd_addr_copy(channel->address, address); 1349 channel->address_type = address_type; 1350 channel->psm = psm; 1351 channel->local_mtu = local_mtu; 1352 channel->remote_mtu = L2CAP_MINIMAL_MTU; 1353 channel->required_security_level = security_level; 1354 1355 // 1356 channel->local_cid = l2cap_next_local_cid(); 1357 channel->con_handle = 0; 1358 1359 // set initial state 1360 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 1361 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 1362 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 1363 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 1364 1365 return channel; 1366 } 1367 #endif 1368 1369 #ifdef ENABLE_CLASSIC 1370 1371 /** 1372 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 1373 * @param packet_handler 1374 * @param address 1375 * @param psm 1376 * @param mtu 1377 * @param local_cid 1378 */ 1379 1380 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){ 1381 // limit MTU to the size of our outtgoing HCI buffer 1382 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 1383 1384 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x mtu %u -> local mtu %u", bd_addr_to_str(address), psm, mtu, local_mtu); 1385 1386 l2cap_channel_t * channel = l2cap_create_channel_entry(channel_packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1387 if (!channel) { 1388 return BTSTACK_MEMORY_ALLOC_FAILED; 1389 } 1390 1391 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1392 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1393 #endif 1394 1395 // add to connections list 1396 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1397 1398 // store local_cid 1399 if (out_local_cid){ 1400 *out_local_cid = channel->local_cid; 1401 } 1402 1403 // check if hci connection is already usable 1404 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1405 if (conn){ 1406 log_info("l2cap_create_channel, hci connection already exists"); 1407 l2cap_handle_connection_complete(conn->con_handle, channel); 1408 // check if remote supported fearures are already received 1409 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1410 l2cap_handle_remote_supported_features_received(channel); 1411 } 1412 } 1413 1414 l2cap_run(); 1415 1416 return 0; 1417 } 1418 1419 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1420 1421 static uint8_t l2cap_ertm_validate_local_config(uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, 1422 uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){ 1423 1424 UNUSED(buffer); 1425 UNUSED(size); 1426 1427 uint8_t result = ERROR_CODE_SUCCESS; 1428 if (max_transmit < 1){ 1429 log_error("max_transmit must be >= 1"); 1430 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1431 } 1432 if (retransmission_timeout_ms < 2000){ 1433 log_error("retransmission_timeout_ms must be >= 2000 ms"); 1434 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1435 } 1436 if (monitor_timeout_ms < 12000){ 1437 log_error("monitor_timeout_ms must be >= 12000 ms"); 1438 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1439 } 1440 if (local_mtu < 48){ 1441 log_error("local_mtu must be >= 48"); 1442 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1443 } 1444 if (num_rx_buffers < 1){ 1445 log_error("num_rx_buffers must be >= 1"); 1446 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1447 } 1448 if (num_tx_buffers < 1){ 1449 log_error("num_rx_buffers must be >= 1"); 1450 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1451 } 1452 return result; 1453 } 1454 1455 static void l2cap_ertm_configure_channel(l2cap_channel_t * channel, int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, 1456 uint16_t monitor_timeout_ms, uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){ 1457 1458 channel->mode = L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION; 1459 channel->ertm_mandatory = ertm_mandatory; 1460 channel->local_max_transmit = max_transmit; 1461 channel->local_retransmission_timeout_ms = retransmission_timeout_ms; 1462 channel->local_monitor_timeout_ms = monitor_timeout_ms; 1463 channel->local_mtu = local_mtu; 1464 channel->num_rx_buffers = num_rx_buffers; 1465 channel->num_tx_buffers = num_tx_buffers; 1466 1467 // align buffer to 16-byte boundary, just in case 1468 int bytes_till_alignment = 16 - (((uintptr_t) buffer) & 0x0f); 1469 buffer += bytes_till_alignment; 1470 size -= bytes_till_alignment; 1471 1472 // setup state buffers 1473 uint32_t pos = 0; 1474 channel->rx_packets_state = (l2cap_ertm_rx_packet_state_t *) &buffer[pos]; 1475 pos += num_rx_buffers * sizeof(l2cap_ertm_rx_packet_state_t); 1476 channel->tx_packets_state = (l2cap_ertm_tx_packet_state_t *) &buffer[pos]; 1477 pos += num_tx_buffers * sizeof(l2cap_ertm_tx_packet_state_t); 1478 1479 // setup reassembly buffer 1480 channel->reassembly_buffer = &buffer[pos]; 1481 pos += local_mtu; 1482 1483 // divide rest of data equally 1484 channel->local_mps = (size - pos) / (num_rx_buffers + num_tx_buffers); 1485 log_info("Local MPS: %u", channel->local_mtu); 1486 channel->rx_packets_data = &buffer[pos]; 1487 pos += num_rx_buffers * channel->local_mtu; 1488 channel->tx_packets_data = &buffer[pos]; 1489 } 1490 1491 uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, 1492 int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, 1493 uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size, uint16_t * out_local_cid){ 1494 1495 log_info("L2CAP_CREATE_CHANNEL addr %s, psm 0x%x, local mtu %u", bd_addr_to_str(address), psm, local_mtu); 1496 1497 // validate local config 1498 uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, local_mtu, num_tx_buffers, num_rx_buffers, buffer, size); 1499 if (result) return result; 1500 1501 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1502 if (!channel) { 1503 return BTSTACK_MEMORY_ALLOC_FAILED; 1504 } 1505 1506 // configure ERTM 1507 l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, 1508 local_mtu, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1509 1510 // add to connections list 1511 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1512 1513 // store local_cid 1514 if (out_local_cid){ 1515 *out_local_cid = channel->local_cid; 1516 } 1517 1518 // check if hci connection is already usable 1519 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1520 if (conn){ 1521 log_info("l2cap_create_channel, hci connection already exists"); 1522 l2cap_handle_connection_complete(conn->con_handle, channel); 1523 // check if remote supported fearures are already received 1524 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1525 l2cap_handle_remote_supported_features_received(channel); 1526 } 1527 } 1528 1529 l2cap_run(); 1530 1531 return 0; 1532 } 1533 #endif 1534 1535 void 1536 l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 1537 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 1538 // find channel for local_cid 1539 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1540 if (channel) { 1541 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1542 } 1543 // process 1544 l2cap_run(); 1545 } 1546 1547 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 1548 btstack_linked_list_iterator_t it; 1549 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1550 while (btstack_linked_list_iterator_has_next(&it)){ 1551 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1552 if ( bd_addr_cmp( channel->address, address) != 0) continue; 1553 // channel for this address found 1554 switch (channel->state){ 1555 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 1556 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1557 // failure, forward error code 1558 l2cap_emit_channel_opened(channel, status); 1559 // discard channel 1560 l2cap_stop_rtx(channel); 1561 btstack_linked_list_iterator_remove(&it); 1562 btstack_memory_l2cap_channel_free(channel); 1563 break; 1564 default: 1565 break; 1566 } 1567 } 1568 } 1569 1570 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 1571 btstack_linked_list_iterator_t it; 1572 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1573 while (btstack_linked_list_iterator_has_next(&it)){ 1574 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1575 if ( ! bd_addr_cmp( channel->address, address) ){ 1576 l2cap_handle_connection_complete(handle, channel); 1577 } 1578 } 1579 // process 1580 l2cap_run(); 1581 } 1582 #endif 1583 1584 static void l2cap_notify_channel_can_send(void){ 1585 1586 #ifdef ENABLE_CLASSIC 1587 btstack_linked_list_iterator_t it; 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->waiting_for_can_send_now) continue; 1592 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1593 channel->waiting_for_can_send_now = 0; 1594 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 1595 } 1596 #endif 1597 1598 int i; 1599 for (i=0;i<L2CAP_FIXED_CHANNEL_TABLE_SIZE;i++){ 1600 if (!fixed_channels[i].callback) continue; 1601 if (!fixed_channels[i].waiting_for_can_send_now) continue; 1602 int can_send = 0; 1603 if (l2cap_fixed_channel_table_index_is_le(i)){ 1604 #ifdef ENABLE_BLE 1605 can_send = hci_can_send_acl_le_packet_now(); 1606 #endif 1607 } else { 1608 #ifdef ENABLE_CLASSIC 1609 can_send = hci_can_send_acl_classic_packet_now(); 1610 #endif 1611 } 1612 if (!can_send) continue; 1613 fixed_channels[i].waiting_for_can_send_now = 0; 1614 l2cap_emit_can_send_now(fixed_channels[i].callback, l2cap_fixed_channel_table_channel_id_for_index(i)); 1615 } 1616 } 1617 1618 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 1619 1620 UNUSED(packet_type); 1621 UNUSED(cid); 1622 UNUSED(size); 1623 1624 bd_addr_t address; 1625 hci_con_handle_t handle; 1626 int hci_con_used; 1627 btstack_linked_list_iterator_t it; 1628 1629 // avoid unused warnings 1630 UNUSED(address); 1631 UNUSED(hci_con_used); 1632 UNUSED(it); 1633 UNUSED(handle); 1634 1635 switch(hci_event_packet_get_type(packet)){ 1636 1637 // Notify channel packet handler if they can send now 1638 case HCI_EVENT_TRANSPORT_PACKET_SENT: 1639 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 1640 l2cap_run(); // try sending signaling packets first 1641 l2cap_notify_channel_can_send(); 1642 break; 1643 1644 case HCI_EVENT_COMMAND_STATUS: 1645 l2cap_run(); // try sending signaling packets first 1646 break; 1647 1648 #ifdef ENABLE_CLASSIC 1649 // handle connection complete events 1650 case HCI_EVENT_CONNECTION_COMPLETE: 1651 reverse_bd_addr(&packet[5], address); 1652 if (packet[2] == 0){ 1653 handle = little_endian_read_16(packet, 3); 1654 l2cap_handle_connection_success_for_addr(address, handle); 1655 } else { 1656 l2cap_handle_connection_failed_for_addr(address, packet[2]); 1657 } 1658 break; 1659 1660 // handle successful create connection cancel command 1661 case HCI_EVENT_COMMAND_COMPLETE: 1662 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 1663 if (packet[5] == 0){ 1664 reverse_bd_addr(&packet[6], address); 1665 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 1666 l2cap_handle_connection_failed_for_addr(address, 0x16); 1667 } 1668 } 1669 l2cap_run(); // try sending signaling packets first 1670 break; 1671 #endif 1672 1673 // handle disconnection complete events 1674 case HCI_EVENT_DISCONNECTION_COMPLETE: 1675 // send l2cap disconnect events for all channels on this handle and free them 1676 #ifdef ENABLE_CLASSIC 1677 handle = little_endian_read_16(packet, 3); 1678 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1679 while (btstack_linked_list_iterator_has_next(&it)){ 1680 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1681 if (channel->con_handle != handle) continue; 1682 l2cap_emit_channel_closed(channel); 1683 l2cap_stop_rtx(channel); 1684 btstack_linked_list_iterator_remove(&it); 1685 btstack_memory_l2cap_channel_free(channel); 1686 } 1687 #endif 1688 #ifdef ENABLE_LE_DATA_CHANNELS 1689 handle = little_endian_read_16(packet, 3); 1690 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1691 while (btstack_linked_list_iterator_has_next(&it)){ 1692 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1693 if (channel->con_handle != handle) continue; 1694 l2cap_emit_channel_closed(channel); 1695 btstack_linked_list_iterator_remove(&it); 1696 btstack_memory_l2cap_channel_free(channel); 1697 } 1698 #endif 1699 break; 1700 1701 // HCI Connection Timeouts 1702 #ifdef ENABLE_CLASSIC 1703 case L2CAP_EVENT_TIMEOUT_CHECK: 1704 handle = little_endian_read_16(packet, 2); 1705 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 1706 if (hci_authentication_active_for_handle(handle)) break; 1707 hci_con_used = 0; 1708 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1709 while (btstack_linked_list_iterator_has_next(&it)){ 1710 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1711 if (channel->con_handle != handle) continue; 1712 hci_con_used = 1; 1713 break; 1714 } 1715 if (hci_con_used) break; 1716 if (!hci_can_send_command_packet_now()) break; 1717 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 1718 break; 1719 1720 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 1721 handle = little_endian_read_16(packet, 3); 1722 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1723 while (btstack_linked_list_iterator_has_next(&it)){ 1724 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1725 if (channel->con_handle != handle) continue; 1726 l2cap_handle_remote_supported_features_received(channel); 1727 break; 1728 } 1729 break; 1730 1731 case GAP_EVENT_SECURITY_LEVEL: 1732 handle = little_endian_read_16(packet, 2); 1733 log_info("l2cap - security level update"); 1734 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1735 while (btstack_linked_list_iterator_has_next(&it)){ 1736 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1737 if (channel->con_handle != handle) continue; 1738 1739 log_info("l2cap - state %u", channel->state); 1740 1741 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 1742 gap_security_level_t required_level = channel->required_security_level; 1743 1744 switch (channel->state){ 1745 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1746 if (actual_level >= required_level){ 1747 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1748 // we need to know if ERTM is supported before sending a config response 1749 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1750 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1751 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 1752 #else 1753 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 1754 l2cap_emit_incoming_connection(channel); 1755 #endif 1756 } else { 1757 channel->reason = 0x0003; // security block 1758 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1759 } 1760 break; 1761 1762 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 1763 if (actual_level >= required_level){ 1764 l2cap_ready_to_connect(channel); 1765 } else { 1766 // disconnnect, authentication not good enough 1767 hci_disconnect_security_block(handle); 1768 } 1769 break; 1770 1771 default: 1772 break; 1773 } 1774 } 1775 break; 1776 #endif 1777 1778 default: 1779 break; 1780 } 1781 1782 l2cap_run(); 1783 } 1784 1785 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 1786 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 1787 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 1788 signaling_responses[signaling_responses_pending].handle = handle; 1789 signaling_responses[signaling_responses_pending].code = code; 1790 signaling_responses[signaling_responses_pending].sig_id = sig_id; 1791 signaling_responses[signaling_responses_pending].cid = cid; 1792 signaling_responses[signaling_responses_pending].data = data; 1793 signaling_responses_pending++; 1794 l2cap_run(); 1795 } 1796 } 1797 1798 #ifdef ENABLE_CLASSIC 1799 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 1800 channel->remote_sig_id = identifier; 1801 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 1802 l2cap_run(); 1803 } 1804 1805 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 1806 1807 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 1808 l2cap_service_t *service = l2cap_get_service(psm); 1809 if (!service) { 1810 // 0x0002 PSM not supported 1811 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 1812 return; 1813 } 1814 1815 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 1816 if (!hci_connection) { 1817 // 1818 log_error("no hci_connection for handle %u", handle); 1819 return; 1820 } 1821 1822 // alloc structure 1823 // log_info("l2cap_handle_connection_request register channel"); 1824 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, hci_connection->address, BD_ADDR_TYPE_CLASSIC, 1825 psm, service->mtu, service->required_security_level); 1826 if (!channel){ 1827 // 0x0004 No resources available 1828 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 1829 return; 1830 } 1831 1832 channel->con_handle = handle; 1833 channel->remote_cid = source_cid; 1834 channel->remote_sig_id = sig_id; 1835 1836 // limit local mtu to max acl packet length - l2cap header 1837 if (channel->local_mtu > l2cap_max_mtu()) { 1838 channel->local_mtu = l2cap_max_mtu(); 1839 } 1840 1841 // set initial state 1842 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 1843 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 1844 1845 // add to connections list 1846 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1847 1848 // assert security requirements 1849 gap_request_security_level(handle, channel->required_security_level); 1850 } 1851 1852 void l2cap_accept_connection(uint16_t local_cid){ 1853 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 1854 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1855 if (!channel) { 1856 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1857 return; 1858 } 1859 1860 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1861 // configure L2CAP Basic mode 1862 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1863 #endif 1864 1865 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1866 1867 // process 1868 l2cap_run(); 1869 } 1870 1871 1872 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1873 uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, 1874 uint16_t monitor_timeout_ms, uint16_t local_mtu, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){ 1875 1876 log_info("L2CAP_ACCEPT_ERTM_CONNECTION local_cid 0x%x", local_cid); 1877 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1878 if (!channel) { 1879 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1880 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1881 } 1882 1883 // validate local config 1884 uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, local_mtu, num_tx_buffers, num_rx_buffers, buffer, size); 1885 if (result) return result; 1886 1887 // configure L2CAP ERTM 1888 l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, monitor_timeout_ms, local_mtu, num_tx_buffers, num_rx_buffers, buffer, size); 1889 1890 // continue 1891 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1892 1893 // process 1894 l2cap_run(); 1895 1896 return ERROR_CODE_SUCCESS; 1897 } 1898 1899 uint8_t l2cap_ertm_set_busy(uint16_t local_cid){ 1900 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1901 if (!channel) { 1902 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1903 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1904 } 1905 if (!channel->local_busy){ 1906 channel->local_busy = 1; 1907 channel->send_supervisor_frame_receiver_not_ready = 1; 1908 l2cap_run(); 1909 } 1910 return ERROR_CODE_SUCCESS; 1911 } 1912 1913 uint8_t l2cap_ertm_set_ready(uint16_t local_cid){ 1914 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1915 if (!channel) { 1916 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1917 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1918 } 1919 if (channel->local_busy){ 1920 channel->local_busy = 0; 1921 channel->send_supervisor_frame_receiver_ready_poll = 1; 1922 l2cap_run(); 1923 } 1924 return ERROR_CODE_SUCCESS; 1925 } 1926 1927 static void l2cap_ertm_handle_req_seq(l2cap_channel_t * l2cap_channel, uint8_t req_seq){ 1928 l2cap_ertm_tx_packet_state_t * tx_state; 1929 while (1){ 1930 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 1931 // calc delta 1932 int delta = (req_seq - tx_state->tx_seq) & 0x03f; 1933 if (delta == 0) break; // all packets acknowledged 1934 if (delta > l2cap_channel->remote_tx_window_size) break; 1935 1936 log_info("RR seq %u => packet with tx_seq %u done", req_seq, tx_state->tx_seq); 1937 1938 // stop retransmission timer 1939 btstack_run_loop_remove_timer(&tx_state->retransmission_timer); 1940 l2cap_channel->tx_read_index++; 1941 if (l2cap_channel->tx_read_index >= l2cap_channel->num_rx_buffers){ 1942 l2cap_channel->tx_read_index = 0; 1943 } 1944 1945 // no unack packages 1946 if (l2cap_channel->tx_read_index == l2cap_channel->tx_write_index) break; 1947 } 1948 } 1949 1950 static l2cap_ertm_tx_packet_state_t * l2cap_ertm_get_tx_state(l2cap_channel_t * l2cap_channel, uint8_t tx_seq){ 1951 int i; 1952 for (i=0;i<l2cap_channel->num_tx_buffers;i++){ 1953 l2cap_ertm_tx_packet_state_t * tx_state = &l2cap_channel->tx_packets_state[i]; 1954 if (tx_state->tx_seq == tx_seq) return tx_state; 1955 } 1956 return NULL; 1957 } 1958 #endif 1959 1960 1961 void l2cap_decline_connection(uint16_t local_cid){ 1962 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 1963 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1964 if (!channel) { 1965 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1966 return; 1967 } 1968 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1969 channel->reason = 0x04; // no resources available 1970 l2cap_run(); 1971 } 1972 1973 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 1974 1975 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1976 1977 uint16_t flags = little_endian_read_16(command, 6); 1978 if (flags & 1) { 1979 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1980 } 1981 1982 // accept the other's configuration options 1983 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 1984 uint16_t pos = 8; 1985 while (pos < end_pos){ 1986 uint8_t option_hint = command[pos] >> 7; 1987 uint8_t option_type = command[pos] & 0x7f; 1988 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 1989 pos++; 1990 uint8_t length = command[pos++]; 1991 // MTU { type(8): 1, len(8):2, MTU(16) } 1992 if (option_type == 1 && length == 2){ 1993 channel->remote_mtu = little_endian_read_16(command, pos); 1994 log_info("Remote MTU %u", channel->remote_mtu); 1995 if (channel->remote_mtu > l2cap_max_mtu()){ 1996 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 1997 channel->remote_mtu = l2cap_max_mtu(); 1998 } 1999 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2000 } 2001 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2002 if (option_type == 2 && length == 2){ 2003 channel->flush_timeout = little_endian_read_16(command, pos); 2004 log_info("Flush timeout: %u ms", channel->flush_timeout); 2005 } 2006 2007 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2008 // Retransmission and Flow Control Option 2009 if (option_type == 4 && length == 9){ 2010 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2011 switch(channel->mode){ 2012 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2013 // Store remote config 2014 channel->remote_tx_window_size = command[pos+1]; 2015 channel->remote_max_transmit = command[pos+2]; 2016 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2017 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2018 channel->remote_mps = little_endian_read_16(command, pos + 7); 2019 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2020 channel->remote_tx_window_size, 2021 channel->remote_max_transmit, 2022 channel->remote_retransmission_timeout_ms, 2023 channel->remote_monitor_timeout_ms, 2024 channel->remote_mps); 2025 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2026 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2027 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2028 } else { 2029 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2030 } 2031 break; 2032 case L2CAP_CHANNEL_MODE_BASIC: 2033 switch (mode){ 2034 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2035 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2036 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2037 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2038 } 2039 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2040 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2041 break; 2042 default: // case L2CAP_CHANNEL_MODE_BASIC: 2043 // TODO store and evaluate configuration 2044 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2045 break; 2046 } 2047 break; 2048 default: 2049 break; 2050 } 2051 } 2052 #endif 2053 // check for unknown options 2054 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 2055 log_info("l2cap cid %u, unknown options", channel->local_cid); 2056 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2057 } 2058 pos += length; 2059 } 2060 } 2061 2062 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2063 log_info("l2cap_signaling_handle_configure_response"); 2064 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2065 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2066 uint16_t pos = 10; 2067 while (pos < end_pos){ 2068 uint8_t option_hint = command[pos] >> 7; 2069 uint8_t option_type = command[pos] & 0x7f; 2070 log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2071 pos++; 2072 uint8_t length = command[pos++]; 2073 2074 // Retransmission and Flow Control Option 2075 if (option_type == 4 && length == 9){ 2076 switch (channel->mode){ 2077 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2078 if (channel->ertm_mandatory){ 2079 // ?? 2080 } else { 2081 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2082 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2083 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2084 } 2085 } 2086 break; 2087 case L2CAP_CHANNEL_MODE_BASIC: 2088 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2089 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2090 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2091 } 2092 break; 2093 default: 2094 break; 2095 } 2096 } 2097 2098 // check for unknown options 2099 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 2100 log_info("l2cap cid %u, unknown options", channel->local_cid); 2101 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2102 } 2103 2104 pos += length; 2105 } 2106 #endif 2107 } 2108 2109 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2110 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2111 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2112 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2113 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2114 if (channel->state == L2CAP_STATE_OPEN) return 0; 2115 return 1; 2116 } 2117 2118 2119 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2120 2121 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2122 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2123 uint16_t result = 0; 2124 2125 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2126 2127 // handle DISCONNECT REQUESTS seperately 2128 if (code == DISCONNECTION_REQUEST){ 2129 switch (channel->state){ 2130 case L2CAP_STATE_CONFIG: 2131 case L2CAP_STATE_OPEN: 2132 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2133 case L2CAP_STATE_WAIT_DISCONNECT: 2134 l2cap_handle_disconnect_request(channel, identifier); 2135 break; 2136 2137 default: 2138 // ignore in other states 2139 break; 2140 } 2141 return; 2142 } 2143 2144 // @STATEMACHINE(l2cap) 2145 switch (channel->state) { 2146 2147 case L2CAP_STATE_WAIT_CONNECT_RSP: 2148 switch (code){ 2149 case CONNECTION_RESPONSE: 2150 l2cap_stop_rtx(channel); 2151 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2152 switch (result) { 2153 case 0: 2154 // successful connection 2155 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2156 channel->state = L2CAP_STATE_CONFIG; 2157 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2158 break; 2159 case 1: 2160 // connection pending. get some coffee, but start the ERTX 2161 l2cap_start_ertx(channel); 2162 break; 2163 default: 2164 // channel closed 2165 channel->state = L2CAP_STATE_CLOSED; 2166 // map l2cap connection response result to BTstack status enumeration 2167 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2168 2169 // drop link key if security block 2170 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2171 gap_drop_link_key_for_bd_addr(channel->address); 2172 } 2173 2174 // discard channel 2175 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2176 btstack_memory_l2cap_channel_free(channel); 2177 break; 2178 } 2179 break; 2180 2181 default: 2182 //@TODO: implement other signaling packets 2183 break; 2184 } 2185 break; 2186 2187 case L2CAP_STATE_CONFIG: 2188 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2189 switch (code) { 2190 case CONFIGURE_REQUEST: 2191 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 2192 l2cap_signaling_handle_configure_request(channel, command); 2193 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 2194 // only done if continuation not set 2195 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 2196 } 2197 break; 2198 case CONFIGURE_RESPONSE: 2199 l2cap_stop_rtx(channel); 2200 l2cap_signaling_handle_configure_response(channel, result, command); 2201 switch (result){ 2202 case 0: // success 2203 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 2204 break; 2205 case 4: // pending 2206 l2cap_start_ertx(channel); 2207 break; 2208 default: 2209 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2210 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 2211 // remote does not offer ertm but it's required 2212 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2213 break; 2214 } 2215 #endif 2216 // retry on negative result 2217 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2218 break; 2219 } 2220 break; 2221 default: 2222 break; 2223 } 2224 if (l2cap_channel_ready_for_open(channel)){ 2225 // for open: 2226 channel->state = L2CAP_STATE_OPEN; 2227 l2cap_emit_channel_opened(channel, 0); 2228 } 2229 break; 2230 2231 case L2CAP_STATE_WAIT_DISCONNECT: 2232 switch (code) { 2233 case DISCONNECTION_RESPONSE: 2234 l2cap_finialize_channel_close(channel); 2235 break; 2236 default: 2237 //@TODO: implement other signaling packets 2238 break; 2239 } 2240 break; 2241 2242 case L2CAP_STATE_CLOSED: 2243 // @TODO handle incoming requests 2244 break; 2245 2246 case L2CAP_STATE_OPEN: 2247 //@TODO: implement other signaling packets, e.g. re-configure 2248 break; 2249 default: 2250 break; 2251 } 2252 // log_info("new state %u", channel->state); 2253 } 2254 2255 2256 static void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){ 2257 2258 btstack_linked_list_iterator_t it; 2259 2260 // get code, signalind identifier and command len 2261 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2262 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2263 2264 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 2265 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_RESPONSE){ 2266 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2267 return; 2268 } 2269 2270 // general commands without an assigned channel 2271 switch(code) { 2272 2273 case CONNECTION_REQUEST: { 2274 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2275 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2276 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 2277 return; 2278 } 2279 2280 case ECHO_REQUEST: 2281 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 2282 return; 2283 2284 case INFORMATION_REQUEST: { 2285 uint16_t infoType = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2286 l2cap_register_signaling_response(handle, code, sig_id, 0, infoType); 2287 return; 2288 } 2289 2290 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2291 case INFORMATION_RESPONSE: { 2292 hci_connection_t * connection = hci_connection_for_handle(handle); 2293 if (!connection) return; 2294 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2295 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2296 if (result != 0) return; 2297 if (info_type != 0x02) return; 2298 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2299 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2300 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2301 // trigger connection request 2302 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2303 while (btstack_linked_list_iterator_has_next(&it)){ 2304 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2305 if (channel->con_handle != handle) continue; 2306 // bail if ERTM was requested but is not supported 2307 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2308 if (channel->ertm_mandatory){ 2309 // channel closed 2310 channel->state = L2CAP_STATE_CLOSED; 2311 // map l2cap connection response result to BTstack status enumeration 2312 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTD); 2313 // discard channel 2314 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2315 btstack_memory_l2cap_channel_free(channel); 2316 continue; 2317 } else { 2318 // fallback to Basic mode 2319 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2320 } 2321 } 2322 // start connecting 2323 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2324 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 2325 } 2326 // respond to connection request 2327 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2328 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2329 l2cap_emit_incoming_connection(channel); 2330 } 2331 } 2332 return; 2333 } 2334 #endif 2335 2336 default: 2337 break; 2338 } 2339 2340 2341 // Get potential destination CID 2342 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2343 2344 // Find channel for this sig_id and connection handle 2345 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2346 while (btstack_linked_list_iterator_has_next(&it)){ 2347 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2348 if (channel->con_handle != handle) continue; 2349 if (code & 1) { 2350 // match odd commands (responses) by previous signaling identifier 2351 if (channel->local_sig_id == sig_id) { 2352 l2cap_signaling_handler_channel(channel, command); 2353 break; 2354 } 2355 } else { 2356 // match even commands (requests) by local channel id 2357 if (channel->local_cid == dest_cid) { 2358 l2cap_signaling_handler_channel(channel, command); 2359 break; 2360 } 2361 } 2362 } 2363 } 2364 #endif 2365 2366 #ifdef ENABLE_BLE 2367 2368 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 2369 uint8_t event[6]; 2370 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 2371 event[1] = 4; 2372 little_endian_store_16(event, 2, con_handle); 2373 little_endian_store_16(event, 4, result); 2374 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2375 if (!l2cap_event_packet_handler) return; 2376 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 2377 } 2378 2379 // @returns valid 2380 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 2381 hci_connection_t * connection; 2382 uint16_t result; 2383 uint8_t event[10]; 2384 2385 #ifdef ENABLE_LE_DATA_CHANNELS 2386 btstack_linked_list_iterator_t it; 2387 l2cap_channel_t * channel; 2388 uint16_t local_cid; 2389 uint16_t le_psm; 2390 uint16_t new_credits; 2391 uint16_t credits_before; 2392 l2cap_service_t * service; 2393 uint16_t source_cid; 2394 #endif 2395 2396 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2397 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u", code, sig_id); 2398 2399 switch (code){ 2400 2401 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 2402 result = little_endian_read_16(command, 4); 2403 l2cap_emit_connection_parameter_update_response(handle, result); 2404 break; 2405 2406 case CONNECTION_PARAMETER_UPDATE_REQUEST: 2407 connection = hci_connection_for_handle(handle); 2408 if (connection){ 2409 if (connection->role != HCI_ROLE_MASTER){ 2410 // reject command without notifying upper layer when not in master role 2411 return 0; 2412 } 2413 int update_parameter = 1; 2414 le_connection_parameter_range_t existing_range; 2415 gap_get_connection_parameter_range(&existing_range); 2416 uint16_t le_conn_interval_min = little_endian_read_16(command,8); 2417 uint16_t le_conn_interval_max = little_endian_read_16(command,10); 2418 uint16_t le_conn_latency = little_endian_read_16(command,12); 2419 uint16_t le_supervision_timeout = little_endian_read_16(command,14); 2420 2421 if (le_conn_interval_min < existing_range.le_conn_interval_min) update_parameter = 0; 2422 if (le_conn_interval_max > existing_range.le_conn_interval_max) update_parameter = 0; 2423 2424 if (le_conn_latency < existing_range.le_conn_latency_min) update_parameter = 0; 2425 if (le_conn_latency > existing_range.le_conn_latency_max) update_parameter = 0; 2426 2427 if (le_supervision_timeout < existing_range.le_supervision_timeout_min) update_parameter = 0; 2428 if (le_supervision_timeout > existing_range.le_supervision_timeout_max) update_parameter = 0; 2429 2430 if (update_parameter){ 2431 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 2432 connection->le_conn_interval_min = le_conn_interval_min; 2433 connection->le_conn_interval_max = le_conn_interval_max; 2434 connection->le_conn_latency = le_conn_latency; 2435 connection->le_supervision_timeout = le_supervision_timeout; 2436 } else { 2437 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 2438 } 2439 connection->le_con_param_update_identifier = sig_id; 2440 } 2441 2442 if (!l2cap_event_packet_handler) break; 2443 2444 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 2445 event[1] = 8; 2446 memcpy(&event[2], &command[4], 8); 2447 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2448 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2449 break; 2450 2451 #ifdef ENABLE_LE_DATA_CHANNELS 2452 2453 case COMMAND_REJECT: 2454 // Find channel for this sig_id and connection handle 2455 channel = NULL; 2456 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2457 while (btstack_linked_list_iterator_has_next(&it)){ 2458 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2459 if (a_channel->con_handle != handle) continue; 2460 if (a_channel->local_sig_id != sig_id) continue; 2461 channel = a_channel; 2462 break; 2463 } 2464 if (!channel) break; 2465 2466 // if received while waiting for le connection response, assume legacy device 2467 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 2468 channel->state = L2CAP_STATE_CLOSED; 2469 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 2470 l2cap_emit_le_channel_opened(channel, 0x0002); 2471 2472 // discard channel 2473 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2474 btstack_memory_l2cap_channel_free(channel); 2475 break; 2476 } 2477 break; 2478 2479 case LE_CREDIT_BASED_CONNECTION_REQUEST: 2480 2481 // get hci connection, bail if not found (must not happen) 2482 connection = hci_connection_for_handle(handle); 2483 if (!connection) return 0; 2484 2485 // check if service registered 2486 le_psm = little_endian_read_16(command, 4); 2487 service = l2cap_le_get_service(le_psm); 2488 source_cid = little_endian_read_16(command, 6); 2489 2490 if (service){ 2491 if (source_cid < 0x40){ 2492 // 0x0009 Connection refused - Invalid Source CID 2493 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 2494 return 1; 2495 } 2496 2497 // go through list of channels for this ACL connection and check if we get a match 2498 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2499 while (btstack_linked_list_iterator_has_next(&it)){ 2500 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2501 if (a_channel->con_handle != handle) continue; 2502 if (a_channel->remote_cid != source_cid) continue; 2503 // 0x000a Connection refused - Source CID already allocated 2504 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 2505 return 1; 2506 } 2507 2508 // security: check encryption 2509 if (service->required_security_level >= LEVEL_2){ 2510 if (sm_encryption_key_size(handle) == 0){ 2511 // 0x0008 Connection refused - insufficient encryption 2512 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 2513 return 1; 2514 } 2515 // anything less than 16 byte key size is insufficient 2516 if (sm_encryption_key_size(handle) < 16){ 2517 // 0x0007 Connection refused – insufficient encryption key size 2518 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 2519 return 1; 2520 } 2521 } 2522 2523 // security: check authencation 2524 if (service->required_security_level >= LEVEL_3){ 2525 if (!sm_authenticated(handle)){ 2526 // 0x0005 Connection refused – insufficient authentication 2527 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 2528 return 1; 2529 } 2530 } 2531 2532 // security: check authorization 2533 if (service->required_security_level >= LEVEL_4){ 2534 if (sm_authorization_state(handle) != AUTHORIZATION_GRANTED){ 2535 // 0x0006 Connection refused – insufficient authorization 2536 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 2537 return 1; 2538 } 2539 } 2540 2541 // allocate channel 2542 channel = l2cap_create_channel_entry(service->packet_handler, connection->address, 2543 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 2544 if (!channel){ 2545 // 0x0004 Connection refused – no resources available 2546 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2547 return 1; 2548 } 2549 2550 channel->con_handle = handle; 2551 channel->remote_cid = source_cid; 2552 channel->remote_sig_id = sig_id; 2553 channel->remote_mtu = little_endian_read_16(command, 8); 2554 channel->remote_mps = little_endian_read_16(command, 10); 2555 channel->credits_outgoing = little_endian_read_16(command, 12); 2556 2557 // set initial state 2558 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2559 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 2560 2561 // add to connections list 2562 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2563 2564 // post connection request event 2565 l2cap_emit_le_incoming_connection(channel); 2566 2567 } else { 2568 // Connection refused – LE_PSM not supported 2569 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2570 } 2571 break; 2572 2573 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 2574 // Find channel for this sig_id and connection handle 2575 channel = NULL; 2576 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2577 while (btstack_linked_list_iterator_has_next(&it)){ 2578 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2579 if (a_channel->con_handle != handle) continue; 2580 if (a_channel->local_sig_id != sig_id) continue; 2581 channel = a_channel; 2582 break; 2583 } 2584 if (!channel) break; 2585 2586 // cid + 0 2587 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 2588 if (result){ 2589 channel->state = L2CAP_STATE_CLOSED; 2590 // map l2cap connection response result to BTstack status enumeration 2591 l2cap_emit_le_channel_opened(channel, result); 2592 2593 // discard channel 2594 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2595 btstack_memory_l2cap_channel_free(channel); 2596 break; 2597 } 2598 2599 // success 2600 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2601 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2602 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 2603 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 2604 channel->state = L2CAP_STATE_OPEN; 2605 l2cap_emit_le_channel_opened(channel, result); 2606 break; 2607 2608 case LE_FLOW_CONTROL_CREDIT: 2609 // find channel 2610 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2611 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2612 if (!channel) { 2613 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2614 break; 2615 } 2616 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2617 credits_before = channel->credits_outgoing; 2618 channel->credits_outgoing += new_credits; 2619 // check for credit overrun 2620 if (credits_before > channel->credits_outgoing){ 2621 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 2622 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2623 break; 2624 } 2625 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 2626 break; 2627 2628 case DISCONNECTION_REQUEST: 2629 // find channel 2630 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2631 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2632 if (!channel) { 2633 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2634 break; 2635 } 2636 channel->remote_sig_id = sig_id; 2637 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2638 break; 2639 2640 #endif 2641 2642 case DISCONNECTION_RESPONSE: 2643 break; 2644 2645 default: 2646 // command unknown -> reject command 2647 return 0; 2648 } 2649 return 1; 2650 } 2651 #endif 2652 2653 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2654 2655 // @param delta number of frames in the future, >= 1 2656 static void l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel_t * l2cap_channel, l2cap_segmentation_and_reassembly_t sar, int delta, uint8_t * payload, uint16_t size){ 2657 log_info("Store SDU with delta %u", delta); 2658 // get rx state for packet to store 2659 int index = l2cap_channel->rx_store_index + delta - 1; 2660 if (index > l2cap_channel->num_rx_buffers){ 2661 index -= l2cap_channel->num_rx_buffers; 2662 } 2663 log_info("Index of packet to store %u", index); 2664 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 2665 // check if buffer is free 2666 if (rx_state->valid){ 2667 log_error("Packet buffer already used"); 2668 return; 2669 } 2670 rx_state->valid = 1; 2671 rx_state->sar = sar; 2672 rx_state->len = size; 2673 uint8_t * rx_buffer = &l2cap_channel->rx_packets_data[index]; 2674 memcpy(rx_buffer, payload, size); 2675 } 2676 2677 static void l2cap_ertm_handle_in_sequence_sdu(l2cap_channel_t * l2cap_channel, l2cap_segmentation_and_reassembly_t sar, uint8_t * payload, uint16_t size){ 2678 switch (sar){ 2679 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 2680 // packet complete -> disapatch 2681 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, payload, size); 2682 break; 2683 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 2684 // TODO: check if reassembly started 2685 // TODO: check sdu_len against local mtu 2686 l2cap_channel->reassembly_sdu_length = little_endian_read_16(payload, 0); 2687 payload += 2; 2688 size -= 2; 2689 memcpy(&l2cap_channel->reassembly_buffer[0], payload, size); 2690 l2cap_channel->reassembly_pos = size; 2691 break; 2692 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 2693 memcpy(&l2cap_channel->reassembly_buffer[l2cap_channel->reassembly_pos], payload, size); 2694 l2cap_channel->reassembly_pos += size; 2695 break; 2696 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 2697 memcpy(&l2cap_channel->reassembly_buffer[l2cap_channel->reassembly_pos], payload, size); 2698 l2cap_channel->reassembly_pos += size; 2699 // packet complete -> disapatch 2700 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->reassembly_buffer, l2cap_channel->reassembly_pos); 2701 l2cap_channel->reassembly_pos = 0; 2702 break; 2703 } 2704 } 2705 #endif 2706 2707 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size ){ 2708 UNUSED(packet_type); 2709 UNUSED(channel); 2710 2711 l2cap_channel_t * l2cap_channel; 2712 UNUSED(l2cap_channel); 2713 2714 // Get Channel ID 2715 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 2716 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 2717 2718 switch (channel_id) { 2719 2720 #ifdef ENABLE_CLASSIC 2721 case L2CAP_CID_SIGNALING: { 2722 uint16_t command_offset = 8; 2723 while (command_offset < size) { 2724 // handle signaling commands 2725 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 2726 2727 // increment command_offset 2728 command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2729 } 2730 break; 2731 } 2732 #endif 2733 2734 #ifdef ENABLE_BLE 2735 case L2CAP_CID_SIGNALING_LE: { 2736 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 2737 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 2738 if (!valid){ 2739 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2740 } 2741 break; 2742 } 2743 #endif 2744 2745 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 2746 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback) { 2747 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2748 } 2749 break; 2750 2751 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 2752 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback) { 2753 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2754 } 2755 break; 2756 2757 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 2758 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback) { 2759 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2760 } 2761 break; 2762 2763 default: 2764 #ifdef ENABLE_CLASSIC 2765 // Find channel for this channel_id and connection handle 2766 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 2767 if (l2cap_channel) { 2768 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2769 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2770 2771 // verify FCS 2772 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 2773 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 2774 log_info("Packet FCS 0x%04x, calculated FCS 0x%04x", fcs_packet, fcs_calculated); 2775 if (fcs_calculated != fcs_packet){ 2776 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 2777 // TODO: trigger retransmission or something like that 2778 break; 2779 } 2780 2781 // switch on packet type 2782 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2783 uint8_t req_seq = (control >> 8) & 0x3f; 2784 int final = (control >> 7) & 0x01; 2785 if (control & 1){ 2786 // S-Frame 2787 int poll = (control >> 4) & 0x01; 2788 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 2789 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 2790 l2cap_ertm_tx_packet_state_t * tx_state; 2791 switch (s){ 2792 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 2793 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 2794 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2795 if (poll && final){ 2796 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 2797 log_error("P=F=1 in S-Frame"); 2798 break; 2799 } 2800 if (poll){ 2801 // check if we did request selective retransmission before <==> we have stored SDU segments 2802 int i; 2803 int num_stored_out_of_order_packets = 0; 2804 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 2805 int index = l2cap_channel->rx_store_index + i; 2806 if (index >= l2cap_channel->num_rx_buffers){ 2807 index -= l2cap_channel->num_rx_buffers; 2808 } 2809 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 2810 if (!rx_state->valid) continue; 2811 num_stored_out_of_order_packets++; 2812 } 2813 if (num_stored_out_of_order_packets){ 2814 l2cap_channel->send_supervisor_frame_selective_reject = 1; 2815 } else { 2816 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 2817 } 2818 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 2819 } 2820 if (final){ 2821 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 2822 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 2823 } 2824 break; 2825 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 2826 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 2827 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2828 // rsetart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_handle_req_seq) 2829 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 2830 break; 2831 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 2832 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 2833 break; 2834 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 2835 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 2836 if (poll){ 2837 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2838 } 2839 // find requested i-frame 2840 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 2841 if (tx_state){ 2842 log_info("Retransmission for tx_seq %u requested", req_seq); 2843 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 2844 tx_state->retransmission_requested = 1; 2845 l2cap_channel->srej_active = 1; 2846 } 2847 break; 2848 default: 2849 break; 2850 } 2851 break; 2852 } else { 2853 // I-Frame 2854 // get control 2855 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 2856 uint8_t tx_seq = (control >> 1) & 0x3f; 2857 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 2858 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 2859 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 2860 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2861 if (final){ 2862 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 2863 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 2864 } 2865 // check ordering 2866 if (l2cap_channel->expected_tx_seq == tx_seq){ 2867 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 2868 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 2869 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 2870 2871 // process SDU 2872 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, &packet[COMPLETE_L2CAP_HEADER+2], size-(COMPLETE_L2CAP_HEADER+2)); 2873 2874 // process stored segments 2875 while (1){ 2876 int index = l2cap_channel->rx_store_index; 2877 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 2878 if (!rx_state->valid) break; 2879 2880 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 2881 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 2882 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 2883 2884 rx_state->valid = 0; 2885 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 2886 2887 // update rx store index 2888 index++; 2889 if (index >= l2cap_channel->num_rx_buffers){ 2890 index = 0; 2891 } 2892 l2cap_channel->rx_store_index = index; 2893 } 2894 2895 // 2896 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 2897 2898 } else { 2899 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 2900 if (delta < 2){ 2901 // store segment 2902 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, &packet[COMPLETE_L2CAP_HEADER+2], size-(COMPLETE_L2CAP_HEADER+2)); 2903 2904 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 2905 l2cap_channel->send_supervisor_frame_selective_reject = 1; 2906 } else { 2907 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 2908 l2cap_channel->send_supervisor_frame_reject = 1; 2909 } 2910 } 2911 } 2912 break; 2913 } 2914 #endif 2915 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2916 } 2917 #endif 2918 #ifdef ENABLE_LE_DATA_CHANNELS 2919 l2cap_channel = l2cap_le_get_channel_for_local_cid(channel_id); 2920 if (l2cap_channel) { 2921 // credit counting 2922 if (l2cap_channel->credits_incoming == 0){ 2923 log_error("LE Data Channel packet received but no incoming credits"); 2924 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2925 break; 2926 } 2927 l2cap_channel->credits_incoming--; 2928 2929 // automatic credits 2930 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 2931 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 2932 } 2933 2934 // first fragment 2935 uint16_t pos = 0; 2936 if (!l2cap_channel->receive_sdu_len){ 2937 l2cap_channel->receive_sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2938 l2cap_channel->receive_sdu_pos = 0; 2939 pos += 2; 2940 size -= 2; 2941 } 2942 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], size-COMPLETE_L2CAP_HEADER); 2943 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 2944 // done? 2945 log_info("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 2946 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 2947 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 2948 l2cap_channel->receive_sdu_len = 0; 2949 } 2950 } else { 2951 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 2952 } 2953 #endif 2954 break; 2955 } 2956 2957 l2cap_run(); 2958 } 2959 2960 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 2961 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 2962 int index = l2cap_fixed_channel_table_index_for_channel_id(channel_id); 2963 if (index < 0) return; 2964 fixed_channels[index].callback = the_packet_handler; 2965 } 2966 2967 #ifdef ENABLE_CLASSIC 2968 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 2969 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 2970 channel->state = L2CAP_STATE_CLOSED; 2971 l2cap_emit_channel_closed(channel); 2972 // discard channel 2973 l2cap_stop_rtx(channel); 2974 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2975 btstack_memory_l2cap_channel_free(channel); 2976 } 2977 2978 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 2979 btstack_linked_list_iterator_t it; 2980 btstack_linked_list_iterator_init(&it, services); 2981 while (btstack_linked_list_iterator_has_next(&it)){ 2982 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 2983 if ( service->psm == psm){ 2984 return service; 2985 }; 2986 } 2987 return NULL; 2988 } 2989 2990 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 2991 return l2cap_get_service_internal(&l2cap_services, psm); 2992 } 2993 2994 2995 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 2996 2997 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 2998 2999 // check for alread registered psm 3000 l2cap_service_t *service = l2cap_get_service(psm); 3001 if (service) { 3002 log_error("l2cap_register_service: PSM %u already registered", psm); 3003 return L2CAP_SERVICE_ALREADY_REGISTERED; 3004 } 3005 3006 // alloc structure 3007 service = btstack_memory_l2cap_service_get(); 3008 if (!service) { 3009 log_error("l2cap_register_service: no memory for l2cap_service_t"); 3010 return BTSTACK_MEMORY_ALLOC_FAILED; 3011 } 3012 3013 // fill in 3014 service->psm = psm; 3015 service->mtu = mtu; 3016 service->packet_handler = service_packet_handler; 3017 service->required_security_level = security_level; 3018 3019 // add to services list 3020 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3021 3022 // enable page scan 3023 gap_connectable_control(1); 3024 3025 return 0; 3026 } 3027 3028 uint8_t l2cap_unregister_service(uint16_t psm){ 3029 3030 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 3031 3032 l2cap_service_t *service = l2cap_get_service(psm); 3033 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3034 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 3035 btstack_memory_l2cap_service_free(service); 3036 3037 // disable page scan when no services registered 3038 if (btstack_linked_list_empty(&l2cap_services)) { 3039 gap_connectable_control(0); 3040 } 3041 return 0; 3042 } 3043 #endif 3044 3045 3046 #ifdef ENABLE_LE_DATA_CHANNELS 3047 3048 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 3049 if (!channel->waiting_for_can_send_now) return; 3050 if (channel->send_sdu_buffer) return; 3051 channel->waiting_for_can_send_now = 0; 3052 log_info("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 3053 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 3054 } 3055 3056 // 1BH2222 3057 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 3058 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", 3059 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 3060 uint8_t event[19]; 3061 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 3062 event[1] = sizeof(event) - 2; 3063 event[2] = channel->address_type; 3064 reverse_bd_addr(channel->address, &event[3]); 3065 little_endian_store_16(event, 9, channel->con_handle); 3066 little_endian_store_16(event, 11, channel->psm); 3067 little_endian_store_16(event, 13, channel->local_cid); 3068 little_endian_store_16(event, 15, channel->remote_cid); 3069 little_endian_store_16(event, 17, channel->remote_mtu); 3070 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3071 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3072 } 3073 // 11BH22222 3074 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 3075 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", 3076 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 3077 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 3078 uint8_t event[23]; 3079 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 3080 event[1] = sizeof(event) - 2; 3081 event[2] = status; 3082 event[3] = channel->address_type; 3083 reverse_bd_addr(channel->address, &event[4]); 3084 little_endian_store_16(event, 10, channel->con_handle); 3085 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 3086 little_endian_store_16(event, 13, channel->psm); 3087 little_endian_store_16(event, 15, channel->local_cid); 3088 little_endian_store_16(event, 17, channel->remote_cid); 3089 little_endian_store_16(event, 19, channel->local_mtu); 3090 little_endian_store_16(event, 21, channel->remote_mtu); 3091 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3092 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3093 } 3094 3095 static l2cap_channel_t * l2cap_le_get_channel_for_local_cid(uint16_t local_cid){ 3096 btstack_linked_list_iterator_t it; 3097 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 3098 while (btstack_linked_list_iterator_has_next(&it)){ 3099 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3100 if ( channel->local_cid == local_cid) { 3101 return channel; 3102 } 3103 } 3104 return NULL; 3105 } 3106 3107 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3108 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 3109 channel->state = L2CAP_STATE_CLOSED; 3110 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 3111 // discard channel 3112 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 3113 btstack_memory_l2cap_channel_free(channel); 3114 } 3115 3116 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 3117 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 3118 } 3119 3120 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 3121 3122 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 3123 3124 // check for alread registered psm 3125 l2cap_service_t *service = l2cap_le_get_service(psm); 3126 if (service) { 3127 return L2CAP_SERVICE_ALREADY_REGISTERED; 3128 } 3129 3130 // alloc structure 3131 service = btstack_memory_l2cap_service_get(); 3132 if (!service) { 3133 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 3134 return BTSTACK_MEMORY_ALLOC_FAILED; 3135 } 3136 3137 // fill in 3138 service->psm = psm; 3139 service->mtu = 0; 3140 service->packet_handler = packet_handler; 3141 service->required_security_level = security_level; 3142 3143 // add to services list 3144 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 3145 3146 // done 3147 return 0; 3148 } 3149 3150 uint8_t l2cap_le_unregister_service(uint16_t psm) { 3151 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 3152 l2cap_service_t *service = l2cap_le_get_service(psm); 3153 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3154 3155 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 3156 btstack_memory_l2cap_service_free(service); 3157 return 0; 3158 } 3159 3160 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 3161 // get channel 3162 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3163 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3164 3165 // validate state 3166 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3167 return ERROR_CODE_COMMAND_DISALLOWED; 3168 } 3169 3170 // set state accept connection 3171 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 3172 channel->receive_sdu_buffer = receive_sdu_buffer; 3173 channel->local_mtu = mtu; 3174 channel->new_credits_incoming = initial_credits; 3175 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3176 3177 // test 3178 // channel->new_credits_incoming = 1; 3179 3180 // go 3181 l2cap_run(); 3182 return 0; 3183 } 3184 3185 /** 3186 * @brief Deny incoming LE Data Channel connection due to resource constraints 3187 * @param local_cid L2CAP LE Data Channel Identifier 3188 */ 3189 3190 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 3191 // get channel 3192 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3193 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3194 3195 // validate state 3196 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3197 return ERROR_CODE_COMMAND_DISALLOWED; 3198 } 3199 3200 // set state decline connection 3201 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 3202 channel->reason = 0x04; // no resources available 3203 l2cap_run(); 3204 return 0; 3205 } 3206 3207 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 3208 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 3209 uint16_t * out_local_cid) { 3210 3211 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 3212 3213 3214 hci_connection_t * connection = hci_connection_for_handle(con_handle); 3215 if (!connection) { 3216 log_error("no hci_connection for handle 0x%04x", con_handle); 3217 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 3218 } 3219 3220 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, connection->address, connection->address_type, psm, mtu, security_level); 3221 if (!channel) { 3222 return BTSTACK_MEMORY_ALLOC_FAILED; 3223 } 3224 log_info("l2cap_le_create_channel %p", channel); 3225 3226 // store local_cid 3227 if (out_local_cid){ 3228 *out_local_cid = channel->local_cid; 3229 } 3230 3231 // provide buffer 3232 channel->con_handle = con_handle; 3233 channel->receive_sdu_buffer = receive_sdu_buffer; 3234 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 3235 channel->new_credits_incoming = initial_credits; 3236 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3237 3238 // add to connections list 3239 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 3240 3241 // go 3242 l2cap_run(); 3243 return 0; 3244 } 3245 3246 /** 3247 * @brief Provide credtis for LE Data Channel 3248 * @param local_cid L2CAP LE Data Channel Identifier 3249 * @param credits Number additional credits for peer 3250 */ 3251 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 3252 3253 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3254 if (!channel) { 3255 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3256 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3257 } 3258 3259 // check state 3260 if (channel->state != L2CAP_STATE_OPEN){ 3261 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 3262 } 3263 3264 // assert incoming credits + credits <= 0xffff 3265 uint32_t total_credits = channel->credits_incoming; 3266 total_credits += channel->new_credits_incoming; 3267 total_credits += credits; 3268 if (total_credits > 0xffff){ 3269 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 3270 channel->new_credits_incoming, credits); 3271 } 3272 3273 // set credits_granted 3274 channel->new_credits_incoming += credits; 3275 3276 // go 3277 l2cap_run(); 3278 return 0; 3279 } 3280 3281 /** 3282 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 3283 * @param local_cid L2CAP LE Data Channel Identifier 3284 */ 3285 int l2cap_le_can_send_now(uint16_t local_cid){ 3286 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3287 if (!channel) { 3288 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3289 return 0; 3290 } 3291 3292 // check state 3293 if (channel->state != L2CAP_STATE_OPEN) return 0; 3294 3295 // check queue 3296 if (channel->send_sdu_buffer) return 0; 3297 3298 // fine, go ahead 3299 return 1; 3300 } 3301 3302 /** 3303 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 3304 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 3305 * so packet handler should be ready to handle it 3306 * @param local_cid L2CAP LE Data Channel Identifier 3307 */ 3308 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 3309 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3310 if (!channel) { 3311 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 3312 return 0; 3313 } 3314 channel->waiting_for_can_send_now = 1; 3315 l2cap_le_notify_channel_can_send(channel); 3316 return 0; 3317 } 3318 3319 /** 3320 * @brief Send data via LE Data Channel 3321 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 3322 * @param local_cid L2CAP LE Data Channel Identifier 3323 * @param data data to send 3324 * @param size data size 3325 */ 3326 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 3327 3328 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3329 if (!channel) { 3330 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3331 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3332 } 3333 3334 if (len > channel->remote_mtu){ 3335 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 3336 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 3337 } 3338 3339 if (channel->send_sdu_buffer){ 3340 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 3341 return BTSTACK_ACL_BUFFERS_FULL; 3342 } 3343 3344 channel->send_sdu_buffer = data; 3345 channel->send_sdu_len = len; 3346 channel->send_sdu_pos = 0; 3347 3348 l2cap_run(); 3349 return 0; 3350 } 3351 3352 /** 3353 * @brief Disconnect from LE Data Channel 3354 * @param local_cid L2CAP LE Data Channel Identifier 3355 */ 3356 uint8_t l2cap_le_disconnect(uint16_t local_cid) 3357 { 3358 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3359 if (!channel) { 3360 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3361 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3362 } 3363 3364 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3365 l2cap_run(); 3366 return 0; 3367 } 3368 3369 #endif 3370