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