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