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