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