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 uint16_t l2cap_max_le_mtu(void){ 1216 if (l2cap_le_custom_max_mtu != 0) return l2cap_le_custom_max_mtu; 1217 return l2cap_max_mtu(); 1218 } 1219 1220 void l2cap_set_max_le_mtu(uint16_t max_mtu){ 1221 if (max_mtu < l2cap_max_mtu()){ 1222 l2cap_le_custom_max_mtu = max_mtu; 1223 } 1224 } 1225 1226 #ifdef ENABLE_CLASSIC 1227 1228 static uint16_t l2cap_setup_options_mtu(uint8_t * config_options, uint16_t mtu){ 1229 config_options[0] = L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT; // MTU 1230 config_options[1] = 2; // len param 1231 little_endian_store_16(config_options, 2, mtu); 1232 return 4; 1233 } 1234 1235 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1236 static int l2cap_ertm_mode(l2cap_channel_t * channel){ 1237 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1238 return ((connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_DONE) 1239 && (connection->l2cap_state.extended_feature_mask & 0x08)); 1240 } 1241 #endif 1242 1243 static uint16_t l2cap_setup_options_request(l2cap_channel_t * channel, uint8_t * config_options){ 1244 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1245 // use ERTM options if supported 1246 if (l2cap_ertm_mode(channel)){ 1247 return l2cap_setup_options_ertm_request(channel, config_options); 1248 } 1249 #endif 1250 uint16_t mtu = channel->local_mtu; 1251 return l2cap_setup_options_mtu(config_options, mtu); 1252 } 1253 1254 static uint16_t l2cap_setup_options_response(l2cap_channel_t * channel, uint8_t * config_options){ 1255 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1256 // use ERTM options if supported 1257 if (l2cap_ertm_mode(channel)){ 1258 return l2cap_setup_options_ertm_response(channel, config_options); 1259 } 1260 #endif 1261 uint16_t mtu = btstack_min(channel->local_mtu, channel->remote_mtu); 1262 return l2cap_setup_options_mtu(config_options, mtu); 1263 } 1264 1265 static uint32_t l2cap_extended_features_mask(void){ 1266 // extended features request supported, features: fixed channels, unicast connectionless data reception 1267 uint32_t features = 0x280; 1268 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1269 features |= 0x0028; 1270 #endif 1271 return features; 1272 } 1273 #endif 1274 1275 // MARK: L2CAP_RUN 1276 // process outstanding signaling tasks 1277 static void l2cap_run(void){ 1278 1279 // log_info("l2cap_run: entered"); 1280 1281 // check pending signaling responses 1282 while (signaling_responses_pending){ 1283 1284 hci_con_handle_t handle = signaling_responses[0].handle; 1285 1286 if (!hci_can_send_acl_packet_now(handle)) break; 1287 1288 uint8_t sig_id = signaling_responses[0].sig_id; 1289 uint8_t response_code = signaling_responses[0].code; 1290 uint16_t result = signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT 1291 #ifdef ENABLE_CLASSIC 1292 uint16_t info_type = signaling_responses[0].data; // INFORMATION_REQUEST 1293 uint16_t source_cid = signaling_responses[0].cid; // CONNECTION_REQUEST 1294 #endif 1295 1296 // remove first item before sending (to avoid sending response mutliple times) 1297 signaling_responses_pending--; 1298 int i; 1299 for (i=0; i < signaling_responses_pending; i++){ 1300 memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t)); 1301 } 1302 1303 switch (response_code){ 1304 #ifdef ENABLE_CLASSIC 1305 case CONNECTION_REQUEST: 1306 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 1307 // also disconnect if result is 0x0003 - security blocked 1308 if (result == 0x0003){ 1309 hci_disconnect_security_block(handle); 1310 } 1311 break; 1312 case ECHO_REQUEST: 1313 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 1314 break; 1315 case INFORMATION_REQUEST: 1316 switch (info_type){ 1317 case L2CAP_INFO_TYPE_CONNECTIONLESS_MTU: { 1318 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 1319 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(connectionless_mtu), &connectionless_mtu); 1320 } 1321 break; 1322 case L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED: { 1323 uint32_t features = l2cap_extended_features_mask(); 1324 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(features), &features); 1325 } 1326 break; 1327 case L2CAP_INFO_TYPE_FIXED_CHANNELS_SUPPORTED: { 1328 uint8_t map[8]; 1329 memset(map, 0, 8); 1330 map[0] = 0x06; // L2CAP Signaling Channel (0x02) + Connectionless reception (0x04) 1331 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(map), &map); 1332 } 1333 break; 1334 default: 1335 // all other types are not supported 1336 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 1, 0, NULL); 1337 break; 1338 } 1339 break; 1340 case COMMAND_REJECT: 1341 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1342 break; 1343 #endif 1344 #ifdef ENABLE_BLE 1345 case LE_CREDIT_BASED_CONNECTION_REQUEST: 1346 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 1347 break; 1348 case COMMAND_REJECT_LE: 1349 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1350 break; 1351 #endif 1352 default: 1353 // should not happen 1354 break; 1355 } 1356 } 1357 1358 #if defined(ENABLE_CLASSIC) || defined(ENABLE_BLE) 1359 btstack_linked_list_iterator_t it; 1360 #endif 1361 1362 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1363 // send l2cap information request if neccessary 1364 hci_connections_get_iterator(&it); 1365 while(btstack_linked_list_iterator_has_next(&it)){ 1366 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1367 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST){ 1368 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE; 1369 // send information request for extended features 1370 uint8_t sig_id = l2cap_next_sig_id(); 1371 uint8_t info_type = L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED; 1372 l2cap_send_signaling_packet(connection->con_handle, INFORMATION_REQUEST, sig_id, info_type); 1373 return; 1374 } 1375 } 1376 #endif 1377 1378 #ifdef ENABLE_CLASSIC 1379 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1380 uint8_t config_options[18]; 1381 #else 1382 uint8_t config_options[10]; 1383 #endif 1384 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1385 while (btstack_linked_list_iterator_has_next(&it)){ 1386 1387 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1388 1389 if (channel->channel_type != L2CAP_CHANNEL_TYPE_CLASSIC) continue; 1390 1391 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1392 switch (channel->state){ 1393 1394 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1395 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 1396 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1397 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 1398 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 1399 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0); 1400 } 1401 break; 1402 1403 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1404 if (!hci_can_send_command_packet_now()) break; 1405 // send connection request - set state first 1406 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 1407 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 1408 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1); 1409 break; 1410 1411 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 1412 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1413 channel->state = L2CAP_STATE_INVALID; 1414 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0); 1415 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1416 l2cap_stop_rtx(channel); 1417 btstack_linked_list_iterator_remove(&it); 1418 btstack_memory_l2cap_channel_free(channel); 1419 break; 1420 1421 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 1422 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1423 channel->state = L2CAP_STATE_CONFIG; 1424 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1425 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 1426 break; 1427 1428 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 1429 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1430 // success, start l2cap handshake 1431 channel->local_sig_id = l2cap_next_sig_id(); 1432 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 1433 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 1434 l2cap_start_rtx(channel); 1435 break; 1436 1437 case L2CAP_STATE_CONFIG: 1438 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1439 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 1440 uint16_t flags = 0; 1441 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1442 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 1443 flags = 1; 1444 } else { 1445 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1446 } 1447 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 1448 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1449 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL); 1450 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1451 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 1452 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1453 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1454 uint16_t options_size = l2cap_setup_options_response(channel, config_options); 1455 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); 1456 #endif 1457 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 1458 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1459 uint16_t options_size = l2cap_setup_options_response(channel, config_options); 1460 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); 1461 } else { 1462 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 1463 } 1464 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1465 } 1466 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 1467 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1468 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 1469 channel->local_sig_id = l2cap_next_sig_id(); 1470 uint16_t options_size = l2cap_setup_options_request(channel, config_options); 1471 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, options_size, &config_options); 1472 l2cap_start_rtx(channel); 1473 } 1474 if (l2cap_channel_ready_for_open(channel)){ 1475 channel->state = L2CAP_STATE_OPEN; 1476 l2cap_emit_channel_opened(channel, 0); // success 1477 } 1478 break; 1479 1480 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1481 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1482 channel->state = L2CAP_STATE_INVALID; 1483 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1484 // 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 :) 1485 l2cap_finialize_channel_close(channel); // -- remove from list 1486 break; 1487 1488 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1489 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1490 channel->local_sig_id = l2cap_next_sig_id(); 1491 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1492 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1493 break; 1494 default: 1495 break; 1496 } 1497 1498 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1499 // send s-frame to acknowledge received packets 1500 if (channel->con_handle == HCI_CON_HANDLE_INVALID) continue; 1501 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1502 1503 if (channel->tx_send_index != channel->tx_write_index){ 1504 // check remote tx window 1505 log_info("unacknowledged_packets %u, remote tx window size %u", channel->unacked_frames, channel->remote_tx_window_size); 1506 if (channel->unacked_frames < channel->remote_tx_window_size){ 1507 channel->unacked_frames++; 1508 int index = channel->tx_send_index; 1509 channel->tx_send_index++; 1510 if (channel->tx_send_index >= channel->num_tx_buffers){ 1511 channel->tx_send_index = 0; 1512 } 1513 l2cap_ertm_send_information_frame(channel, index, 0); // final = 0 1514 continue; 1515 } 1516 } 1517 1518 if (channel->send_supervisor_frame_receiver_ready){ 1519 channel->send_supervisor_frame_receiver_ready = 0; 1520 log_info("Send S-Frame: RR %u, final %u", channel->req_seq, channel->set_final_bit_after_packet_with_poll_bit_set); 1521 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); 1522 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1523 l2cap_ertm_send_supervisor_frame(channel, control); 1524 continue; 1525 } 1526 if (channel->send_supervisor_frame_receiver_ready_poll){ 1527 channel->send_supervisor_frame_receiver_ready_poll = 0; 1528 log_info("Send S-Frame: RR %u with poll=1 ", channel->req_seq); 1529 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 1, 0, channel->req_seq); 1530 l2cap_ertm_send_supervisor_frame(channel, control); 1531 continue; 1532 } 1533 if (channel->send_supervisor_frame_receiver_not_ready){ 1534 channel->send_supervisor_frame_receiver_not_ready = 0; 1535 log_info("Send S-Frame: RNR %u", channel->req_seq); 1536 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 1537 l2cap_ertm_send_supervisor_frame(channel, control); 1538 continue; 1539 } 1540 if (channel->send_supervisor_frame_reject){ 1541 channel->send_supervisor_frame_reject = 0; 1542 log_info("Send S-Frame: REJ %u", channel->req_seq); 1543 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT, 0, 0, channel->req_seq); 1544 l2cap_ertm_send_supervisor_frame(channel, control); 1545 continue; 1546 } 1547 if (channel->send_supervisor_frame_selective_reject){ 1548 channel->send_supervisor_frame_selective_reject = 0; 1549 log_info("Send S-Frame: SREJ %u", channel->expected_tx_seq); 1550 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); 1551 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1552 l2cap_ertm_send_supervisor_frame(channel, control); 1553 continue; 1554 } 1555 1556 if (channel->srej_active){ 1557 int i; 1558 for (i=0;i<channel->num_tx_buffers;i++){ 1559 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[i]; 1560 if (tx_state->retransmission_requested) { 1561 tx_state->retransmission_requested = 0; 1562 uint8_t final = channel->set_final_bit_after_packet_with_poll_bit_set; 1563 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1564 l2cap_ertm_send_information_frame(channel, i, final); 1565 break; 1566 } 1567 } 1568 if (i == channel->num_tx_buffers){ 1569 // no retransmission request found 1570 channel->srej_active = 0; 1571 } else { 1572 // packet was sent 1573 continue; 1574 } 1575 } 1576 #endif 1577 1578 } 1579 #endif 1580 1581 #ifdef ENABLE_LE_DATA_CHANNELS 1582 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1583 while (btstack_linked_list_iterator_has_next(&it)){ 1584 uint8_t * acl_buffer; 1585 uint8_t * l2cap_payload; 1586 uint16_t pos; 1587 uint16_t payload_size; 1588 uint16_t mps; 1589 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1590 1591 if (channel->channel_type != L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL) continue; 1592 1593 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1594 switch (channel->state){ 1595 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1596 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1597 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1598 // le psm, source cid, mtu, mps, initial credits 1599 channel->local_sig_id = l2cap_next_sig_id(); 1600 channel->credits_incoming = channel->new_credits_incoming; 1601 channel->new_credits_incoming = 0; 1602 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1603 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); 1604 break; 1605 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1606 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1607 // TODO: support larger MPS 1608 channel->state = L2CAP_STATE_OPEN; 1609 channel->credits_incoming = channel->new_credits_incoming; 1610 channel->new_credits_incoming = 0; 1611 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1612 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); 1613 // notify client 1614 l2cap_emit_le_channel_opened(channel, 0); 1615 break; 1616 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1617 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1618 channel->state = L2CAP_STATE_INVALID; 1619 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1620 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1621 l2cap_stop_rtx(channel); 1622 btstack_linked_list_iterator_remove(&it); 1623 btstack_memory_l2cap_channel_free(channel); 1624 break; 1625 case L2CAP_STATE_OPEN: 1626 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1627 1628 // send credits 1629 if (channel->new_credits_incoming){ 1630 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1631 channel->local_sig_id = l2cap_next_sig_id(); 1632 uint16_t new_credits = channel->new_credits_incoming; 1633 channel->new_credits_incoming = 0; 1634 channel->credits_incoming += new_credits; 1635 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1636 break; 1637 } 1638 1639 // send data 1640 if (!channel->send_sdu_buffer) break; 1641 if (!channel->credits_outgoing) break; 1642 1643 // send part of SDU 1644 hci_reserve_packet_buffer(); 1645 acl_buffer = hci_get_outgoing_packet_buffer(); 1646 l2cap_payload = acl_buffer + 8; 1647 pos = 0; 1648 if (!channel->send_sdu_pos){ 1649 // store SDU len 1650 channel->send_sdu_pos += 2; 1651 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 1652 pos += 2; 1653 } 1654 payload_size = btstack_min(channel->send_sdu_len + 2 - channel->send_sdu_pos, channel->remote_mps - pos); 1655 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 1656 memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len 1657 pos += payload_size; 1658 channel->send_sdu_pos += payload_size; 1659 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 1660 // done 1661 1662 channel->credits_outgoing--; 1663 1664 if (channel->send_sdu_pos >= channel->send_sdu_len + 2){ 1665 channel->send_sdu_buffer = NULL; 1666 // send done event 1667 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 1668 // inform about can send now 1669 l2cap_le_notify_channel_can_send(channel); 1670 } 1671 hci_send_acl_packet_buffer(8 + pos); 1672 break; 1673 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1674 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1675 channel->local_sig_id = l2cap_next_sig_id(); 1676 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1677 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1678 break; 1679 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1680 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1681 channel->state = L2CAP_STATE_INVALID; 1682 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1683 l2cap_le_finialize_channel_close(channel); // -- remove from list 1684 break; 1685 default: 1686 break; 1687 } 1688 } 1689 #endif 1690 1691 #ifdef ENABLE_BLE 1692 // send l2cap con paramter update if necessary 1693 hci_connections_get_iterator(&it); 1694 while(btstack_linked_list_iterator_has_next(&it)){ 1695 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1696 if (connection->address_type != BD_ADDR_TYPE_LE_PUBLIC && connection->address_type != BD_ADDR_TYPE_LE_RANDOM) continue; 1697 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1698 switch (connection->le_con_parameter_update_state){ 1699 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1700 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1701 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, connection->le_con_param_update_identifier, 1702 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1703 break; 1704 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1705 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1706 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1707 break; 1708 case CON_PARAMETER_UPDATE_DENY: 1709 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1710 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1711 break; 1712 default: 1713 break; 1714 } 1715 } 1716 #endif 1717 1718 // log_info("l2cap_run: exit"); 1719 } 1720 1721 #ifdef ENABLE_CLASSIC 1722 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1723 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 1724 log_info("l2cap_handle_connection_complete expected state"); 1725 // success, start l2cap handshake 1726 channel->con_handle = con_handle; 1727 // check remote SSP feature first 1728 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1729 } 1730 } 1731 1732 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1733 1734 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1735 // assumption: outgoing connection 1736 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1737 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1738 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1739 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1740 return; 1741 } 1742 #endif 1743 1744 // fine, go ahead 1745 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1746 } 1747 1748 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1749 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1750 1751 // we have been waiting for remote supported features, if both support SSP, 1752 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)); 1753 if (gap_ssp_supported_on_both_sides(channel->con_handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){ 1754 // request security level 2 1755 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 1756 channel->required_security_level = LEVEL_2; 1757 gap_request_security_level(channel->con_handle, LEVEL_2); 1758 return; 1759 } 1760 1761 l2cap_ready_to_connect(channel); 1762 } 1763 #endif 1764 1765 #ifdef L2CAP_USES_CHANNELS 1766 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, 1767 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 1768 1769 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 1770 if (!channel) { 1771 return NULL; 1772 } 1773 1774 // Init memory (make valgrind happy) 1775 memset(channel, 0, sizeof(l2cap_channel_t)); 1776 1777 // fill in 1778 channel->packet_handler = packet_handler; 1779 channel->channel_type = channel_type; 1780 bd_addr_copy(channel->address, address); 1781 channel->address_type = address_type; 1782 channel->psm = psm; 1783 channel->local_mtu = local_mtu; 1784 channel->remote_mtu = L2CAP_DEFAULT_MTU; 1785 channel->required_security_level = security_level; 1786 1787 // 1788 channel->local_cid = l2cap_next_local_cid(); 1789 channel->con_handle = HCI_CON_HANDLE_INVALID; 1790 1791 // set initial state 1792 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 1793 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 1794 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 1795 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 1796 1797 return channel; 1798 } 1799 #endif 1800 1801 #ifdef ENABLE_CLASSIC 1802 1803 /** 1804 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 1805 * @param packet_handler 1806 * @param address 1807 * @param psm 1808 * @param mtu 1809 * @param local_cid 1810 */ 1811 1812 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){ 1813 // limit MTU to the size of our outtgoing HCI buffer 1814 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 1815 1816 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x mtu %u -> local mtu %u", bd_addr_to_str(address), psm, mtu, local_mtu); 1817 1818 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); 1819 if (!channel) { 1820 return BTSTACK_MEMORY_ALLOC_FAILED; 1821 } 1822 1823 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1824 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1825 #endif 1826 1827 // add to connections list 1828 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1829 1830 // store local_cid 1831 if (out_local_cid){ 1832 *out_local_cid = channel->local_cid; 1833 } 1834 1835 // check if hci connection is already usable 1836 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1837 if (conn){ 1838 log_info("l2cap_create_channel, hci connection already exists"); 1839 l2cap_handle_connection_complete(conn->con_handle, channel); 1840 // check if remote supported fearures are already received 1841 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1842 l2cap_handle_remote_supported_features_received(channel); 1843 } 1844 } 1845 1846 l2cap_run(); 1847 1848 return 0; 1849 } 1850 1851 void 1852 l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 1853 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 1854 // find channel for local_cid 1855 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1856 if (channel) { 1857 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1858 } 1859 // process 1860 l2cap_run(); 1861 } 1862 1863 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 1864 // mark all channels before emitting open events as these could trigger new connetion requests to the same device 1865 btstack_linked_list_iterator_t it; 1866 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1867 while (btstack_linked_list_iterator_has_next(&it)){ 1868 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1869 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 1870 if (bd_addr_cmp( channel->address, address) != 0) continue; 1871 // channel for this address found 1872 switch (channel->state){ 1873 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 1874 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1875 channel->state = L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD; 1876 break; 1877 default: 1878 break; 1879 } 1880 } 1881 // emit and free marked entries. restart loop to deal with list changes 1882 int done = 0; 1883 while (!done) { 1884 done = 1; 1885 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1886 while (btstack_linked_list_iterator_has_next(&it)){ 1887 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1888 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 1889 if (channel->state == L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD){ 1890 done = 0; 1891 // failure, forward error code 1892 l2cap_emit_channel_opened(channel, status); 1893 // discard channel 1894 l2cap_stop_rtx(channel); 1895 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1896 btstack_memory_l2cap_channel_free(channel); 1897 break; 1898 } 1899 } 1900 } 1901 1902 } 1903 1904 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 1905 btstack_linked_list_iterator_t it; 1906 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1907 while (btstack_linked_list_iterator_has_next(&it)){ 1908 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1909 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 1910 if ( ! bd_addr_cmp( channel->address, address) ){ 1911 l2cap_handle_connection_complete(handle, channel); 1912 } 1913 } 1914 // process 1915 l2cap_run(); 1916 } 1917 #endif 1918 1919 static void l2cap_notify_channel_can_send(void){ 1920 int done = 0; 1921 while (!done){ 1922 done = 1; 1923 btstack_linked_list_iterator_t it; 1924 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1925 while (btstack_linked_list_iterator_has_next(&it)){ 1926 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1927 if (!channel->waiting_for_can_send_now) continue; 1928 int can_send = 0; 1929 if (l2cap_is_le_channel_type(channel->channel_type)){ 1930 #ifdef ENABLE_BLE 1931 can_send = hci_can_send_acl_le_packet_now(); 1932 #endif 1933 } else { 1934 #ifdef ENABLE_CLASSIC 1935 can_send = hci_can_send_acl_classic_packet_now(); 1936 #endif 1937 } 1938 if (!can_send) continue; 1939 // requeue for fairness 1940 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1941 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 1942 // emit can send 1943 channel->waiting_for_can_send_now = 0; 1944 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 1945 // exit inner loop as we just broke the iterator, but try again 1946 done = 0; 1947 break; 1948 } 1949 } 1950 } 1951 1952 #ifdef L2CAP_USES_CHANNELS 1953 1954 static int l2cap_send_open_failed_on_hci_disconnect(l2cap_channel_t * channel){ 1955 // open cannot fail for for incoming connections 1956 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) return 0; 1957 1958 // check state 1959 switch (channel->state){ 1960 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1961 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 1962 case L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES: 1963 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 1964 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 1965 case L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES: 1966 case L2CAP_STATE_WAIT_CONNECT_RSP: 1967 case L2CAP_STATE_CONFIG: 1968 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 1969 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1970 case L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE: 1971 case L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD: 1972 return 1; 1973 1974 case L2CAP_STATE_OPEN: 1975 case L2CAP_STATE_CLOSED: 1976 case L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES: 1977 case L2CAP_STATE_WAIT_DISCONNECT: 1978 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_INSUFFICIENT_SECURITY: 1979 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 1980 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 1981 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1982 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1983 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1984 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1985 case L2CAP_STATE_INVALID: 1986 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1987 return 0; 1988 // no default here, to get a warning about new states 1989 } 1990 // still, the compiler insists on a return value 1991 return 0; 1992 } 1993 #endif 1994 1995 #ifdef ENABLE_CLASSIC 1996 static void l2cap_handle_hci_disconnect_event(l2cap_channel_t * channel){ 1997 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 1998 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 1999 } else { 2000 l2cap_emit_channel_closed(channel); 2001 } 2002 btstack_memory_l2cap_channel_free(channel); 2003 } 2004 #endif 2005 2006 #ifdef ENABLE_LE_DATA_CHANNELS 2007 static void l2cap_handle_hci_le_disconnect_event(l2cap_channel_t * channel){ 2008 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2009 l2cap_emit_le_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2010 } else { 2011 l2cap_emit_le_channel_closed(channel); 2012 } 2013 btstack_memory_l2cap_channel_free(channel); 2014 } 2015 #endif 2016 2017 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 2018 2019 UNUSED(packet_type); // ok: registered with hci_event_callback_registration 2020 UNUSED(cid); // ok: there is no channel 2021 UNUSED(size); // ok: fixed format events read from HCI buffer 2022 2023 #ifdef ENABLE_CLASSIC 2024 bd_addr_t address; 2025 int hci_con_used; 2026 #endif 2027 #ifdef L2CAP_USES_CHANNELS 2028 hci_con_handle_t handle; 2029 btstack_linked_list_iterator_t it; 2030 #endif 2031 2032 switch(hci_event_packet_get_type(packet)){ 2033 2034 // Notify channel packet handler if they can send now 2035 case HCI_EVENT_TRANSPORT_PACKET_SENT: 2036 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 2037 l2cap_run(); // try sending signaling packets first 2038 l2cap_notify_channel_can_send(); 2039 break; 2040 2041 case HCI_EVENT_COMMAND_STATUS: 2042 l2cap_run(); // try sending signaling packets first 2043 break; 2044 2045 #ifdef ENABLE_CLASSIC 2046 // handle connection complete events 2047 case HCI_EVENT_CONNECTION_COMPLETE: 2048 reverse_bd_addr(&packet[5], address); 2049 if (packet[2] == 0){ 2050 handle = little_endian_read_16(packet, 3); 2051 l2cap_handle_connection_success_for_addr(address, handle); 2052 } else { 2053 l2cap_handle_connection_failed_for_addr(address, packet[2]); 2054 } 2055 break; 2056 2057 // handle successful create connection cancel command 2058 case HCI_EVENT_COMMAND_COMPLETE: 2059 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 2060 if (packet[5] == 0){ 2061 reverse_bd_addr(&packet[6], address); 2062 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 2063 l2cap_handle_connection_failed_for_addr(address, 0x16); 2064 } 2065 } 2066 l2cap_run(); // try sending signaling packets first 2067 break; 2068 #endif 2069 2070 #ifdef L2CAP_USES_CHANNELS 2071 // handle disconnection complete events 2072 case HCI_EVENT_DISCONNECTION_COMPLETE: 2073 handle = little_endian_read_16(packet, 3); 2074 // send l2cap open failed or closed events for all channels on this handle and free them 2075 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2076 while (btstack_linked_list_iterator_has_next(&it)){ 2077 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2078 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2079 if (channel->con_handle != handle) continue; 2080 btstack_linked_list_iterator_remove(&it); 2081 switch(channel->channel_type){ 2082 #ifdef ENABLE_CLASSIC 2083 case L2CAP_CHANNEL_TYPE_CLASSIC: 2084 l2cap_stop_rtx(channel); 2085 l2cap_handle_hci_disconnect_event(channel); 2086 break; 2087 #endif 2088 #ifdef ENABLE_LE_DATA_CHANNELS 2089 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2090 l2cap_handle_hci_le_disconnect_event(channel); 2091 break; 2092 #endif 2093 default: 2094 break; 2095 } 2096 } 2097 #endif 2098 2099 // HCI Connection Timeouts 2100 #ifdef ENABLE_CLASSIC 2101 case L2CAP_EVENT_TIMEOUT_CHECK: 2102 handle = little_endian_read_16(packet, 2); 2103 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 2104 if (hci_authentication_active_for_handle(handle)) break; 2105 hci_con_used = 0; 2106 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2107 while (btstack_linked_list_iterator_has_next(&it)){ 2108 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2109 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2110 if (channel->con_handle != handle) continue; 2111 hci_con_used = 1; 2112 break; 2113 } 2114 if (hci_con_used) break; 2115 if (!hci_can_send_command_packet_now()) break; 2116 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 2117 break; 2118 2119 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 2120 handle = little_endian_read_16(packet, 3); 2121 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2122 while (btstack_linked_list_iterator_has_next(&it)){ 2123 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2124 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2125 if (channel->con_handle != handle) continue; 2126 l2cap_handle_remote_supported_features_received(channel); 2127 break; 2128 } 2129 break; 2130 2131 case GAP_EVENT_SECURITY_LEVEL: 2132 handle = little_endian_read_16(packet, 2); 2133 log_info("l2cap - security level update"); 2134 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2135 while (btstack_linked_list_iterator_has_next(&it)){ 2136 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2137 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2138 if (channel->con_handle != handle) continue; 2139 2140 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 2141 gap_security_level_t required_level = channel->required_security_level; 2142 2143 log_info("channel state %u: actual %u >= required %u?", channel->state, actual_level, required_level); 2144 2145 switch (channel->state){ 2146 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2147 if (actual_level >= required_level){ 2148 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2149 // we need to know if ERTM is supported before sending a config response 2150 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 2151 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2152 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 2153 #else 2154 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2155 l2cap_emit_incoming_connection(channel); 2156 #endif 2157 } else { 2158 channel->reason = 0x0003; // security block 2159 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2160 } 2161 break; 2162 2163 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2164 if (actual_level >= required_level){ 2165 l2cap_ready_to_connect(channel); 2166 } else { 2167 // disconnnect, authentication not good enough 2168 hci_disconnect_security_block(handle); 2169 } 2170 break; 2171 2172 default: 2173 break; 2174 } 2175 } 2176 break; 2177 #endif 2178 2179 default: 2180 break; 2181 } 2182 2183 l2cap_run(); 2184 } 2185 2186 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 2187 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 2188 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 2189 signaling_responses[signaling_responses_pending].handle = handle; 2190 signaling_responses[signaling_responses_pending].code = code; 2191 signaling_responses[signaling_responses_pending].sig_id = sig_id; 2192 signaling_responses[signaling_responses_pending].cid = cid; 2193 signaling_responses[signaling_responses_pending].data = data; 2194 signaling_responses_pending++; 2195 l2cap_run(); 2196 } 2197 } 2198 2199 #ifdef ENABLE_CLASSIC 2200 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 2201 channel->remote_sig_id = identifier; 2202 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2203 l2cap_run(); 2204 } 2205 2206 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 2207 2208 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 2209 l2cap_service_t *service = l2cap_get_service(psm); 2210 if (!service) { 2211 // 0x0002 PSM not supported 2212 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2213 return; 2214 } 2215 2216 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 2217 if (!hci_connection) { 2218 // 2219 log_error("no hci_connection for handle %u", handle); 2220 return; 2221 } 2222 2223 // alloc structure 2224 // log_info("l2cap_handle_connection_request register channel"); 2225 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CLASSIC, hci_connection->address, BD_ADDR_TYPE_CLASSIC, 2226 psm, service->mtu, service->required_security_level); 2227 if (!channel){ 2228 // 0x0004 No resources available 2229 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2230 return; 2231 } 2232 2233 channel->con_handle = handle; 2234 channel->remote_cid = source_cid; 2235 channel->remote_sig_id = sig_id; 2236 2237 // limit local mtu to max acl packet length - l2cap header 2238 if (channel->local_mtu > l2cap_max_mtu()) { 2239 channel->local_mtu = l2cap_max_mtu(); 2240 } 2241 2242 // set initial state 2243 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 2244 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 2245 2246 // add to connections list 2247 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 2248 2249 // assert security requirements 2250 gap_request_security_level(handle, channel->required_security_level); 2251 } 2252 2253 void l2cap_accept_connection(uint16_t local_cid){ 2254 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 2255 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2256 if (!channel) { 2257 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 2258 return; 2259 } 2260 2261 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2262 // configure L2CAP Basic mode 2263 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2264 #endif 2265 2266 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 2267 2268 // process 2269 l2cap_run(); 2270 } 2271 2272 void l2cap_decline_connection(uint16_t local_cid){ 2273 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 2274 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 2275 if (!channel) { 2276 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 2277 return; 2278 } 2279 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2280 channel->reason = 0x04; // no resources available 2281 l2cap_run(); 2282 } 2283 2284 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2285 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 2286 2287 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2288 uint8_t use_fcs = 1; 2289 #endif 2290 2291 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2292 2293 uint16_t flags = little_endian_read_16(command, 6); 2294 if (flags & 1) { 2295 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 2296 } 2297 2298 // accept the other's configuration options 2299 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2300 uint16_t pos = 8; 2301 while (pos < end_pos){ 2302 uint8_t option_hint = command[pos] >> 7; 2303 uint8_t option_type = command[pos] & 0x7f; 2304 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2305 pos++; 2306 uint8_t length = command[pos++]; 2307 // MTU { type(8): 1, len(8):2, MTU(16) } 2308 if (option_type == L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT && length == 2){ 2309 channel->remote_mtu = little_endian_read_16(command, pos); 2310 log_info("Remote MTU %u", channel->remote_mtu); 2311 if (channel->remote_mtu > l2cap_max_mtu()){ 2312 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 2313 channel->remote_mtu = l2cap_max_mtu(); 2314 } 2315 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2316 } 2317 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2318 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FLUSH_TIMEOUT && length == 2){ 2319 channel->flush_timeout = little_endian_read_16(command, pos); 2320 log_info("Flush timeout: %u ms", channel->flush_timeout); 2321 } 2322 2323 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2324 // Retransmission and Flow Control Option 2325 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2326 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2327 switch(channel->mode){ 2328 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2329 // Store remote config 2330 channel->remote_tx_window_size = command[pos+1]; 2331 channel->remote_max_transmit = command[pos+2]; 2332 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2333 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2334 channel->remote_mps = little_endian_read_16(command, pos + 7); 2335 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2336 channel->remote_tx_window_size, 2337 channel->remote_max_transmit, 2338 channel->remote_retransmission_timeout_ms, 2339 channel->remote_monitor_timeout_ms, 2340 channel->remote_mps); 2341 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2342 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2343 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2344 } else { 2345 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2346 } 2347 break; 2348 case L2CAP_CHANNEL_MODE_BASIC: 2349 switch (mode){ 2350 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2351 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2352 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2353 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2354 } 2355 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2356 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2357 break; 2358 default: // case L2CAP_CHANNEL_MODE_BASIC: 2359 // TODO store and evaluate configuration 2360 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2361 break; 2362 } 2363 break; 2364 default: 2365 break; 2366 } 2367 } 2368 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE && length == 1){ 2369 use_fcs = command[pos]; 2370 } 2371 #endif 2372 // check for unknown options 2373 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2374 log_info("l2cap cid %u, unknown options", channel->local_cid); 2375 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2376 } 2377 pos += length; 2378 } 2379 2380 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2381 // "FCS" has precedence over "No FCS" 2382 uint8_t update = channel->fcs_option || use_fcs; 2383 log_info("local fcs: %u, remote fcs: %u -> %u", channel->fcs_option, use_fcs, update); 2384 channel->fcs_option = update; 2385 #endif 2386 } 2387 2388 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2389 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2390 log_info("l2cap_signaling_handle_configure_response"); 2391 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2392 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2393 uint16_t pos = 10; 2394 while (pos < end_pos){ 2395 uint8_t option_hint = command[pos] >> 7; 2396 uint8_t option_type = command[pos] & 0x7f; 2397 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2398 pos++; 2399 uint8_t length = command[pos++]; 2400 2401 // Retransmission and Flow Control Option 2402 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2403 switch (channel->mode){ 2404 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2405 if (channel->ertm_mandatory){ 2406 // ?? 2407 } else { 2408 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2409 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2410 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2411 } 2412 } 2413 break; 2414 case L2CAP_CHANNEL_MODE_BASIC: 2415 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2416 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2417 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2418 } 2419 break; 2420 default: 2421 break; 2422 } 2423 } 2424 2425 // check for unknown options 2426 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2427 log_info("l2cap cid %u, unknown options", channel->local_cid); 2428 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2429 } 2430 2431 pos += length; 2432 } 2433 #else 2434 UNUSED(channel); // ok: no code 2435 UNUSED(result); // ok: no code 2436 UNUSED(command); // ok: no code 2437 #endif 2438 } 2439 2440 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2441 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2442 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2443 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2444 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2445 if (channel->state == L2CAP_STATE_OPEN) return 0; 2446 return 1; 2447 } 2448 2449 2450 // @pre command len is valid, see check in l2cap_signaling_handler_dispatch 2451 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2452 2453 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2454 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2455 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2456 uint16_t result = 0; 2457 2458 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2459 2460 // handle DISCONNECT REQUESTS seperately 2461 if (code == DISCONNECTION_REQUEST){ 2462 switch (channel->state){ 2463 case L2CAP_STATE_CONFIG: 2464 case L2CAP_STATE_OPEN: 2465 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2466 case L2CAP_STATE_WAIT_DISCONNECT: 2467 l2cap_handle_disconnect_request(channel, identifier); 2468 break; 2469 2470 default: 2471 // ignore in other states 2472 break; 2473 } 2474 return; 2475 } 2476 2477 // @STATEMACHINE(l2cap) 2478 switch (channel->state) { 2479 2480 case L2CAP_STATE_WAIT_CONNECT_RSP: 2481 switch (code){ 2482 case CONNECTION_RESPONSE: 2483 if (cmd_len < 8){ 2484 // command imcomplete 2485 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2486 break; 2487 } 2488 l2cap_stop_rtx(channel); 2489 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2490 switch (result) { 2491 case 0: 2492 // successful connection 2493 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2494 channel->state = L2CAP_STATE_CONFIG; 2495 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2496 break; 2497 case 1: 2498 // connection pending. get some coffee, but start the ERTX 2499 l2cap_start_ertx(channel); 2500 break; 2501 default: 2502 // channel closed 2503 channel->state = L2CAP_STATE_CLOSED; 2504 // map l2cap connection response result to BTstack status enumeration 2505 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2506 2507 // drop link key if security block 2508 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2509 gap_drop_link_key_for_bd_addr(channel->address); 2510 } 2511 2512 // discard channel 2513 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2514 btstack_memory_l2cap_channel_free(channel); 2515 break; 2516 } 2517 break; 2518 2519 default: 2520 //@TODO: implement other signaling packets 2521 break; 2522 } 2523 break; 2524 2525 case L2CAP_STATE_CONFIG: 2526 switch (code) { 2527 case CONFIGURE_REQUEST: 2528 if (cmd_len < 4){ 2529 // command incomplete 2530 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2531 break; 2532 } 2533 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 2534 l2cap_signaling_handle_configure_request(channel, command); 2535 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 2536 // only done if continuation not set 2537 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 2538 } 2539 break; 2540 case CONFIGURE_RESPONSE: 2541 if (cmd_len < 6){ 2542 // command incomplete 2543 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2544 break; 2545 } 2546 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2547 l2cap_stop_rtx(channel); 2548 l2cap_signaling_handle_configure_response(channel, result, command); 2549 switch (result){ 2550 case 0: // success 2551 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 2552 break; 2553 case 4: // pending 2554 l2cap_start_ertx(channel); 2555 break; 2556 default: 2557 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2558 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 2559 // remote does not offer ertm but it's required 2560 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2561 break; 2562 } 2563 #endif 2564 // retry on negative result 2565 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2566 break; 2567 } 2568 break; 2569 default: 2570 break; 2571 } 2572 if (l2cap_channel_ready_for_open(channel)){ 2573 // for open: 2574 channel->state = L2CAP_STATE_OPEN; 2575 l2cap_emit_channel_opened(channel, 0); 2576 } 2577 break; 2578 2579 case L2CAP_STATE_WAIT_DISCONNECT: 2580 switch (code) { 2581 case DISCONNECTION_RESPONSE: 2582 l2cap_finialize_channel_close(channel); 2583 break; 2584 default: 2585 //@TODO: implement other signaling packets 2586 break; 2587 } 2588 break; 2589 2590 case L2CAP_STATE_CLOSED: 2591 // @TODO handle incoming requests 2592 break; 2593 2594 case L2CAP_STATE_OPEN: 2595 //@TODO: implement other signaling packets, e.g. re-configure 2596 break; 2597 default: 2598 break; 2599 } 2600 // log_info("new state %u", channel->state); 2601 } 2602 2603 2604 // @pre command len is valid, see check in l2cap_acl_classic_handler 2605 static void l2cap_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command){ 2606 2607 btstack_linked_list_iterator_t it; 2608 2609 // get code, signalind identifier and command len 2610 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2611 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2612 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2613 2614 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 2615 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_RESPONSE){ 2616 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2617 return; 2618 } 2619 2620 // general commands without an assigned channel 2621 switch(code) { 2622 2623 case CONNECTION_REQUEST: 2624 if (cmd_len == 4){ 2625 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2626 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2627 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 2628 } else { 2629 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2630 } 2631 return; 2632 2633 case ECHO_REQUEST: 2634 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 2635 return; 2636 2637 case INFORMATION_REQUEST: 2638 if (cmd_len == 2) { 2639 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2640 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 2641 } else { 2642 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2643 } 2644 return; 2645 2646 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2647 case INFORMATION_RESPONSE: { 2648 hci_connection_t * connection = hci_connection_for_handle(handle); 2649 if (!connection) return; 2650 if (cmd_len >= 4) { 2651 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2652 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2653 if (result != 0) return; 2654 if (info_type != L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) return; 2655 if (cmd_len >= 6) { 2656 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2657 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2658 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2659 // trigger connection request 2660 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2661 while (btstack_linked_list_iterator_has_next(&it)){ 2662 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2663 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2664 if (channel->con_handle != handle) continue; 2665 // bail if ERTM was requested but is not supported 2666 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2667 if (channel->ertm_mandatory){ 2668 // channel closed 2669 channel->state = L2CAP_STATE_CLOSED; 2670 // map l2cap connection response result to BTstack status enumeration 2671 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 2672 // discard channel 2673 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2674 btstack_memory_l2cap_channel_free(channel); 2675 continue; 2676 } else { 2677 // fallback to Basic mode 2678 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2679 } 2680 } 2681 // start connecting 2682 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2683 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 2684 } 2685 // respond to connection request 2686 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2687 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2688 l2cap_emit_incoming_connection(channel); 2689 } 2690 } 2691 return; // cmd len valid 2692 } 2693 } 2694 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2695 return; 2696 } 2697 #endif 2698 2699 default: 2700 break; 2701 } 2702 2703 // Get potential destination CID 2704 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2705 2706 // Find channel for this sig_id and connection handle 2707 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2708 while (btstack_linked_list_iterator_has_next(&it)){ 2709 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2710 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2711 if (channel->con_handle != handle) continue; 2712 if (code & 1) { 2713 // match odd commands (responses) by previous signaling identifier 2714 if (channel->local_sig_id == sig_id) { 2715 l2cap_signaling_handler_channel(channel, command); 2716 break; 2717 } 2718 } else { 2719 // match even commands (requests) by local channel id 2720 if (channel->local_cid == dest_cid) { 2721 l2cap_signaling_handler_channel(channel, command); 2722 break; 2723 } 2724 } 2725 } 2726 } 2727 #endif 2728 2729 #ifdef ENABLE_BLE 2730 2731 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 2732 uint8_t event[6]; 2733 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 2734 event[1] = 4; 2735 little_endian_store_16(event, 2, con_handle); 2736 little_endian_store_16(event, 4, result); 2737 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2738 if (!l2cap_event_packet_handler) return; 2739 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 2740 } 2741 2742 // @returns valid 2743 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 2744 hci_connection_t * connection; 2745 uint16_t result; 2746 uint8_t event[10]; 2747 2748 #ifdef ENABLE_LE_DATA_CHANNELS 2749 btstack_linked_list_iterator_t it; 2750 l2cap_channel_t * channel; 2751 uint16_t local_cid; 2752 uint16_t le_psm; 2753 uint16_t new_credits; 2754 uint16_t credits_before; 2755 l2cap_service_t * service; 2756 uint16_t source_cid; 2757 #endif 2758 2759 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2760 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2761 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 2762 2763 switch (code){ 2764 2765 case CONNECTION_PARAMETER_UPDATE_REQUEST: 2766 // check size 2767 if (len < 8) return 0; 2768 connection = hci_connection_for_handle(handle); 2769 if (connection){ 2770 if (connection->role != HCI_ROLE_MASTER){ 2771 // reject command without notifying upper layer when not in master role 2772 return 0; 2773 } 2774 le_connection_parameter_range_t existing_range; 2775 gap_get_connection_parameter_range(&existing_range); 2776 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2777 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2778 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2779 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 2780 2781 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 2782 if (update_parameter){ 2783 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 2784 connection->le_conn_interval_min = le_conn_interval_min; 2785 connection->le_conn_interval_max = le_conn_interval_max; 2786 connection->le_conn_latency = le_conn_latency; 2787 connection->le_supervision_timeout = le_supervision_timeout; 2788 } else { 2789 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 2790 } 2791 connection->le_con_param_update_identifier = sig_id; 2792 } 2793 2794 if (!l2cap_event_packet_handler) break; 2795 2796 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 2797 event[1] = 8; 2798 memcpy(&event[2], &command[4], 8); 2799 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2800 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2801 break; 2802 2803 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 2804 // check size 2805 if (len < 2) return 0; 2806 result = little_endian_read_16(command, 4); 2807 l2cap_emit_connection_parameter_update_response(handle, result); 2808 break; 2809 2810 #ifdef ENABLE_LE_DATA_CHANNELS 2811 2812 case COMMAND_REJECT: 2813 // Find channel for this sig_id and connection handle 2814 channel = NULL; 2815 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2816 while (btstack_linked_list_iterator_has_next(&it)){ 2817 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2818 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 2819 if (a_channel->con_handle != handle) continue; 2820 if (a_channel->local_sig_id != sig_id) continue; 2821 channel = a_channel; 2822 break; 2823 } 2824 if (!channel) break; 2825 2826 // if received while waiting for le connection response, assume legacy device 2827 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 2828 channel->state = L2CAP_STATE_CLOSED; 2829 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 2830 l2cap_emit_le_channel_opened(channel, 0x0002); 2831 2832 // discard channel 2833 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2834 btstack_memory_l2cap_channel_free(channel); 2835 break; 2836 } 2837 break; 2838 2839 case LE_CREDIT_BASED_CONNECTION_REQUEST: 2840 // check size 2841 if (len < 10) return 0; 2842 2843 // get hci connection, bail if not found (must not happen) 2844 connection = hci_connection_for_handle(handle); 2845 if (!connection) return 0; 2846 2847 // check if service registered 2848 le_psm = little_endian_read_16(command, 4); 2849 service = l2cap_le_get_service(le_psm); 2850 source_cid = little_endian_read_16(command, 6); 2851 2852 if (service){ 2853 if (source_cid < 0x40){ 2854 // 0x0009 Connection refused - Invalid Source CID 2855 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 2856 return 1; 2857 } 2858 2859 // go through list of channels for this ACL connection and check if we get a match 2860 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2861 while (btstack_linked_list_iterator_has_next(&it)){ 2862 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2863 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 2864 if (a_channel->con_handle != handle) continue; 2865 if (a_channel->remote_cid != source_cid) continue; 2866 // 0x000a Connection refused - Source CID already allocated 2867 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 2868 return 1; 2869 } 2870 2871 // security: check encryption 2872 if (service->required_security_level >= LEVEL_2){ 2873 if (gap_encryption_key_size(handle) == 0){ 2874 // 0x0008 Connection refused - insufficient encryption 2875 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 2876 return 1; 2877 } 2878 // anything less than 16 byte key size is insufficient 2879 if (gap_encryption_key_size(handle) < 16){ 2880 // 0x0007 Connection refused – insufficient encryption key size 2881 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 2882 return 1; 2883 } 2884 } 2885 2886 // security: check authencation 2887 if (service->required_security_level >= LEVEL_3){ 2888 if (!gap_authenticated(handle)){ 2889 // 0x0005 Connection refused – insufficient authentication 2890 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 2891 return 1; 2892 } 2893 } 2894 2895 // security: check authorization 2896 if (service->required_security_level >= LEVEL_4){ 2897 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 2898 // 0x0006 Connection refused – insufficient authorization 2899 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 2900 return 1; 2901 } 2902 } 2903 2904 // allocate channel 2905 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 2906 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 2907 if (!channel){ 2908 // 0x0004 Connection refused – no resources available 2909 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2910 return 1; 2911 } 2912 2913 channel->con_handle = handle; 2914 channel->remote_cid = source_cid; 2915 channel->remote_sig_id = sig_id; 2916 channel->remote_mtu = little_endian_read_16(command, 8); 2917 channel->remote_mps = little_endian_read_16(command, 10); 2918 channel->credits_outgoing = little_endian_read_16(command, 12); 2919 2920 // set initial state 2921 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2922 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 2923 2924 // add to connections list 2925 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 2926 2927 // post connection request event 2928 l2cap_emit_le_incoming_connection(channel); 2929 2930 } else { 2931 // Connection refused – LE_PSM not supported 2932 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2933 } 2934 break; 2935 2936 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 2937 // check size 2938 if (len < 10) return 0; 2939 2940 // Find channel for this sig_id and connection handle 2941 channel = NULL; 2942 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2943 while (btstack_linked_list_iterator_has_next(&it)){ 2944 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2945 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 2946 if (a_channel->con_handle != handle) continue; 2947 if (a_channel->local_sig_id != sig_id) continue; 2948 channel = a_channel; 2949 break; 2950 } 2951 if (!channel) break; 2952 2953 // cid + 0 2954 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 2955 if (result){ 2956 channel->state = L2CAP_STATE_CLOSED; 2957 // map l2cap connection response result to BTstack status enumeration 2958 l2cap_emit_le_channel_opened(channel, result); 2959 2960 // discard channel 2961 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2962 btstack_memory_l2cap_channel_free(channel); 2963 break; 2964 } 2965 2966 // success 2967 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2968 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2969 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 2970 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 2971 channel->state = L2CAP_STATE_OPEN; 2972 l2cap_emit_le_channel_opened(channel, result); 2973 break; 2974 2975 case LE_FLOW_CONTROL_CREDIT: 2976 // check size 2977 if (len < 4) return 0; 2978 2979 // find channel 2980 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2981 channel = l2cap_get_channel_for_local_cid(local_cid); 2982 if (!channel) { 2983 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2984 break; 2985 } 2986 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2987 credits_before = channel->credits_outgoing; 2988 channel->credits_outgoing += new_credits; 2989 // check for credit overrun 2990 if (credits_before > channel->credits_outgoing){ 2991 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 2992 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2993 break; 2994 } 2995 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 2996 break; 2997 2998 case DISCONNECTION_REQUEST: 2999 3000 // check size 3001 if (len < 4) return 0; 3002 3003 // find channel 3004 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3005 channel = l2cap_get_channel_for_local_cid(local_cid); 3006 if (!channel) { 3007 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3008 break; 3009 } 3010 channel->remote_sig_id = sig_id; 3011 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3012 break; 3013 3014 #endif 3015 3016 case DISCONNECTION_RESPONSE: 3017 break; 3018 3019 default: 3020 // command unknown -> reject command 3021 return 0; 3022 } 3023 return 1; 3024 } 3025 #endif 3026 3027 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3028 #ifdef ENABLE_CLASSIC 3029 l2cap_channel_t * l2cap_channel; 3030 l2cap_fixed_channel_t * l2cap_fixed_channel; 3031 3032 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3033 switch (channel_id) { 3034 3035 case L2CAP_CID_SIGNALING: { 3036 uint16_t command_offset = 8; 3037 while (command_offset < size) { 3038 // assert signaling command is fully inside packet 3039 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3040 uint32_t next_command_offset = ((uint32_t) command_offset) + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3041 if (next_command_offset > size){ 3042 log_error("l2cap signaling command len invalid -> drop"); 3043 break; 3044 } 3045 // handle signaling command 3046 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3047 // go to next command 3048 command_offset = (uint16_t) next_command_offset; 3049 } 3050 break; 3051 } 3052 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3053 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3054 if (!l2cap_fixed_channel) break; 3055 if (!l2cap_fixed_channel->packet_handler) break; 3056 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3057 break; 3058 3059 default: 3060 // Find channel for this channel_id and connection handle 3061 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 3062 if (l2cap_channel) { 3063 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3064 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3065 3066 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3067 3068 // assert control + FCS fields are inside 3069 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) break; 3070 3071 if (l2cap_channel->fcs_option){ 3072 // verify FCS (required if one side requested it) 3073 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3074 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3075 if (fcs_calculated == fcs_packet){ 3076 log_info("Packet FCS 0x%04x verified", fcs_packet); 3077 } else { 3078 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3079 // TODO: trigger retransmission or something like that 3080 break; 3081 } 3082 } 3083 3084 // switch on packet type 3085 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3086 uint8_t req_seq = (control >> 8) & 0x3f; 3087 int final = (control >> 7) & 0x01; 3088 if (control & 1){ 3089 // S-Frame 3090 int poll = (control >> 4) & 0x01; 3091 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3092 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3093 l2cap_ertm_tx_packet_state_t * tx_state; 3094 switch (s){ 3095 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3096 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3097 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3098 if (poll && final){ 3099 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3100 log_error("P=F=1 in S-Frame"); 3101 break; 3102 } 3103 if (poll){ 3104 // check if we did request selective retransmission before <==> we have stored SDU segments 3105 int i; 3106 int num_stored_out_of_order_packets = 0; 3107 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3108 int index = l2cap_channel->rx_store_index + i; 3109 if (index >= l2cap_channel->num_rx_buffers){ 3110 index -= l2cap_channel->num_rx_buffers; 3111 } 3112 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3113 if (!rx_state->valid) continue; 3114 num_stored_out_of_order_packets++; 3115 } 3116 if (num_stored_out_of_order_packets){ 3117 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3118 } else { 3119 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3120 } 3121 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3122 } 3123 if (final){ 3124 // Stop-MonitorTimer 3125 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3126 // If UnackedFrames > 0 then Start-RetransTimer 3127 if (l2cap_channel->unacked_frames){ 3128 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3129 } 3130 3131 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3132 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 3133 } 3134 break; 3135 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3136 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3137 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3138 // rsetart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3139 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 3140 break; 3141 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3142 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3143 break; 3144 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3145 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3146 if (poll){ 3147 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3148 } 3149 // find requested i-frame 3150 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3151 if (tx_state){ 3152 log_info("Retransmission for tx_seq %u requested", req_seq); 3153 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3154 tx_state->retransmission_requested = 1; 3155 l2cap_channel->srej_active = 1; 3156 } 3157 break; 3158 default: 3159 break; 3160 } 3161 break; 3162 } else { 3163 // I-Frame 3164 // get control 3165 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3166 uint8_t tx_seq = (control >> 1) & 0x3f; 3167 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3168 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3169 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3170 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3171 if (final){ 3172 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3173 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 3174 } 3175 3176 // get SDU 3177 const uint8_t * sdu_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3178 uint16_t sdu_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3179 3180 // assert SDU size is smaller or equal to our buffers 3181 if (sdu_len > l2cap_channel->local_mps) break; 3182 3183 // check ordering 3184 if (l2cap_channel->expected_tx_seq == tx_seq){ 3185 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3186 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3187 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3188 3189 // process SDU 3190 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, sdu_data, sdu_len); 3191 3192 // process stored segments 3193 while (1){ 3194 int index = l2cap_channel->rx_store_index; 3195 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3196 if (!rx_state->valid) break; 3197 3198 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3199 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3200 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3201 3202 rx_state->valid = 0; 3203 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3204 3205 // update rx store index 3206 index++; 3207 if (index >= l2cap_channel->num_rx_buffers){ 3208 index = 0; 3209 } 3210 l2cap_channel->rx_store_index = index; 3211 } 3212 3213 // 3214 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3215 3216 } else { 3217 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3218 if (delta < 2){ 3219 // store segment 3220 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, sdu_data, sdu_len); 3221 3222 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3223 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3224 } else { 3225 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3226 l2cap_channel->send_supervisor_frame_reject = 1; 3227 } 3228 } 3229 } 3230 break; 3231 } 3232 #endif 3233 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3234 } 3235 break; 3236 } 3237 #else 3238 UNUSED(handle); // ok: no code 3239 UNUSED(packet); // ok: no code 3240 UNUSED(size); // ok: no code 3241 #endif 3242 } 3243 3244 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3245 #ifdef ENABLE_BLE 3246 3247 l2cap_fixed_channel_t * l2cap_fixed_channel; 3248 3249 #ifdef ENABLE_LE_DATA_CHANNELS 3250 l2cap_channel_t * l2cap_channel; 3251 #endif 3252 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3253 switch (channel_id) { 3254 3255 case L2CAP_CID_SIGNALING_LE: { 3256 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3257 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3258 if (COMPLETE_L2CAP_HEADER + 4 + len > size) break; 3259 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3260 if (!valid){ 3261 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3262 } 3263 break; 3264 } 3265 3266 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3267 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3268 if (!l2cap_fixed_channel) break; 3269 if (!l2cap_fixed_channel->packet_handler) break; 3270 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3271 break; 3272 3273 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3274 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3275 if (!l2cap_fixed_channel) break; 3276 if (!l2cap_fixed_channel->packet_handler) break; 3277 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3278 break; 3279 3280 default: 3281 3282 #ifdef ENABLE_LE_DATA_CHANNELS 3283 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 3284 if (l2cap_channel) { 3285 // credit counting 3286 if (l2cap_channel->credits_incoming == 0){ 3287 log_error("LE Data Channel packet received but no incoming credits"); 3288 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3289 break; 3290 } 3291 l2cap_channel->credits_incoming--; 3292 3293 // automatic credits 3294 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 3295 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3296 } 3297 3298 // first fragment 3299 uint16_t pos = 0; 3300 if (!l2cap_channel->receive_sdu_len){ 3301 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3302 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3303 l2cap_channel->receive_sdu_len = sdu_len; 3304 l2cap_channel->receive_sdu_pos = 0; 3305 pos += 2; 3306 size -= 2; 3307 } 3308 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3309 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3310 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3311 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], fragment_size); 3312 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3313 // done? 3314 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3315 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3316 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3317 l2cap_channel->receive_sdu_len = 0; 3318 } 3319 } else { 3320 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 3321 } 3322 #endif 3323 break; 3324 } 3325 #else 3326 UNUSED(handle); // ok: no code 3327 UNUSED(packet); // ok: no code 3328 UNUSED(size); // ok: no code 3329 #endif 3330 } 3331 3332 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3333 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3334 UNUSED(channel); // ok: there is no channel 3335 3336 // Assert full L2CAP header present 3337 if (size < COMPLETE_L2CAP_HEADER) return; 3338 3339 // Dispatch to Classic or LE handler 3340 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3341 hci_connection_t *conn = hci_connection_for_handle(handle); 3342 if (!conn) return; 3343 if (conn->address_type == BD_ADDR_TYPE_CLASSIC){ 3344 l2cap_acl_classic_handler(handle, packet, size); 3345 } else { 3346 l2cap_acl_le_handler(handle, packet, size); 3347 } 3348 3349 l2cap_run(); 3350 } 3351 3352 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3353 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 3354 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3355 if (!channel) return; 3356 channel->packet_handler = the_packet_handler; 3357 } 3358 3359 #ifdef ENABLE_CLASSIC 3360 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3361 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3362 channel->state = L2CAP_STATE_CLOSED; 3363 l2cap_emit_channel_closed(channel); 3364 // discard channel 3365 l2cap_stop_rtx(channel); 3366 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3367 btstack_memory_l2cap_channel_free(channel); 3368 } 3369 #endif 3370 3371 #ifdef L2CAP_USES_CHANNELS 3372 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3373 btstack_linked_list_iterator_t it; 3374 btstack_linked_list_iterator_init(&it, services); 3375 while (btstack_linked_list_iterator_has_next(&it)){ 3376 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3377 if ( service->psm == psm){ 3378 return service; 3379 }; 3380 } 3381 return NULL; 3382 } 3383 #endif 3384 3385 #ifdef ENABLE_CLASSIC 3386 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3387 return l2cap_get_service_internal(&l2cap_services, psm); 3388 } 3389 3390 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3391 3392 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3393 3394 // check for alread registered psm 3395 l2cap_service_t *service = l2cap_get_service(psm); 3396 if (service) { 3397 log_error("l2cap_register_service: PSM %u already registered", psm); 3398 return L2CAP_SERVICE_ALREADY_REGISTERED; 3399 } 3400 3401 // alloc structure 3402 service = btstack_memory_l2cap_service_get(); 3403 if (!service) { 3404 log_error("l2cap_register_service: no memory for l2cap_service_t"); 3405 return BTSTACK_MEMORY_ALLOC_FAILED; 3406 } 3407 3408 // fill in 3409 service->psm = psm; 3410 service->mtu = mtu; 3411 service->packet_handler = service_packet_handler; 3412 service->required_security_level = security_level; 3413 3414 // add to services list 3415 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3416 3417 // enable page scan 3418 gap_connectable_control(1); 3419 3420 return 0; 3421 } 3422 3423 uint8_t l2cap_unregister_service(uint16_t psm){ 3424 3425 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 3426 3427 l2cap_service_t *service = l2cap_get_service(psm); 3428 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3429 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 3430 btstack_memory_l2cap_service_free(service); 3431 3432 // disable page scan when no services registered 3433 if (btstack_linked_list_empty(&l2cap_services)) { 3434 gap_connectable_control(0); 3435 } 3436 return 0; 3437 } 3438 #endif 3439 3440 3441 #ifdef ENABLE_LE_DATA_CHANNELS 3442 3443 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 3444 if (!channel->waiting_for_can_send_now) return; 3445 if (channel->send_sdu_buffer) return; 3446 channel->waiting_for_can_send_now = 0; 3447 log_debug("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 3448 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 3449 } 3450 3451 // 1BH2222 3452 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 3453 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", 3454 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 3455 uint8_t event[19]; 3456 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 3457 event[1] = sizeof(event) - 2; 3458 event[2] = channel->address_type; 3459 reverse_bd_addr(channel->address, &event[3]); 3460 little_endian_store_16(event, 9, channel->con_handle); 3461 little_endian_store_16(event, 11, channel->psm); 3462 little_endian_store_16(event, 13, channel->local_cid); 3463 little_endian_store_16(event, 15, channel->remote_cid); 3464 little_endian_store_16(event, 17, channel->remote_mtu); 3465 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3466 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3467 } 3468 // 11BH22222 3469 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 3470 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", 3471 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 3472 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 3473 uint8_t event[23]; 3474 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 3475 event[1] = sizeof(event) - 2; 3476 event[2] = status; 3477 event[3] = channel->address_type; 3478 reverse_bd_addr(channel->address, &event[4]); 3479 little_endian_store_16(event, 10, channel->con_handle); 3480 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 3481 little_endian_store_16(event, 13, channel->psm); 3482 little_endian_store_16(event, 15, channel->local_cid); 3483 little_endian_store_16(event, 17, channel->remote_cid); 3484 little_endian_store_16(event, 19, channel->local_mtu); 3485 little_endian_store_16(event, 21, channel->remote_mtu); 3486 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3487 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3488 } 3489 // 2 3490 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 3491 log_info("L2CAP_EVENT_LE_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 3492 uint8_t event[4]; 3493 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 3494 event[1] = sizeof(event) - 2; 3495 little_endian_store_16(event, 2, channel->local_cid); 3496 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3497 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3498 } 3499 3500 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3501 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 3502 channel->state = L2CAP_STATE_CLOSED; 3503 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 3504 // discard channel 3505 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3506 btstack_memory_l2cap_channel_free(channel); 3507 } 3508 3509 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 3510 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 3511 } 3512 3513 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 3514 3515 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 3516 3517 // check for alread registered psm 3518 l2cap_service_t *service = l2cap_le_get_service(psm); 3519 if (service) { 3520 return L2CAP_SERVICE_ALREADY_REGISTERED; 3521 } 3522 3523 // alloc structure 3524 service = btstack_memory_l2cap_service_get(); 3525 if (!service) { 3526 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 3527 return BTSTACK_MEMORY_ALLOC_FAILED; 3528 } 3529 3530 // fill in 3531 service->psm = psm; 3532 service->mtu = 0; 3533 service->packet_handler = packet_handler; 3534 service->required_security_level = security_level; 3535 3536 // add to services list 3537 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 3538 3539 // done 3540 return 0; 3541 } 3542 3543 uint8_t l2cap_le_unregister_service(uint16_t psm) { 3544 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 3545 l2cap_service_t *service = l2cap_le_get_service(psm); 3546 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3547 3548 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 3549 btstack_memory_l2cap_service_free(service); 3550 return 0; 3551 } 3552 3553 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 3554 // get channel 3555 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3556 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3557 3558 // validate state 3559 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3560 return ERROR_CODE_COMMAND_DISALLOWED; 3561 } 3562 3563 // set state accept connection 3564 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 3565 channel->receive_sdu_buffer = receive_sdu_buffer; 3566 channel->local_mtu = mtu; 3567 channel->new_credits_incoming = initial_credits; 3568 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3569 3570 // test 3571 // channel->new_credits_incoming = 1; 3572 3573 // go 3574 l2cap_run(); 3575 return 0; 3576 } 3577 3578 /** 3579 * @brief Deny incoming LE Data Channel connection due to resource constraints 3580 * @param local_cid L2CAP LE Data Channel Identifier 3581 */ 3582 3583 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 3584 // get channel 3585 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3586 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3587 3588 // validate state 3589 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3590 return ERROR_CODE_COMMAND_DISALLOWED; 3591 } 3592 3593 // set state decline connection 3594 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 3595 channel->reason = 0x04; // no resources available 3596 l2cap_run(); 3597 return 0; 3598 } 3599 3600 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 3601 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 3602 uint16_t * out_local_cid) { 3603 3604 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 3605 3606 3607 hci_connection_t * connection = hci_connection_for_handle(con_handle); 3608 if (!connection) { 3609 log_error("no hci_connection for handle 0x%04x", con_handle); 3610 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 3611 } 3612 3613 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); 3614 if (!channel) { 3615 return BTSTACK_MEMORY_ALLOC_FAILED; 3616 } 3617 log_info("l2cap_le_create_channel %p", channel); 3618 3619 // store local_cid 3620 if (out_local_cid){ 3621 *out_local_cid = channel->local_cid; 3622 } 3623 3624 // provide buffer 3625 channel->con_handle = con_handle; 3626 channel->receive_sdu_buffer = receive_sdu_buffer; 3627 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 3628 channel->new_credits_incoming = initial_credits; 3629 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3630 3631 // add to connections list 3632 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 3633 3634 // go 3635 l2cap_run(); 3636 return 0; 3637 } 3638 3639 /** 3640 * @brief Provide credtis for LE Data Channel 3641 * @param local_cid L2CAP LE Data Channel Identifier 3642 * @param credits Number additional credits for peer 3643 */ 3644 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 3645 3646 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3647 if (!channel) { 3648 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3649 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3650 } 3651 3652 // check state 3653 if (channel->state != L2CAP_STATE_OPEN){ 3654 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 3655 } 3656 3657 // assert incoming credits + credits <= 0xffff 3658 uint32_t total_credits = channel->credits_incoming; 3659 total_credits += channel->new_credits_incoming; 3660 total_credits += credits; 3661 if (total_credits > 0xffff){ 3662 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 3663 channel->new_credits_incoming, credits); 3664 } 3665 3666 // set credits_granted 3667 channel->new_credits_incoming += credits; 3668 3669 // go 3670 l2cap_run(); 3671 return 0; 3672 } 3673 3674 /** 3675 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 3676 * @param local_cid L2CAP LE Data Channel Identifier 3677 */ 3678 int l2cap_le_can_send_now(uint16_t local_cid){ 3679 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3680 if (!channel) { 3681 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3682 return 0; 3683 } 3684 3685 // check state 3686 if (channel->state != L2CAP_STATE_OPEN) return 0; 3687 3688 // check queue 3689 if (channel->send_sdu_buffer) return 0; 3690 3691 // fine, go ahead 3692 return 1; 3693 } 3694 3695 /** 3696 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 3697 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 3698 * so packet handler should be ready to handle it 3699 * @param local_cid L2CAP LE Data Channel Identifier 3700 */ 3701 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 3702 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3703 if (!channel) { 3704 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 3705 return 0; 3706 } 3707 channel->waiting_for_can_send_now = 1; 3708 l2cap_le_notify_channel_can_send(channel); 3709 return 0; 3710 } 3711 3712 /** 3713 * @brief Send data via LE Data Channel 3714 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 3715 * @param local_cid L2CAP LE Data Channel Identifier 3716 * @param data data to send 3717 * @param size data size 3718 */ 3719 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 3720 3721 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3722 if (!channel) { 3723 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3724 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3725 } 3726 3727 if (len > channel->remote_mtu){ 3728 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 3729 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 3730 } 3731 3732 if (channel->send_sdu_buffer){ 3733 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 3734 return BTSTACK_ACL_BUFFERS_FULL; 3735 } 3736 3737 channel->send_sdu_buffer = data; 3738 channel->send_sdu_len = len; 3739 channel->send_sdu_pos = 0; 3740 3741 l2cap_run(); 3742 return 0; 3743 } 3744 3745 /** 3746 * @brief Disconnect from LE Data Channel 3747 * @param local_cid L2CAP LE Data Channel Identifier 3748 */ 3749 uint8_t l2cap_le_disconnect(uint16_t local_cid) 3750 { 3751 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3752 if (!channel) { 3753 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3754 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3755 } 3756 3757 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3758 l2cap_run(); 3759 return 0; 3760 } 3761 3762 #endif 3763