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