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