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