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