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