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