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