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