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