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 btstack_linked_list_iterator_remove(&it); 1471 l2cap_free_channel_entry(channel); 1472 break; 1473 1474 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 1475 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1476 channel->state = L2CAP_STATE_CONFIG; 1477 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1478 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 1479 break; 1480 1481 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 1482 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1483 // success, start l2cap handshake 1484 channel->local_sig_id = l2cap_next_sig_id(); 1485 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 1486 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 1487 l2cap_start_rtx(channel); 1488 break; 1489 1490 case L2CAP_STATE_CONFIG: 1491 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1492 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1493 // fallback to basic mode if ERTM requested but not not supported by remote 1494 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1495 if (!l2cap_ertm_mode(channel)){ 1496 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 1497 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1498 } 1499 } 1500 #endif 1501 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 1502 uint16_t flags = 0; 1503 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1504 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 1505 flags = 1; 1506 } else { 1507 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1508 } 1509 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 1510 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1511 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL); 1512 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1513 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 1514 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1515 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1516 uint16_t options_size = l2cap_setup_options_response(channel, config_options); 1517 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); 1518 #endif 1519 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 1520 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1521 uint16_t options_size = l2cap_setup_options_response(channel, config_options); 1522 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); 1523 } else { 1524 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 1525 } 1526 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1527 } 1528 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 1529 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1530 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 1531 channel->local_sig_id = l2cap_next_sig_id(); 1532 uint16_t options_size = l2cap_setup_options_request(channel, config_options); 1533 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, options_size, &config_options); 1534 l2cap_start_rtx(channel); 1535 } 1536 if (l2cap_channel_ready_for_open(channel)){ 1537 channel->state = L2CAP_STATE_OPEN; 1538 l2cap_emit_channel_opened(channel, 0); // success 1539 } 1540 break; 1541 1542 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1543 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1544 channel->state = L2CAP_STATE_INVALID; 1545 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1546 // 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 :) 1547 l2cap_finialize_channel_close(channel); // -- remove from list 1548 channel = NULL; 1549 break; 1550 1551 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1552 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1553 channel->local_sig_id = l2cap_next_sig_id(); 1554 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1555 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1556 break; 1557 default: 1558 break; 1559 } 1560 1561 1562 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1563 1564 // handle channel finalize on L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE 1565 if (!channel) continue; 1566 1567 // ERTM mode 1568 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1569 1570 // check if we can still send 1571 if (channel->con_handle == HCI_CON_HANDLE_INVALID) continue; 1572 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1573 1574 // send if we have more data and remote windows isn't full yet 1575 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); 1576 if (channel->unacked_frames < btstack_min(channel->num_stored_tx_frames, channel->remote_tx_window_size)){ 1577 channel->unacked_frames++; 1578 int index = channel->tx_send_index; 1579 channel->tx_send_index++; 1580 if (channel->tx_send_index >= channel->num_tx_buffers){ 1581 channel->tx_send_index = 0; 1582 } 1583 l2cap_ertm_send_information_frame(channel, index, 0); // final = 0 1584 continue; 1585 } 1586 1587 if (channel->send_supervisor_frame_receiver_ready){ 1588 channel->send_supervisor_frame_receiver_ready = 0; 1589 log_info("Send S-Frame: RR %u, final %u", channel->req_seq, channel->set_final_bit_after_packet_with_poll_bit_set); 1590 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); 1591 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1592 l2cap_ertm_send_supervisor_frame(channel, control); 1593 continue; 1594 } 1595 if (channel->send_supervisor_frame_receiver_ready_poll){ 1596 channel->send_supervisor_frame_receiver_ready_poll = 0; 1597 log_info("Send S-Frame: RR %u with poll=1 ", channel->req_seq); 1598 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 1, 0, channel->req_seq); 1599 l2cap_ertm_send_supervisor_frame(channel, control); 1600 continue; 1601 } 1602 if (channel->send_supervisor_frame_receiver_not_ready){ 1603 channel->send_supervisor_frame_receiver_not_ready = 0; 1604 log_info("Send S-Frame: RNR %u", channel->req_seq); 1605 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 1606 l2cap_ertm_send_supervisor_frame(channel, control); 1607 continue; 1608 } 1609 if (channel->send_supervisor_frame_reject){ 1610 channel->send_supervisor_frame_reject = 0; 1611 log_info("Send S-Frame: REJ %u", channel->req_seq); 1612 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT, 0, 0, channel->req_seq); 1613 l2cap_ertm_send_supervisor_frame(channel, control); 1614 continue; 1615 } 1616 if (channel->send_supervisor_frame_selective_reject){ 1617 channel->send_supervisor_frame_selective_reject = 0; 1618 log_info("Send S-Frame: SREJ %u", channel->expected_tx_seq); 1619 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); 1620 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1621 l2cap_ertm_send_supervisor_frame(channel, control); 1622 continue; 1623 } 1624 1625 if (channel->srej_active){ 1626 int i; 1627 for (i=0;i<channel->num_tx_buffers;i++){ 1628 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[i]; 1629 if (tx_state->retransmission_requested) { 1630 tx_state->retransmission_requested = 0; 1631 uint8_t final = channel->set_final_bit_after_packet_with_poll_bit_set; 1632 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1633 l2cap_ertm_send_information_frame(channel, i, final); 1634 break; 1635 } 1636 } 1637 if (i == channel->num_tx_buffers){ 1638 // no retransmission request found 1639 channel->srej_active = 0; 1640 } else { 1641 // packet was sent 1642 continue; 1643 } 1644 } 1645 } 1646 #endif 1647 1648 } 1649 #endif 1650 1651 #ifdef ENABLE_LE_DATA_CHANNELS 1652 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1653 while (btstack_linked_list_iterator_has_next(&it)){ 1654 uint8_t * acl_buffer; 1655 uint8_t * l2cap_payload; 1656 uint16_t pos; 1657 uint16_t payload_size; 1658 uint16_t mps; 1659 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1660 1661 if (channel->channel_type != L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL) continue; 1662 1663 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1664 switch (channel->state){ 1665 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1666 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1667 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1668 // le psm, source cid, mtu, mps, initial credits 1669 channel->local_sig_id = l2cap_next_sig_id(); 1670 channel->credits_incoming = channel->new_credits_incoming; 1671 channel->new_credits_incoming = 0; 1672 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1673 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); 1674 break; 1675 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1676 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1677 // TODO: support larger MPS 1678 channel->state = L2CAP_STATE_OPEN; 1679 channel->credits_incoming = channel->new_credits_incoming; 1680 channel->new_credits_incoming = 0; 1681 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1682 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); 1683 // notify client 1684 l2cap_emit_le_channel_opened(channel, 0); 1685 break; 1686 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1687 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1688 channel->state = L2CAP_STATE_INVALID; 1689 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1690 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1691 btstack_linked_list_iterator_remove(&it); 1692 l2cap_free_channel_entry(channel); 1693 break; 1694 case L2CAP_STATE_OPEN: 1695 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1696 1697 // send credits 1698 if (channel->new_credits_incoming){ 1699 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1700 channel->local_sig_id = l2cap_next_sig_id(); 1701 uint16_t new_credits = channel->new_credits_incoming; 1702 channel->new_credits_incoming = 0; 1703 channel->credits_incoming += new_credits; 1704 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1705 break; 1706 } 1707 1708 // send data 1709 if (!channel->send_sdu_buffer) break; 1710 if (!channel->credits_outgoing) break; 1711 1712 // send part of SDU 1713 hci_reserve_packet_buffer(); 1714 acl_buffer = hci_get_outgoing_packet_buffer(); 1715 l2cap_payload = acl_buffer + 8; 1716 pos = 0; 1717 if (!channel->send_sdu_pos){ 1718 // store SDU len 1719 channel->send_sdu_pos += 2; 1720 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 1721 pos += 2; 1722 } 1723 payload_size = btstack_min(channel->send_sdu_len + 2 - channel->send_sdu_pos, channel->remote_mps - pos); 1724 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 1725 memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len 1726 pos += payload_size; 1727 channel->send_sdu_pos += payload_size; 1728 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 1729 // done 1730 1731 channel->credits_outgoing--; 1732 1733 if (channel->send_sdu_pos >= channel->send_sdu_len + 2){ 1734 channel->send_sdu_buffer = NULL; 1735 // send done event 1736 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 1737 // inform about can send now 1738 l2cap_le_notify_channel_can_send(channel); 1739 } 1740 hci_send_acl_packet_buffer(8 + pos); 1741 break; 1742 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1743 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1744 channel->local_sig_id = l2cap_next_sig_id(); 1745 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1746 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1747 break; 1748 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1749 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1750 channel->state = L2CAP_STATE_INVALID; 1751 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1752 l2cap_le_finialize_channel_close(channel); // -- remove from list 1753 break; 1754 default: 1755 break; 1756 } 1757 } 1758 #endif 1759 1760 #ifdef ENABLE_BLE 1761 // send l2cap con paramter update if necessary 1762 hci_connections_get_iterator(&it); 1763 while(btstack_linked_list_iterator_has_next(&it)){ 1764 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1765 if (connection->address_type != BD_ADDR_TYPE_LE_PUBLIC && connection->address_type != BD_ADDR_TYPE_LE_RANDOM) continue; 1766 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1767 switch (connection->le_con_parameter_update_state){ 1768 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1769 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1770 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, l2cap_next_sig_id(), 1771 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1772 break; 1773 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1774 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1775 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1776 break; 1777 case CON_PARAMETER_UPDATE_DENY: 1778 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1779 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1780 break; 1781 default: 1782 break; 1783 } 1784 } 1785 #endif 1786 1787 // log_info("l2cap_run: exit"); 1788 } 1789 1790 #ifdef ENABLE_CLASSIC 1791 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1792 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 1793 log_info("connection complete con_handle %04x - for channel %p cid 0x%04x", (int) con_handle, channel, channel->local_cid); 1794 // success, start l2cap handshake 1795 channel->con_handle = con_handle; 1796 // check remote SSP feature first 1797 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1798 } 1799 } 1800 1801 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1802 1803 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1804 // assumption: outgoing connection 1805 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1806 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1807 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1808 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1809 return; 1810 } 1811 #endif 1812 1813 // fine, go ahead 1814 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1815 } 1816 1817 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1818 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1819 1820 // we have been waiting for remote supported features, if both support SSP, 1821 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)); 1822 if (gap_ssp_supported_on_both_sides(channel->con_handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){ 1823 // request security level 2 1824 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 1825 channel->required_security_level = LEVEL_2; 1826 gap_request_security_level(channel->con_handle, LEVEL_2); 1827 return; 1828 } 1829 1830 l2cap_ready_to_connect(channel); 1831 } 1832 #endif 1833 1834 #ifdef L2CAP_USES_CHANNELS 1835 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, 1836 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 1837 1838 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 1839 if (!channel) { 1840 return NULL; 1841 } 1842 1843 // fill in 1844 channel->packet_handler = packet_handler; 1845 channel->channel_type = channel_type; 1846 bd_addr_copy(channel->address, address); 1847 channel->address_type = address_type; 1848 channel->psm = psm; 1849 channel->local_mtu = local_mtu; 1850 channel->remote_mtu = L2CAP_DEFAULT_MTU; 1851 channel->required_security_level = security_level; 1852 1853 // 1854 channel->local_cid = l2cap_next_local_cid(); 1855 channel->con_handle = HCI_CON_HANDLE_INVALID; 1856 1857 // set initial state 1858 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 1859 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 1860 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 1861 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 1862 1863 log_info("create channel %p, local_cid 0x%04x", channel, channel->local_cid); 1864 1865 return channel; 1866 } 1867 1868 static void l2cap_free_channel_entry(l2cap_channel_t * channel){ 1869 log_info("free channel %p, local_cid 0x%04x", channel, channel->local_cid); 1870 // assert all timers are stopped 1871 l2cap_stop_rtx(channel); 1872 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1873 l2cap_ertm_stop_retransmission_timer(channel); 1874 l2cap_ertm_stop_monitor_timer(channel); 1875 #endif 1876 // free memory 1877 btstack_memory_l2cap_channel_free(channel); 1878 } 1879 #endif 1880 1881 #ifdef ENABLE_CLASSIC 1882 1883 /** 1884 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 1885 * @param packet_handler 1886 * @param address 1887 * @param psm 1888 * @param mtu 1889 * @param local_cid 1890 */ 1891 1892 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){ 1893 // limit MTU to the size of our outtgoing HCI buffer 1894 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 1895 1896 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x mtu %u -> local mtu %u", bd_addr_to_str(address), psm, mtu, local_mtu); 1897 1898 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); 1899 if (!channel) { 1900 return BTSTACK_MEMORY_ALLOC_FAILED; 1901 } 1902 1903 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1904 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1905 #endif 1906 1907 // add to connections list 1908 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1909 1910 // store local_cid 1911 if (out_local_cid){ 1912 *out_local_cid = channel->local_cid; 1913 } 1914 1915 // check if hci connection is already usable 1916 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1917 if (conn){ 1918 log_info("l2cap_create_channel, hci connection 0x%04x already exists", conn->con_handle); 1919 l2cap_handle_connection_complete(conn->con_handle, channel); 1920 // check if remote supported fearures are already received 1921 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1922 l2cap_handle_remote_supported_features_received(channel); 1923 } 1924 } 1925 1926 l2cap_run(); 1927 1928 return 0; 1929 } 1930 1931 void l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 1932 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 1933 // find channel for local_cid 1934 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1935 if (channel) { 1936 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1937 } 1938 // process 1939 l2cap_run(); 1940 } 1941 1942 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 1943 // mark all channels before emitting open events as these could trigger new connetion requests to the same device 1944 btstack_linked_list_iterator_t it; 1945 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1946 while (btstack_linked_list_iterator_has_next(&it)){ 1947 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1948 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 1949 if (bd_addr_cmp( channel->address, address) != 0) continue; 1950 // channel for this address found 1951 switch (channel->state){ 1952 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 1953 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1954 channel->state = L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD; 1955 break; 1956 default: 1957 break; 1958 } 1959 } 1960 // emit and free marked entries. restart loop to deal with list changes 1961 int done = 0; 1962 while (!done) { 1963 done = 1; 1964 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1965 while (btstack_linked_list_iterator_has_next(&it)){ 1966 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1967 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 1968 if (channel->state == L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD){ 1969 done = 0; 1970 // failure, forward error code 1971 l2cap_handle_channel_open_failed(channel, status); 1972 // discard channel 1973 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1974 l2cap_free_channel_entry(channel); 1975 break; 1976 } 1977 } 1978 } 1979 1980 } 1981 1982 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 1983 btstack_linked_list_iterator_t it; 1984 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1985 while (btstack_linked_list_iterator_has_next(&it)){ 1986 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1987 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 1988 if ( ! bd_addr_cmp( channel->address, address) ){ 1989 l2cap_handle_connection_complete(handle, channel); 1990 } 1991 } 1992 // process 1993 l2cap_run(); 1994 } 1995 #endif 1996 1997 static void l2cap_notify_channel_can_send(void){ 1998 int done = 0; 1999 while (!done){ 2000 done = 1; 2001 btstack_linked_list_iterator_t it; 2002 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2003 while (btstack_linked_list_iterator_has_next(&it)){ 2004 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2005 if (!channel->waiting_for_can_send_now) continue; 2006 int can_send = 0; 2007 if (l2cap_is_le_channel_type(channel->channel_type)){ 2008 #ifdef ENABLE_BLE 2009 can_send = hci_can_send_acl_le_packet_now(); 2010 #endif 2011 } else { 2012 #ifdef ENABLE_CLASSIC 2013 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2014 // skip ertm channels as they only depend on free buffers in storage 2015 if (channel->mode == L2CAP_CHANNEL_MODE_BASIC){ 2016 can_send = hci_can_send_acl_classic_packet_now(); 2017 } 2018 #else 2019 can_send = hci_can_send_acl_classic_packet_now(); 2020 #endif /* ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE */ 2021 #endif /* ENABLE_CLASSIC */ 2022 } 2023 if (!can_send) continue; 2024 // requeue for fairness 2025 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2026 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2027 // emit can send 2028 channel->waiting_for_can_send_now = 0; 2029 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2030 // exit inner loop as we just broke the iterator, but try again 2031 done = 0; 2032 break; 2033 } 2034 } 2035 } 2036 2037 #ifdef L2CAP_USES_CHANNELS 2038 2039 static int l2cap_send_open_failed_on_hci_disconnect(l2cap_channel_t * channel){ 2040 // open cannot fail for for incoming connections 2041 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) return 0; 2042 2043 // check state 2044 switch (channel->state){ 2045 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2046 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2047 case L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES: 2048 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2049 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 2050 case L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES: 2051 case L2CAP_STATE_WAIT_CONNECT_RSP: 2052 case L2CAP_STATE_CONFIG: 2053 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 2054 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 2055 case L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE: 2056 case L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD: 2057 return 1; 2058 2059 case L2CAP_STATE_OPEN: 2060 case L2CAP_STATE_CLOSED: 2061 case L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES: 2062 case L2CAP_STATE_WAIT_DISCONNECT: 2063 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_INSUFFICIENT_SECURITY: 2064 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 2065 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 2066 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2067 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 2068 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 2069 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 2070 case L2CAP_STATE_INVALID: 2071 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2072 return 0; 2073 // no default here, to get a warning about new states 2074 } 2075 // still, the compiler insists on a return value 2076 return 0; 2077 } 2078 #endif 2079 2080 #ifdef ENABLE_CLASSIC 2081 static void l2cap_handle_hci_disconnect_event(l2cap_channel_t * channel){ 2082 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2083 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2084 } else { 2085 l2cap_handle_channel_closed(channel); 2086 } 2087 l2cap_free_channel_entry(channel); 2088 } 2089 #endif 2090 2091 #ifdef ENABLE_LE_DATA_CHANNELS 2092 static void l2cap_handle_hci_le_disconnect_event(l2cap_channel_t * channel){ 2093 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2094 l2cap_emit_le_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2095 } else { 2096 l2cap_emit_le_channel_closed(channel); 2097 } 2098 l2cap_free_channel_entry(channel); 2099 } 2100 #endif 2101 2102 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 2103 2104 UNUSED(packet_type); // ok: registered with hci_event_callback_registration 2105 UNUSED(cid); // ok: there is no channel 2106 UNUSED(size); // ok: fixed format events read from HCI buffer 2107 2108 #ifdef ENABLE_CLASSIC 2109 bd_addr_t address; 2110 int hci_con_used; 2111 #endif 2112 #ifdef L2CAP_USES_CHANNELS 2113 hci_con_handle_t handle; 2114 btstack_linked_list_iterator_t it; 2115 #endif 2116 2117 switch(hci_event_packet_get_type(packet)){ 2118 2119 // Notify channel packet handler if they can send now 2120 case HCI_EVENT_TRANSPORT_PACKET_SENT: 2121 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 2122 case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED: 2123 l2cap_run(); // try sending signaling packets first 2124 l2cap_notify_channel_can_send(); 2125 break; 2126 2127 case HCI_EVENT_COMMAND_STATUS: 2128 #ifdef ENABLE_CLASSIC 2129 // check command status for create connection for errors 2130 if (HCI_EVENT_IS_COMMAND_STATUS(packet, hci_create_connection)){ 2131 // cache outgoing address and reset 2132 memcpy(address, l2cap_outgoing_classic_addr, 6); 2133 memset(l2cap_outgoing_classic_addr, 0, 6); 2134 // error => outgoing connection failed 2135 uint8_t status = hci_event_command_status_get_status(packet); 2136 if (status){ 2137 l2cap_handle_connection_failed_for_addr(address, status); 2138 } 2139 } 2140 #endif 2141 l2cap_run(); // try sending signaling packets first 2142 break; 2143 2144 #ifdef ENABLE_CLASSIC 2145 // handle connection complete events 2146 case HCI_EVENT_CONNECTION_COMPLETE: 2147 reverse_bd_addr(&packet[5], address); 2148 if (packet[2] == 0){ 2149 handle = little_endian_read_16(packet, 3); 2150 l2cap_handle_connection_success_for_addr(address, handle); 2151 } else { 2152 l2cap_handle_connection_failed_for_addr(address, packet[2]); 2153 } 2154 break; 2155 2156 // handle successful create connection cancel command 2157 case HCI_EVENT_COMMAND_COMPLETE: 2158 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 2159 if (packet[5] == 0){ 2160 reverse_bd_addr(&packet[6], address); 2161 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 2162 l2cap_handle_connection_failed_for_addr(address, 0x16); 2163 } 2164 } 2165 l2cap_run(); // try sending signaling packets first 2166 break; 2167 #endif 2168 2169 #ifdef L2CAP_USES_CHANNELS 2170 // handle disconnection complete events 2171 case HCI_EVENT_DISCONNECTION_COMPLETE: 2172 handle = little_endian_read_16(packet, 3); 2173 // send l2cap open failed or closed events for all channels on this handle and free them 2174 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2175 while (btstack_linked_list_iterator_has_next(&it)){ 2176 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2177 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2178 if (channel->con_handle != handle) continue; 2179 btstack_linked_list_iterator_remove(&it); 2180 switch(channel->channel_type){ 2181 #ifdef ENABLE_CLASSIC 2182 case L2CAP_CHANNEL_TYPE_CLASSIC: 2183 l2cap_handle_hci_disconnect_event(channel); 2184 break; 2185 #endif 2186 #ifdef ENABLE_LE_DATA_CHANNELS 2187 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2188 l2cap_handle_hci_le_disconnect_event(channel); 2189 break; 2190 #endif 2191 default: 2192 break; 2193 } 2194 } 2195 break; 2196 #endif 2197 2198 2199 // HCI Connection Timeouts 2200 #ifdef ENABLE_CLASSIC 2201 case L2CAP_EVENT_TIMEOUT_CHECK: 2202 handle = little_endian_read_16(packet, 2); 2203 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 2204 if (hci_authentication_active_for_handle(handle)) break; 2205 hci_con_used = 0; 2206 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2207 while (btstack_linked_list_iterator_has_next(&it)){ 2208 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2209 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2210 if (channel->con_handle != handle) continue; 2211 hci_con_used = 1; 2212 break; 2213 } 2214 if (hci_con_used) break; 2215 if (!hci_can_send_command_packet_now()) break; 2216 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 2217 break; 2218 2219 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 2220 handle = little_endian_read_16(packet, 3); 2221 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2222 while (btstack_linked_list_iterator_has_next(&it)){ 2223 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2224 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2225 if (channel->con_handle != handle) continue; 2226 log_info("remote supported features, channel %p, cid %04x - state %u", channel, channel->local_cid, channel->state); 2227 l2cap_handle_remote_supported_features_received(channel); 2228 } 2229 break; 2230 2231 case GAP_EVENT_SECURITY_LEVEL: 2232 handle = little_endian_read_16(packet, 2); 2233 log_info("l2cap - security level update for handle 0x%04x", handle); 2234 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2235 while (btstack_linked_list_iterator_has_next(&it)){ 2236 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2237 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2238 if (channel->con_handle != handle) continue; 2239 2240 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 2241 gap_security_level_t required_level = channel->required_security_level; 2242 2243 log_info("channel %p, cid %04x - state %u: actual %u >= required %u?", channel, channel->local_cid, channel->state, actual_level, required_level); 2244 2245 switch (channel->state){ 2246 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2247 if (actual_level >= required_level){ 2248 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2249 // we need to know if ERTM is supported before sending a config response 2250 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 2251 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2252 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 2253 #else 2254 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2255 l2cap_emit_incoming_connection(channel); 2256 #endif 2257 } else { 2258 channel->reason = 0x0003; // security block 2259 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2260 } 2261 break; 2262 2263 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2264 if (actual_level >= required_level){ 2265 l2cap_ready_to_connect(channel); 2266 } else { 2267 // disconnnect, authentication not good enough 2268 hci_disconnect_security_block(handle); 2269 } 2270 break; 2271 2272 default: 2273 break; 2274 } 2275 } 2276 break; 2277 #endif 2278 2279 default: 2280 break; 2281 } 2282 2283 l2cap_run(); 2284 } 2285 2286 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 2287 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 2288 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 2289 signaling_responses[signaling_responses_pending].handle = handle; 2290 signaling_responses[signaling_responses_pending].code = code; 2291 signaling_responses[signaling_responses_pending].sig_id = sig_id; 2292 signaling_responses[signaling_responses_pending].cid = cid; 2293 signaling_responses[signaling_responses_pending].data = data; 2294 signaling_responses_pending++; 2295 l2cap_run(); 2296 } 2297 } 2298 2299 #ifdef ENABLE_CLASSIC 2300 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 2301 channel->remote_sig_id = identifier; 2302 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2303 l2cap_run(); 2304 } 2305 2306 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 2307 2308 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 2309 l2cap_service_t *service = l2cap_get_service(psm); 2310 if (!service) { 2311 // 0x0002 PSM not supported 2312 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2313 return; 2314 } 2315 2316 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 2317 if (!hci_connection) { 2318 // 2319 log_error("no hci_connection for handle %u", handle); 2320 return; 2321 } 2322 2323 // alloc structure 2324 // log_info("l2cap_handle_connection_request register channel"); 2325 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CLASSIC, hci_connection->address, BD_ADDR_TYPE_CLASSIC, 2326 psm, service->mtu, service->required_security_level); 2327 if (!channel){ 2328 // 0x0004 No resources available 2329 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2330 return; 2331 } 2332 2333 channel->con_handle = handle; 2334 channel->remote_cid = source_cid; 2335 channel->remote_sig_id = sig_id; 2336 2337 // limit local mtu to max acl packet length - l2cap header 2338 if (channel->local_mtu > l2cap_max_mtu()) { 2339 channel->local_mtu = l2cap_max_mtu(); 2340 } 2341 2342 // set initial state 2343 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 2344 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 2345 2346 // add to connections list 2347 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 2348 2349 // assert security requirements 2350 gap_request_security_level(handle, channel->required_security_level); 2351 } 2352 2353 void l2cap_accept_connection(uint16_t local_cid){ 2354 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 2355 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2356 if (!channel) { 2357 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 2358 return; 2359 } 2360 2361 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2362 // configure L2CAP Basic mode 2363 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2364 #endif 2365 2366 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 2367 2368 // process 2369 l2cap_run(); 2370 } 2371 2372 void l2cap_decline_connection(uint16_t local_cid){ 2373 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 2374 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 2375 if (!channel) { 2376 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 2377 return; 2378 } 2379 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2380 channel->reason = 0x04; // no resources available 2381 l2cap_run(); 2382 } 2383 2384 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2385 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 2386 2387 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2388 uint8_t use_fcs = 1; 2389 #endif 2390 2391 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2392 2393 uint16_t flags = little_endian_read_16(command, 6); 2394 if (flags & 1) { 2395 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 2396 } 2397 2398 // accept the other's configuration options 2399 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2400 uint16_t pos = 8; 2401 while (pos < end_pos){ 2402 uint8_t option_hint = command[pos] >> 7; 2403 uint8_t option_type = command[pos] & 0x7f; 2404 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2405 pos++; 2406 uint8_t length = command[pos++]; 2407 // MTU { type(8): 1, len(8):2, MTU(16) } 2408 if (option_type == L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT && length == 2){ 2409 channel->remote_mtu = little_endian_read_16(command, pos); 2410 log_info("Remote MTU %u", channel->remote_mtu); 2411 if (channel->remote_mtu > l2cap_max_mtu()){ 2412 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 2413 channel->remote_mtu = l2cap_max_mtu(); 2414 } 2415 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2416 } 2417 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2418 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FLUSH_TIMEOUT && length == 2){ 2419 channel->flush_timeout = little_endian_read_16(command, pos); 2420 log_info("Flush timeout: %u ms", channel->flush_timeout); 2421 } 2422 2423 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2424 // Retransmission and Flow Control Option 2425 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2426 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2427 switch(channel->mode){ 2428 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2429 // Store remote config 2430 channel->remote_tx_window_size = command[pos+1]; 2431 channel->remote_max_transmit = command[pos+2]; 2432 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2433 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2434 channel->remote_mps = little_endian_read_16(command, pos + 7); 2435 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2436 channel->remote_tx_window_size, 2437 channel->remote_max_transmit, 2438 channel->remote_retransmission_timeout_ms, 2439 channel->remote_monitor_timeout_ms, 2440 channel->remote_mps); 2441 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2442 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2443 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2444 } else { 2445 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2446 } 2447 break; 2448 case L2CAP_CHANNEL_MODE_BASIC: 2449 switch (mode){ 2450 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2451 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2452 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2453 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2454 } 2455 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2456 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2457 break; 2458 default: // case L2CAP_CHANNEL_MODE_BASIC: 2459 // TODO store and evaluate configuration 2460 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2461 break; 2462 } 2463 break; 2464 default: 2465 break; 2466 } 2467 } 2468 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE && length == 1){ 2469 use_fcs = command[pos]; 2470 } 2471 #endif 2472 // check for unknown options 2473 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2474 log_info("l2cap cid %u, unknown options", channel->local_cid); 2475 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2476 } 2477 pos += length; 2478 } 2479 2480 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2481 // "FCS" has precedence over "No FCS" 2482 uint8_t update = channel->fcs_option || use_fcs; 2483 log_info("local fcs: %u, remote fcs: %u -> %u", channel->fcs_option, use_fcs, update); 2484 channel->fcs_option = update; 2485 #endif 2486 } 2487 2488 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2489 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2490 log_info("l2cap_signaling_handle_configure_response"); 2491 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2492 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2493 uint16_t pos = 10; 2494 while (pos < end_pos){ 2495 uint8_t option_hint = command[pos] >> 7; 2496 uint8_t option_type = command[pos] & 0x7f; 2497 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2498 pos++; 2499 uint8_t length = command[pos++]; 2500 2501 // Retransmission and Flow Control Option 2502 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2503 switch (channel->mode){ 2504 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2505 if (channel->ertm_mandatory){ 2506 // ?? 2507 } else { 2508 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2509 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2510 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2511 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2512 } 2513 } 2514 break; 2515 case L2CAP_CHANNEL_MODE_BASIC: 2516 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2517 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2518 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2519 } 2520 break; 2521 default: 2522 break; 2523 } 2524 } 2525 2526 // check for unknown options 2527 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2528 log_info("l2cap cid %u, unknown options", channel->local_cid); 2529 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2530 } 2531 2532 pos += length; 2533 } 2534 #else 2535 UNUSED(channel); // ok: no code 2536 UNUSED(result); // ok: no code 2537 UNUSED(command); // ok: no code 2538 #endif 2539 } 2540 2541 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2542 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2543 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2544 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2545 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2546 if (channel->state == L2CAP_STATE_OPEN) return 0; 2547 return 1; 2548 } 2549 2550 2551 // @pre command len is valid, see check in l2cap_signaling_handler_dispatch 2552 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2553 2554 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2555 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2556 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2557 uint16_t result = 0; 2558 2559 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2560 2561 // handle DISCONNECT REQUESTS seperately 2562 if (code == DISCONNECTION_REQUEST){ 2563 switch (channel->state){ 2564 case L2CAP_STATE_CONFIG: 2565 case L2CAP_STATE_OPEN: 2566 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2567 case L2CAP_STATE_WAIT_DISCONNECT: 2568 l2cap_handle_disconnect_request(channel, identifier); 2569 break; 2570 2571 default: 2572 // ignore in other states 2573 break; 2574 } 2575 return; 2576 } 2577 2578 // @STATEMACHINE(l2cap) 2579 switch (channel->state) { 2580 2581 case L2CAP_STATE_WAIT_CONNECT_RSP: 2582 switch (code){ 2583 case CONNECTION_RESPONSE: 2584 if (cmd_len < 8){ 2585 // command imcomplete 2586 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2587 break; 2588 } 2589 l2cap_stop_rtx(channel); 2590 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2591 switch (result) { 2592 case 0: 2593 // successful connection 2594 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2595 channel->state = L2CAP_STATE_CONFIG; 2596 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2597 break; 2598 case 1: 2599 // connection pending. get some coffee, but start the ERTX 2600 l2cap_start_ertx(channel); 2601 break; 2602 default: 2603 // channel closed 2604 channel->state = L2CAP_STATE_CLOSED; 2605 // map l2cap connection response result to BTstack status enumeration 2606 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2607 2608 // drop link key if security block 2609 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2610 gap_drop_link_key_for_bd_addr(channel->address); 2611 } 2612 2613 // discard channel 2614 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2615 l2cap_free_channel_entry(channel); 2616 break; 2617 } 2618 break; 2619 2620 default: 2621 //@TODO: implement other signaling packets 2622 break; 2623 } 2624 break; 2625 2626 case L2CAP_STATE_CONFIG: 2627 switch (code) { 2628 case CONFIGURE_REQUEST: 2629 if (cmd_len < 4){ 2630 // command incomplete 2631 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2632 break; 2633 } 2634 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 2635 l2cap_signaling_handle_configure_request(channel, command); 2636 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 2637 // only done if continuation not set 2638 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 2639 } 2640 break; 2641 case CONFIGURE_RESPONSE: 2642 if (cmd_len < 6){ 2643 // command incomplete 2644 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2645 break; 2646 } 2647 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2648 l2cap_stop_rtx(channel); 2649 l2cap_signaling_handle_configure_response(channel, result, command); 2650 switch (result){ 2651 case 0: // success 2652 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 2653 break; 2654 case 4: // pending 2655 l2cap_start_ertx(channel); 2656 break; 2657 default: 2658 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2659 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 2660 // remote does not offer ertm but it's required 2661 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2662 break; 2663 } 2664 #endif 2665 // retry on negative result 2666 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2667 break; 2668 } 2669 break; 2670 default: 2671 break; 2672 } 2673 if (l2cap_channel_ready_for_open(channel)){ 2674 // for open: 2675 channel->state = L2CAP_STATE_OPEN; 2676 l2cap_emit_channel_opened(channel, 0); 2677 } 2678 break; 2679 2680 case L2CAP_STATE_WAIT_DISCONNECT: 2681 switch (code) { 2682 case DISCONNECTION_RESPONSE: 2683 l2cap_finialize_channel_close(channel); 2684 break; 2685 default: 2686 //@TODO: implement other signaling packets 2687 break; 2688 } 2689 break; 2690 2691 case L2CAP_STATE_CLOSED: 2692 // @TODO handle incoming requests 2693 break; 2694 2695 case L2CAP_STATE_OPEN: 2696 //@TODO: implement other signaling packets, e.g. re-configure 2697 break; 2698 default: 2699 break; 2700 } 2701 // log_info("new state %u", channel->state); 2702 } 2703 2704 2705 // @pre command len is valid, see check in l2cap_acl_classic_handler 2706 static void l2cap_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command){ 2707 2708 btstack_linked_list_iterator_t it; 2709 2710 // get code, signalind identifier and command len 2711 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2712 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2713 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2714 2715 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 2716 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_RESPONSE){ 2717 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2718 return; 2719 } 2720 2721 // general commands without an assigned channel 2722 switch(code) { 2723 2724 case CONNECTION_REQUEST: 2725 if (cmd_len == 4){ 2726 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2727 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2728 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 2729 } else { 2730 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2731 } 2732 return; 2733 2734 case ECHO_REQUEST: 2735 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 2736 return; 2737 2738 case INFORMATION_REQUEST: 2739 if (cmd_len == 2) { 2740 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2741 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 2742 } else { 2743 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2744 } 2745 return; 2746 2747 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2748 case INFORMATION_RESPONSE: { 2749 hci_connection_t * connection = hci_connection_for_handle(handle); 2750 if (!connection) return; 2751 if (cmd_len >= 4) { 2752 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2753 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2754 if (result != 0) return; 2755 if (info_type != L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) return; 2756 if (cmd_len >= 6) { 2757 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2758 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2759 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2760 // trigger connection request 2761 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2762 while (btstack_linked_list_iterator_has_next(&it)){ 2763 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2764 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2765 if (channel->con_handle != handle) continue; 2766 // bail if ERTM was requested but is not supported 2767 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2768 if (channel->ertm_mandatory){ 2769 // channel closed 2770 channel->state = L2CAP_STATE_CLOSED; 2771 // map l2cap connection response result to BTstack status enumeration 2772 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 2773 // discard channel 2774 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2775 l2cap_free_channel_entry(channel); 2776 continue; 2777 } else { 2778 // fallback to Basic mode 2779 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2780 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2781 } 2782 } 2783 // start connecting 2784 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2785 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 2786 } 2787 // respond to connection request 2788 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2789 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2790 l2cap_emit_incoming_connection(channel); 2791 } 2792 } 2793 return; // cmd len valid 2794 } 2795 } 2796 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2797 return; 2798 } 2799 #endif 2800 2801 default: 2802 break; 2803 } 2804 2805 // Get potential destination CID 2806 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2807 2808 // Find channel for this sig_id and connection handle 2809 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2810 while (btstack_linked_list_iterator_has_next(&it)){ 2811 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2812 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2813 if (channel->con_handle != handle) continue; 2814 if (code & 1) { 2815 // match odd commands (responses) by previous signaling identifier 2816 if (channel->local_sig_id == sig_id) { 2817 l2cap_signaling_handler_channel(channel, command); 2818 break; 2819 } 2820 } else { 2821 // match even commands (requests) by local channel id 2822 if (channel->local_cid == dest_cid) { 2823 l2cap_signaling_handler_channel(channel, command); 2824 break; 2825 } 2826 } 2827 } 2828 } 2829 #endif 2830 2831 #ifdef ENABLE_BLE 2832 2833 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 2834 uint8_t event[6]; 2835 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 2836 event[1] = 4; 2837 little_endian_store_16(event, 2, con_handle); 2838 little_endian_store_16(event, 4, result); 2839 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2840 if (!l2cap_event_packet_handler) return; 2841 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 2842 } 2843 2844 // @returns valid 2845 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 2846 hci_connection_t * connection; 2847 uint16_t result; 2848 uint8_t event[12]; 2849 2850 #ifdef ENABLE_LE_DATA_CHANNELS 2851 btstack_linked_list_iterator_t it; 2852 l2cap_channel_t * channel; 2853 uint16_t local_cid; 2854 uint16_t le_psm; 2855 uint16_t new_credits; 2856 uint16_t credits_before; 2857 l2cap_service_t * service; 2858 uint16_t source_cid; 2859 #endif 2860 2861 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2862 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2863 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 2864 2865 switch (code){ 2866 2867 case CONNECTION_PARAMETER_UPDATE_REQUEST: 2868 // check size 2869 if (len < 8) return 0; 2870 connection = hci_connection_for_handle(handle); 2871 if (connection){ 2872 if (connection->role != HCI_ROLE_MASTER){ 2873 // reject command without notifying upper layer when not in master role 2874 return 0; 2875 } 2876 le_connection_parameter_range_t existing_range; 2877 gap_get_connection_parameter_range(&existing_range); 2878 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2879 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2880 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2881 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 2882 2883 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 2884 if (update_parameter){ 2885 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 2886 connection->le_conn_interval_min = le_conn_interval_min; 2887 connection->le_conn_interval_max = le_conn_interval_max; 2888 connection->le_conn_latency = le_conn_latency; 2889 connection->le_supervision_timeout = le_supervision_timeout; 2890 } else { 2891 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 2892 } 2893 connection->le_con_param_update_identifier = sig_id; 2894 } 2895 2896 if (!l2cap_event_packet_handler) break; 2897 2898 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 2899 event[1] = 8; 2900 little_endian_store_16(event, 2, handle); 2901 memcpy(&event[4], &command[4], 8); 2902 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2903 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2904 break; 2905 2906 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 2907 // check size 2908 if (len < 2) return 0; 2909 result = little_endian_read_16(command, 4); 2910 l2cap_emit_connection_parameter_update_response(handle, result); 2911 break; 2912 2913 #ifdef ENABLE_LE_DATA_CHANNELS 2914 2915 case COMMAND_REJECT: 2916 // Find channel for this sig_id and connection handle 2917 channel = NULL; 2918 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2919 while (btstack_linked_list_iterator_has_next(&it)){ 2920 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2921 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 2922 if (a_channel->con_handle != handle) continue; 2923 if (a_channel->local_sig_id != sig_id) continue; 2924 channel = a_channel; 2925 break; 2926 } 2927 if (!channel) break; 2928 2929 // if received while waiting for le connection response, assume legacy device 2930 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 2931 channel->state = L2CAP_STATE_CLOSED; 2932 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 2933 l2cap_emit_le_channel_opened(channel, 0x0002); 2934 2935 // discard channel 2936 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2937 l2cap_free_channel_entry(channel); 2938 break; 2939 } 2940 break; 2941 2942 case LE_CREDIT_BASED_CONNECTION_REQUEST: 2943 // check size 2944 if (len < 10) return 0; 2945 2946 // get hci connection, bail if not found (must not happen) 2947 connection = hci_connection_for_handle(handle); 2948 if (!connection) return 0; 2949 2950 // check if service registered 2951 le_psm = little_endian_read_16(command, 4); 2952 service = l2cap_le_get_service(le_psm); 2953 source_cid = little_endian_read_16(command, 6); 2954 2955 if (service){ 2956 if (source_cid < 0x40){ 2957 // 0x0009 Connection refused - Invalid Source CID 2958 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 2959 return 1; 2960 } 2961 2962 // go through list of channels for this ACL connection and check if we get a match 2963 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2964 while (btstack_linked_list_iterator_has_next(&it)){ 2965 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2966 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 2967 if (a_channel->con_handle != handle) continue; 2968 if (a_channel->remote_cid != source_cid) continue; 2969 // 0x000a Connection refused - Source CID already allocated 2970 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 2971 return 1; 2972 } 2973 2974 // security: check encryption 2975 if (service->required_security_level >= LEVEL_2){ 2976 if (gap_encryption_key_size(handle) == 0){ 2977 // 0x0008 Connection refused - insufficient encryption 2978 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 2979 return 1; 2980 } 2981 // anything less than 16 byte key size is insufficient 2982 if (gap_encryption_key_size(handle) < 16){ 2983 // 0x0007 Connection refused – insufficient encryption key size 2984 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 2985 return 1; 2986 } 2987 } 2988 2989 // security: check authencation 2990 if (service->required_security_level >= LEVEL_3){ 2991 if (!gap_authenticated(handle)){ 2992 // 0x0005 Connection refused – insufficient authentication 2993 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 2994 return 1; 2995 } 2996 } 2997 2998 // security: check authorization 2999 if (service->required_security_level >= LEVEL_4){ 3000 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 3001 // 0x0006 Connection refused – insufficient authorization 3002 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 3003 return 1; 3004 } 3005 } 3006 3007 // allocate channel 3008 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 3009 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 3010 if (!channel){ 3011 // 0x0004 Connection refused – no resources available 3012 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 3013 return 1; 3014 } 3015 3016 channel->con_handle = handle; 3017 channel->remote_cid = source_cid; 3018 channel->remote_sig_id = sig_id; 3019 channel->remote_mtu = little_endian_read_16(command, 8); 3020 channel->remote_mps = little_endian_read_16(command, 10); 3021 channel->credits_outgoing = little_endian_read_16(command, 12); 3022 3023 // set initial state 3024 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3025 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3026 3027 // add to connections list 3028 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 3029 3030 // post connection request event 3031 l2cap_emit_le_incoming_connection(channel); 3032 3033 } else { 3034 // Connection refused – LE_PSM not supported 3035 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 3036 } 3037 break; 3038 3039 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 3040 // check size 3041 if (len < 10) return 0; 3042 3043 // Find channel for this sig_id and connection handle 3044 channel = NULL; 3045 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3046 while (btstack_linked_list_iterator_has_next(&it)){ 3047 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3048 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3049 if (a_channel->con_handle != handle) continue; 3050 if (a_channel->local_sig_id != sig_id) continue; 3051 channel = a_channel; 3052 break; 3053 } 3054 if (!channel) break; 3055 3056 // cid + 0 3057 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 3058 if (result){ 3059 channel->state = L2CAP_STATE_CLOSED; 3060 // map l2cap connection response result to BTstack status enumeration 3061 l2cap_emit_le_channel_opened(channel, result); 3062 3063 // discard channel 3064 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3065 l2cap_free_channel_entry(channel); 3066 break; 3067 } 3068 3069 // success 3070 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3071 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3072 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3073 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3074 channel->state = L2CAP_STATE_OPEN; 3075 l2cap_emit_le_channel_opened(channel, result); 3076 break; 3077 3078 case LE_FLOW_CONTROL_CREDIT: 3079 // check size 3080 if (len < 4) return 0; 3081 3082 // find channel 3083 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3084 channel = l2cap_get_channel_for_local_cid(local_cid); 3085 if (!channel) { 3086 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3087 break; 3088 } 3089 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3090 credits_before = channel->credits_outgoing; 3091 channel->credits_outgoing += new_credits; 3092 // check for credit overrun 3093 if (credits_before > channel->credits_outgoing){ 3094 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 3095 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3096 break; 3097 } 3098 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 3099 break; 3100 3101 case DISCONNECTION_REQUEST: 3102 3103 // check size 3104 if (len < 4) return 0; 3105 3106 // find channel 3107 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3108 channel = l2cap_get_channel_for_local_cid(local_cid); 3109 if (!channel) { 3110 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3111 break; 3112 } 3113 channel->remote_sig_id = sig_id; 3114 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3115 break; 3116 3117 #endif 3118 3119 case DISCONNECTION_RESPONSE: 3120 break; 3121 3122 default: 3123 // command unknown -> reject command 3124 return 0; 3125 } 3126 return 1; 3127 } 3128 #endif 3129 3130 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3131 #ifdef ENABLE_CLASSIC 3132 l2cap_channel_t * l2cap_channel; 3133 l2cap_fixed_channel_t * l2cap_fixed_channel; 3134 3135 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3136 switch (channel_id) { 3137 3138 case L2CAP_CID_SIGNALING: { 3139 uint32_t command_offset = 8; 3140 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 3141 // assert signaling command is fully inside packet 3142 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3143 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3144 if (next_command_offset > size){ 3145 log_error("l2cap signaling command len invalid -> drop"); 3146 break; 3147 } 3148 // handle signaling command 3149 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3150 // go to next command 3151 command_offset = next_command_offset; 3152 } 3153 break; 3154 } 3155 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3156 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3157 if (!l2cap_fixed_channel) break; 3158 if (!l2cap_fixed_channel->packet_handler) break; 3159 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3160 break; 3161 3162 default: 3163 // Find channel for this channel_id and connection handle 3164 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 3165 if (l2cap_channel) { 3166 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3167 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3168 3169 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3170 3171 // assert control + FCS fields are inside 3172 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) break; 3173 3174 if (l2cap_channel->fcs_option){ 3175 // verify FCS (required if one side requested it) 3176 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3177 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3178 3179 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 3180 // simulate fcs error 3181 static int counter = 0; 3182 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 3183 log_info("Simulate fcs error"); 3184 fcs_calculated++; 3185 counter = 0; 3186 } 3187 #endif 3188 3189 if (fcs_calculated == fcs_packet){ 3190 log_info("Packet FCS 0x%04x verified", fcs_packet); 3191 } else { 3192 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3193 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 3194 break; 3195 } 3196 } 3197 3198 // switch on packet type 3199 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3200 uint8_t req_seq = (control >> 8) & 0x3f; 3201 int final = (control >> 7) & 0x01; 3202 if (control & 1){ 3203 // S-Frame 3204 int poll = (control >> 4) & 0x01; 3205 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3206 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3207 l2cap_ertm_tx_packet_state_t * tx_state; 3208 switch (s){ 3209 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3210 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3211 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3212 if (poll && final){ 3213 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3214 log_error("P=F=1 in S-Frame"); 3215 break; 3216 } 3217 if (poll){ 3218 // check if we did request selective retransmission before <==> we have stored SDU segments 3219 int i; 3220 int num_stored_out_of_order_packets = 0; 3221 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3222 int index = l2cap_channel->rx_store_index + i; 3223 if (index >= l2cap_channel->num_rx_buffers){ 3224 index -= l2cap_channel->num_rx_buffers; 3225 } 3226 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3227 if (!rx_state->valid) continue; 3228 num_stored_out_of_order_packets++; 3229 } 3230 if (num_stored_out_of_order_packets){ 3231 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3232 } else { 3233 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3234 } 3235 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3236 } 3237 if (final){ 3238 // Stop-MonitorTimer 3239 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3240 // If UnackedFrames > 0 then Start-RetransTimer 3241 if (l2cap_channel->unacked_frames){ 3242 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3243 } 3244 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3245 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3246 } 3247 break; 3248 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3249 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3250 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3251 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3252 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3253 break; 3254 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3255 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3256 break; 3257 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3258 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3259 if (poll){ 3260 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3261 } 3262 // find requested i-frame 3263 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3264 if (tx_state){ 3265 log_info("Retransmission for tx_seq %u requested", req_seq); 3266 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3267 tx_state->retransmission_requested = 1; 3268 l2cap_channel->srej_active = 1; 3269 } 3270 break; 3271 default: 3272 break; 3273 } 3274 break; 3275 } else { 3276 // I-Frame 3277 // get control 3278 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3279 uint8_t tx_seq = (control >> 1) & 0x3f; 3280 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3281 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3282 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3283 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3284 if (final){ 3285 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3286 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3287 } 3288 3289 // get SDU 3290 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3291 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3292 3293 // assert SDU size is smaller or equal to our buffers 3294 uint16_t max_payload_size = 0; 3295 switch (sar){ 3296 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 3297 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 3298 // SDU Length + MPS 3299 max_payload_size = l2cap_channel->local_mps + 2; 3300 break; 3301 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 3302 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 3303 max_payload_size = l2cap_channel->local_mps; 3304 break; 3305 } 3306 if (payload_len > max_payload_size){ 3307 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 3308 break; 3309 } 3310 3311 // check ordering 3312 if (l2cap_channel->expected_tx_seq == tx_seq){ 3313 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3314 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3315 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3316 3317 // process SDU 3318 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 3319 3320 // process stored segments 3321 while (1){ 3322 int index = l2cap_channel->rx_store_index; 3323 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3324 if (!rx_state->valid) break; 3325 3326 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3327 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3328 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3329 3330 rx_state->valid = 0; 3331 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3332 3333 // update rx store index 3334 index++; 3335 if (index >= l2cap_channel->num_rx_buffers){ 3336 index = 0; 3337 } 3338 l2cap_channel->rx_store_index = index; 3339 } 3340 3341 // 3342 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3343 3344 } else { 3345 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3346 if (delta < 2){ 3347 // store segment 3348 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 3349 3350 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3351 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3352 } else { 3353 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3354 l2cap_channel->send_supervisor_frame_reject = 1; 3355 } 3356 } 3357 } 3358 break; 3359 } 3360 #endif 3361 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3362 } 3363 break; 3364 } 3365 #else 3366 UNUSED(handle); // ok: no code 3367 UNUSED(packet); // ok: no code 3368 UNUSED(size); // ok: no code 3369 #endif 3370 } 3371 3372 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3373 #ifdef ENABLE_BLE 3374 3375 l2cap_fixed_channel_t * l2cap_fixed_channel; 3376 3377 #ifdef ENABLE_LE_DATA_CHANNELS 3378 l2cap_channel_t * l2cap_channel; 3379 #endif 3380 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3381 switch (channel_id) { 3382 3383 case L2CAP_CID_SIGNALING_LE: { 3384 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3385 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3386 if (COMPLETE_L2CAP_HEADER + 4 + len > size) break; 3387 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3388 if (!valid){ 3389 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3390 } 3391 break; 3392 } 3393 3394 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3395 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3396 if (!l2cap_fixed_channel) break; 3397 if (!l2cap_fixed_channel->packet_handler) break; 3398 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3399 break; 3400 3401 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3402 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3403 if (!l2cap_fixed_channel) break; 3404 if (!l2cap_fixed_channel->packet_handler) break; 3405 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3406 break; 3407 3408 default: 3409 3410 #ifdef ENABLE_LE_DATA_CHANNELS 3411 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 3412 if (l2cap_channel) { 3413 // credit counting 3414 if (l2cap_channel->credits_incoming == 0){ 3415 log_error("LE Data Channel packet received but no incoming credits"); 3416 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3417 break; 3418 } 3419 l2cap_channel->credits_incoming--; 3420 3421 // automatic credits 3422 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 3423 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3424 } 3425 3426 // first fragment 3427 uint16_t pos = 0; 3428 if (!l2cap_channel->receive_sdu_len){ 3429 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3430 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3431 l2cap_channel->receive_sdu_len = sdu_len; 3432 l2cap_channel->receive_sdu_pos = 0; 3433 pos += 2; 3434 size -= 2; 3435 } 3436 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3437 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3438 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3439 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], fragment_size); 3440 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3441 // done? 3442 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3443 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3444 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3445 l2cap_channel->receive_sdu_len = 0; 3446 } 3447 } else { 3448 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 3449 } 3450 #endif 3451 break; 3452 } 3453 #else 3454 UNUSED(handle); // ok: no code 3455 UNUSED(packet); // ok: no code 3456 UNUSED(size); // ok: no code 3457 #endif 3458 } 3459 3460 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3461 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3462 UNUSED(channel); // ok: there is no channel 3463 3464 // Assert full L2CAP header present 3465 if (size < COMPLETE_L2CAP_HEADER) return; 3466 3467 // Dispatch to Classic or LE handler 3468 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3469 hci_connection_t *conn = hci_connection_for_handle(handle); 3470 if (!conn) return; 3471 if (conn->address_type == BD_ADDR_TYPE_CLASSIC){ 3472 l2cap_acl_classic_handler(handle, packet, size); 3473 } else { 3474 l2cap_acl_le_handler(handle, packet, size); 3475 } 3476 3477 l2cap_run(); 3478 } 3479 3480 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3481 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 3482 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3483 if (!channel) return; 3484 channel->packet_handler = the_packet_handler; 3485 } 3486 3487 #ifdef ENABLE_CLASSIC 3488 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3489 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3490 channel->state = L2CAP_STATE_CLOSED; 3491 l2cap_handle_channel_closed(channel); 3492 // discard channel 3493 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3494 l2cap_free_channel_entry(channel); 3495 } 3496 #endif 3497 3498 #ifdef L2CAP_USES_CHANNELS 3499 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3500 btstack_linked_list_iterator_t it; 3501 btstack_linked_list_iterator_init(&it, services); 3502 while (btstack_linked_list_iterator_has_next(&it)){ 3503 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3504 if ( service->psm == psm){ 3505 return service; 3506 }; 3507 } 3508 return NULL; 3509 } 3510 #endif 3511 3512 #ifdef ENABLE_CLASSIC 3513 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3514 return l2cap_get_service_internal(&l2cap_services, psm); 3515 } 3516 3517 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3518 3519 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3520 3521 // check for alread registered psm 3522 l2cap_service_t *service = l2cap_get_service(psm); 3523 if (service) { 3524 log_error("l2cap_register_service: PSM %u already registered", psm); 3525 return L2CAP_SERVICE_ALREADY_REGISTERED; 3526 } 3527 3528 // alloc structure 3529 service = btstack_memory_l2cap_service_get(); 3530 if (!service) { 3531 log_error("l2cap_register_service: no memory for l2cap_service_t"); 3532 return BTSTACK_MEMORY_ALLOC_FAILED; 3533 } 3534 3535 // fill in 3536 service->psm = psm; 3537 service->mtu = mtu; 3538 service->packet_handler = service_packet_handler; 3539 service->required_security_level = security_level; 3540 3541 // add to services list 3542 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3543 3544 // enable page scan 3545 gap_connectable_control(1); 3546 3547 return 0; 3548 } 3549 3550 uint8_t l2cap_unregister_service(uint16_t psm){ 3551 3552 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 3553 3554 l2cap_service_t *service = l2cap_get_service(psm); 3555 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3556 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 3557 btstack_memory_l2cap_service_free(service); 3558 3559 // disable page scan when no services registered 3560 if (btstack_linked_list_empty(&l2cap_services)) { 3561 gap_connectable_control(0); 3562 } 3563 return 0; 3564 } 3565 #endif 3566 3567 3568 #ifdef ENABLE_LE_DATA_CHANNELS 3569 3570 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 3571 if (!channel->waiting_for_can_send_now) return; 3572 if (channel->send_sdu_buffer) return; 3573 channel->waiting_for_can_send_now = 0; 3574 log_debug("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 3575 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 3576 } 3577 3578 // 1BH2222 3579 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 3580 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", 3581 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 3582 uint8_t event[19]; 3583 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 3584 event[1] = sizeof(event) - 2; 3585 event[2] = channel->address_type; 3586 reverse_bd_addr(channel->address, &event[3]); 3587 little_endian_store_16(event, 9, channel->con_handle); 3588 little_endian_store_16(event, 11, channel->psm); 3589 little_endian_store_16(event, 13, channel->local_cid); 3590 little_endian_store_16(event, 15, channel->remote_cid); 3591 little_endian_store_16(event, 17, channel->remote_mtu); 3592 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3593 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3594 } 3595 // 11BH22222 3596 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 3597 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", 3598 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 3599 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 3600 uint8_t event[23]; 3601 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 3602 event[1] = sizeof(event) - 2; 3603 event[2] = status; 3604 event[3] = channel->address_type; 3605 reverse_bd_addr(channel->address, &event[4]); 3606 little_endian_store_16(event, 10, channel->con_handle); 3607 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 3608 little_endian_store_16(event, 13, channel->psm); 3609 little_endian_store_16(event, 15, channel->local_cid); 3610 little_endian_store_16(event, 17, channel->remote_cid); 3611 little_endian_store_16(event, 19, channel->local_mtu); 3612 little_endian_store_16(event, 21, channel->remote_mtu); 3613 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3614 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3615 } 3616 // 2 3617 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 3618 log_info("L2CAP_EVENT_LE_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 3619 uint8_t event[4]; 3620 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 3621 event[1] = sizeof(event) - 2; 3622 little_endian_store_16(event, 2, channel->local_cid); 3623 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3624 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3625 } 3626 3627 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3628 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 3629 channel->state = L2CAP_STATE_CLOSED; 3630 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 3631 // discard channel 3632 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3633 l2cap_free_channel_entry(channel); 3634 } 3635 3636 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 3637 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 3638 } 3639 3640 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 3641 3642 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 3643 3644 // check for alread registered psm 3645 l2cap_service_t *service = l2cap_le_get_service(psm); 3646 if (service) { 3647 return L2CAP_SERVICE_ALREADY_REGISTERED; 3648 } 3649 3650 // alloc structure 3651 service = btstack_memory_l2cap_service_get(); 3652 if (!service) { 3653 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 3654 return BTSTACK_MEMORY_ALLOC_FAILED; 3655 } 3656 3657 // fill in 3658 service->psm = psm; 3659 service->mtu = 0; 3660 service->packet_handler = packet_handler; 3661 service->required_security_level = security_level; 3662 3663 // add to services list 3664 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 3665 3666 // done 3667 return 0; 3668 } 3669 3670 uint8_t l2cap_le_unregister_service(uint16_t psm) { 3671 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 3672 l2cap_service_t *service = l2cap_le_get_service(psm); 3673 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3674 3675 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 3676 btstack_memory_l2cap_service_free(service); 3677 return 0; 3678 } 3679 3680 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 3681 // get channel 3682 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3683 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3684 3685 // validate state 3686 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3687 return ERROR_CODE_COMMAND_DISALLOWED; 3688 } 3689 3690 // set state accept connection 3691 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 3692 channel->receive_sdu_buffer = receive_sdu_buffer; 3693 channel->local_mtu = mtu; 3694 channel->new_credits_incoming = initial_credits; 3695 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3696 3697 // test 3698 // channel->new_credits_incoming = 1; 3699 3700 // go 3701 l2cap_run(); 3702 return 0; 3703 } 3704 3705 /** 3706 * @brief Deny incoming LE Data Channel connection due to resource constraints 3707 * @param local_cid L2CAP LE Data Channel Identifier 3708 */ 3709 3710 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 3711 // get channel 3712 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3713 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3714 3715 // validate state 3716 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3717 return ERROR_CODE_COMMAND_DISALLOWED; 3718 } 3719 3720 // set state decline connection 3721 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 3722 channel->reason = 0x04; // no resources available 3723 l2cap_run(); 3724 return 0; 3725 } 3726 3727 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 3728 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 3729 uint16_t * out_local_cid) { 3730 3731 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 3732 3733 3734 hci_connection_t * connection = hci_connection_for_handle(con_handle); 3735 if (!connection) { 3736 log_error("no hci_connection for handle 0x%04x", con_handle); 3737 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 3738 } 3739 3740 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); 3741 if (!channel) { 3742 return BTSTACK_MEMORY_ALLOC_FAILED; 3743 } 3744 log_info("l2cap_le_create_channel %p", channel); 3745 3746 // store local_cid 3747 if (out_local_cid){ 3748 *out_local_cid = channel->local_cid; 3749 } 3750 3751 // provide buffer 3752 channel->con_handle = con_handle; 3753 channel->receive_sdu_buffer = receive_sdu_buffer; 3754 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 3755 channel->new_credits_incoming = initial_credits; 3756 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3757 3758 // add to connections list 3759 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 3760 3761 // go 3762 l2cap_run(); 3763 return 0; 3764 } 3765 3766 /** 3767 * @brief Provide credtis for LE Data Channel 3768 * @param local_cid L2CAP LE Data Channel Identifier 3769 * @param credits Number additional credits for peer 3770 */ 3771 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 3772 3773 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3774 if (!channel) { 3775 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3776 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3777 } 3778 3779 // check state 3780 if (channel->state != L2CAP_STATE_OPEN){ 3781 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 3782 } 3783 3784 // assert incoming credits + credits <= 0xffff 3785 uint32_t total_credits = channel->credits_incoming; 3786 total_credits += channel->new_credits_incoming; 3787 total_credits += credits; 3788 if (total_credits > 0xffff){ 3789 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 3790 channel->new_credits_incoming, credits); 3791 } 3792 3793 // set credits_granted 3794 channel->new_credits_incoming += credits; 3795 3796 // go 3797 l2cap_run(); 3798 return 0; 3799 } 3800 3801 /** 3802 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 3803 * @param local_cid L2CAP LE Data Channel Identifier 3804 */ 3805 int l2cap_le_can_send_now(uint16_t local_cid){ 3806 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3807 if (!channel) { 3808 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3809 return 0; 3810 } 3811 3812 // check state 3813 if (channel->state != L2CAP_STATE_OPEN) return 0; 3814 3815 // check queue 3816 if (channel->send_sdu_buffer) return 0; 3817 3818 // fine, go ahead 3819 return 1; 3820 } 3821 3822 /** 3823 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 3824 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 3825 * so packet handler should be ready to handle it 3826 * @param local_cid L2CAP LE Data Channel Identifier 3827 */ 3828 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 3829 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3830 if (!channel) { 3831 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 3832 return 0; 3833 } 3834 channel->waiting_for_can_send_now = 1; 3835 l2cap_le_notify_channel_can_send(channel); 3836 return 0; 3837 } 3838 3839 /** 3840 * @brief Send data via LE Data Channel 3841 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 3842 * @param local_cid L2CAP LE Data Channel Identifier 3843 * @param data data to send 3844 * @param size data size 3845 */ 3846 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 3847 3848 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3849 if (!channel) { 3850 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3851 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3852 } 3853 3854 if (len > channel->remote_mtu){ 3855 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 3856 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 3857 } 3858 3859 if (channel->send_sdu_buffer){ 3860 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 3861 return BTSTACK_ACL_BUFFERS_FULL; 3862 } 3863 3864 channel->send_sdu_buffer = data; 3865 channel->send_sdu_len = len; 3866 channel->send_sdu_pos = 0; 3867 3868 l2cap_run(); 3869 return 0; 3870 } 3871 3872 /** 3873 * @brief Disconnect from LE Data Channel 3874 * @param local_cid L2CAP LE Data Channel Identifier 3875 */ 3876 uint8_t l2cap_le_disconnect(uint16_t local_cid) 3877 { 3878 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3879 if (!channel) { 3880 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3881 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3882 } 3883 3884 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3885 l2cap_run(); 3886 return 0; 3887 } 3888 3889 #endif 3890