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 (cmd_len >= 4) { 2755 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2756 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2757 if (result != 0) return; 2758 if (info_type != L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) return; 2759 if (cmd_len >= 6) { 2760 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2761 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2762 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2763 // trigger connection request 2764 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2765 while (btstack_linked_list_iterator_has_next(&it)){ 2766 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2767 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2768 if (channel->con_handle != handle) continue; 2769 // bail if ERTM was requested but is not supported 2770 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2771 if (channel->ertm_mandatory){ 2772 // channel closed 2773 channel->state = L2CAP_STATE_CLOSED; 2774 // map l2cap connection response result to BTstack status enumeration 2775 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 2776 // discard channel 2777 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2778 l2cap_free_channel_entry(channel); 2779 continue; 2780 } else { 2781 // fallback to Basic mode 2782 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2783 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2784 } 2785 } 2786 // start connecting 2787 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2788 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 2789 } 2790 // respond to connection request 2791 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2792 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2793 l2cap_emit_incoming_connection(channel); 2794 } 2795 } 2796 return; // cmd len valid 2797 } 2798 } 2799 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2800 return; 2801 } 2802 #endif 2803 2804 default: 2805 break; 2806 } 2807 2808 // Get potential destination CID 2809 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2810 2811 // Find channel for this sig_id and connection handle 2812 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2813 while (btstack_linked_list_iterator_has_next(&it)){ 2814 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2815 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2816 if (channel->con_handle != handle) continue; 2817 if (code & 1) { 2818 // match odd commands (responses) by previous signaling identifier 2819 if (channel->local_sig_id == sig_id) { 2820 l2cap_signaling_handler_channel(channel, command); 2821 break; 2822 } 2823 } else { 2824 // match even commands (requests) by local channel id 2825 if (channel->local_cid == dest_cid) { 2826 l2cap_signaling_handler_channel(channel, command); 2827 break; 2828 } 2829 } 2830 } 2831 } 2832 #endif 2833 2834 #ifdef ENABLE_BLE 2835 2836 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 2837 uint8_t event[6]; 2838 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 2839 event[1] = 4; 2840 little_endian_store_16(event, 2, con_handle); 2841 little_endian_store_16(event, 4, result); 2842 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2843 if (!l2cap_event_packet_handler) return; 2844 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 2845 } 2846 2847 // @returns valid 2848 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 2849 hci_connection_t * connection; 2850 uint16_t result; 2851 uint8_t event[12]; 2852 2853 #ifdef ENABLE_LE_DATA_CHANNELS 2854 btstack_linked_list_iterator_t it; 2855 l2cap_channel_t * channel; 2856 uint16_t local_cid; 2857 uint16_t le_psm; 2858 uint16_t new_credits; 2859 uint16_t credits_before; 2860 l2cap_service_t * service; 2861 uint16_t source_cid; 2862 #endif 2863 2864 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2865 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2866 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 2867 2868 switch (code){ 2869 2870 case CONNECTION_PARAMETER_UPDATE_REQUEST: 2871 // check size 2872 if (len < 8) return 0; 2873 connection = hci_connection_for_handle(handle); 2874 if (connection){ 2875 if (connection->role != HCI_ROLE_MASTER){ 2876 // reject command without notifying upper layer when not in master role 2877 return 0; 2878 } 2879 le_connection_parameter_range_t existing_range; 2880 gap_get_connection_parameter_range(&existing_range); 2881 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2882 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2883 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2884 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 2885 2886 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 2887 if (update_parameter){ 2888 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 2889 connection->le_conn_interval_min = le_conn_interval_min; 2890 connection->le_conn_interval_max = le_conn_interval_max; 2891 connection->le_conn_latency = le_conn_latency; 2892 connection->le_supervision_timeout = le_supervision_timeout; 2893 } else { 2894 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 2895 } 2896 connection->le_con_param_update_identifier = sig_id; 2897 } 2898 2899 if (!l2cap_event_packet_handler) break; 2900 2901 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 2902 event[1] = 8; 2903 little_endian_store_16(event, 2, handle); 2904 memcpy(&event[4], &command[4], 8); 2905 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2906 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2907 break; 2908 2909 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 2910 // check size 2911 if (len < 2) return 0; 2912 result = little_endian_read_16(command, 4); 2913 l2cap_emit_connection_parameter_update_response(handle, result); 2914 break; 2915 2916 #ifdef ENABLE_LE_DATA_CHANNELS 2917 2918 case COMMAND_REJECT: 2919 // Find channel for this sig_id and connection handle 2920 channel = NULL; 2921 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2922 while (btstack_linked_list_iterator_has_next(&it)){ 2923 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2924 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 2925 if (a_channel->con_handle != handle) continue; 2926 if (a_channel->local_sig_id != sig_id) continue; 2927 channel = a_channel; 2928 break; 2929 } 2930 if (!channel) break; 2931 2932 // if received while waiting for le connection response, assume legacy device 2933 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 2934 channel->state = L2CAP_STATE_CLOSED; 2935 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 2936 l2cap_emit_le_channel_opened(channel, 0x0002); 2937 2938 // discard channel 2939 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2940 l2cap_free_channel_entry(channel); 2941 break; 2942 } 2943 break; 2944 2945 case LE_CREDIT_BASED_CONNECTION_REQUEST: 2946 // check size 2947 if (len < 10) return 0; 2948 2949 // get hci connection, bail if not found (must not happen) 2950 connection = hci_connection_for_handle(handle); 2951 if (!connection) return 0; 2952 2953 // check if service registered 2954 le_psm = little_endian_read_16(command, 4); 2955 service = l2cap_le_get_service(le_psm); 2956 source_cid = little_endian_read_16(command, 6); 2957 2958 if (service){ 2959 if (source_cid < 0x40){ 2960 // 0x0009 Connection refused - Invalid Source CID 2961 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 2962 return 1; 2963 } 2964 2965 // go through list of channels for this ACL connection and check if we get a match 2966 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2967 while (btstack_linked_list_iterator_has_next(&it)){ 2968 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2969 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 2970 if (a_channel->con_handle != handle) continue; 2971 if (a_channel->remote_cid != source_cid) continue; 2972 // 0x000a Connection refused - Source CID already allocated 2973 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 2974 return 1; 2975 } 2976 2977 // security: check encryption 2978 if (service->required_security_level >= LEVEL_2){ 2979 if (gap_encryption_key_size(handle) == 0){ 2980 // 0x0008 Connection refused - insufficient encryption 2981 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 2982 return 1; 2983 } 2984 // anything less than 16 byte key size is insufficient 2985 if (gap_encryption_key_size(handle) < 16){ 2986 // 0x0007 Connection refused – insufficient encryption key size 2987 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 2988 return 1; 2989 } 2990 } 2991 2992 // security: check authencation 2993 if (service->required_security_level >= LEVEL_3){ 2994 if (!gap_authenticated(handle)){ 2995 // 0x0005 Connection refused – insufficient authentication 2996 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 2997 return 1; 2998 } 2999 } 3000 3001 // security: check authorization 3002 if (service->required_security_level >= LEVEL_4){ 3003 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 3004 // 0x0006 Connection refused – insufficient authorization 3005 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 3006 return 1; 3007 } 3008 } 3009 3010 // allocate channel 3011 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 3012 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 3013 if (!channel){ 3014 // 0x0004 Connection refused – no resources available 3015 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 3016 return 1; 3017 } 3018 3019 channel->con_handle = handle; 3020 channel->remote_cid = source_cid; 3021 channel->remote_sig_id = sig_id; 3022 channel->remote_mtu = little_endian_read_16(command, 8); 3023 channel->remote_mps = little_endian_read_16(command, 10); 3024 channel->credits_outgoing = little_endian_read_16(command, 12); 3025 3026 // set initial state 3027 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3028 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3029 3030 // add to connections list 3031 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 3032 3033 // post connection request event 3034 l2cap_emit_le_incoming_connection(channel); 3035 3036 } else { 3037 // Connection refused – LE_PSM not supported 3038 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 3039 } 3040 break; 3041 3042 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 3043 // check size 3044 if (len < 10) return 0; 3045 3046 // Find channel for this sig_id and connection handle 3047 channel = NULL; 3048 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3049 while (btstack_linked_list_iterator_has_next(&it)){ 3050 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3051 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3052 if (a_channel->con_handle != handle) continue; 3053 if (a_channel->local_sig_id != sig_id) continue; 3054 channel = a_channel; 3055 break; 3056 } 3057 if (!channel) break; 3058 3059 // cid + 0 3060 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 3061 if (result){ 3062 channel->state = L2CAP_STATE_CLOSED; 3063 // map l2cap connection response result to BTstack status enumeration 3064 l2cap_emit_le_channel_opened(channel, result); 3065 3066 // discard channel 3067 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3068 l2cap_free_channel_entry(channel); 3069 break; 3070 } 3071 3072 // success 3073 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3074 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3075 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3076 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3077 channel->state = L2CAP_STATE_OPEN; 3078 l2cap_emit_le_channel_opened(channel, result); 3079 break; 3080 3081 case LE_FLOW_CONTROL_CREDIT: 3082 // check size 3083 if (len < 4) return 0; 3084 3085 // find channel 3086 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3087 channel = l2cap_get_channel_for_local_cid(local_cid); 3088 if (!channel) { 3089 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3090 break; 3091 } 3092 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3093 credits_before = channel->credits_outgoing; 3094 channel->credits_outgoing += new_credits; 3095 // check for credit overrun 3096 if (credits_before > channel->credits_outgoing){ 3097 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 3098 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3099 break; 3100 } 3101 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 3102 break; 3103 3104 case DISCONNECTION_REQUEST: 3105 3106 // check size 3107 if (len < 4) return 0; 3108 3109 // find channel 3110 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3111 channel = l2cap_get_channel_for_local_cid(local_cid); 3112 if (!channel) { 3113 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3114 break; 3115 } 3116 channel->remote_sig_id = sig_id; 3117 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3118 break; 3119 3120 #endif 3121 3122 case DISCONNECTION_RESPONSE: 3123 break; 3124 3125 default: 3126 // command unknown -> reject command 3127 return 0; 3128 } 3129 return 1; 3130 } 3131 #endif 3132 3133 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3134 #ifdef ENABLE_CLASSIC 3135 l2cap_channel_t * l2cap_channel; 3136 l2cap_fixed_channel_t * l2cap_fixed_channel; 3137 3138 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3139 switch (channel_id) { 3140 3141 case L2CAP_CID_SIGNALING: { 3142 uint32_t command_offset = 8; 3143 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 3144 // assert signaling command is fully inside packet 3145 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3146 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3147 if (next_command_offset > size){ 3148 log_error("l2cap signaling command len invalid -> drop"); 3149 break; 3150 } 3151 // handle signaling command 3152 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3153 // go to next command 3154 command_offset = next_command_offset; 3155 } 3156 break; 3157 } 3158 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3159 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3160 if (!l2cap_fixed_channel) break; 3161 if (!l2cap_fixed_channel->packet_handler) break; 3162 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3163 break; 3164 3165 default: 3166 // Find channel for this channel_id and connection handle 3167 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 3168 if (l2cap_channel) { 3169 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3170 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3171 3172 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3173 3174 // assert control + FCS fields are inside 3175 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) break; 3176 3177 if (l2cap_channel->fcs_option){ 3178 // verify FCS (required if one side requested it) 3179 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3180 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3181 3182 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 3183 // simulate fcs error 3184 static int counter = 0; 3185 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 3186 log_info("Simulate fcs error"); 3187 fcs_calculated++; 3188 counter = 0; 3189 } 3190 #endif 3191 3192 if (fcs_calculated == fcs_packet){ 3193 log_info("Packet FCS 0x%04x verified", fcs_packet); 3194 } else { 3195 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3196 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 3197 break; 3198 } 3199 } 3200 3201 // switch on packet type 3202 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3203 uint8_t req_seq = (control >> 8) & 0x3f; 3204 int final = (control >> 7) & 0x01; 3205 if (control & 1){ 3206 // S-Frame 3207 int poll = (control >> 4) & 0x01; 3208 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3209 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3210 l2cap_ertm_tx_packet_state_t * tx_state; 3211 switch (s){ 3212 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3213 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3214 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3215 if (poll && final){ 3216 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3217 log_error("P=F=1 in S-Frame"); 3218 break; 3219 } 3220 if (poll){ 3221 // check if we did request selective retransmission before <==> we have stored SDU segments 3222 int i; 3223 int num_stored_out_of_order_packets = 0; 3224 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3225 int index = l2cap_channel->rx_store_index + i; 3226 if (index >= l2cap_channel->num_rx_buffers){ 3227 index -= l2cap_channel->num_rx_buffers; 3228 } 3229 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3230 if (!rx_state->valid) continue; 3231 num_stored_out_of_order_packets++; 3232 } 3233 if (num_stored_out_of_order_packets){ 3234 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3235 } else { 3236 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3237 } 3238 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3239 } 3240 if (final){ 3241 // Stop-MonitorTimer 3242 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3243 // If UnackedFrames > 0 then Start-RetransTimer 3244 if (l2cap_channel->unacked_frames){ 3245 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3246 } 3247 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3248 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3249 } 3250 break; 3251 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3252 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3253 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3254 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3255 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3256 break; 3257 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3258 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3259 break; 3260 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3261 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3262 if (poll){ 3263 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3264 } 3265 // find requested i-frame 3266 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3267 if (tx_state){ 3268 log_info("Retransmission for tx_seq %u requested", req_seq); 3269 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3270 tx_state->retransmission_requested = 1; 3271 l2cap_channel->srej_active = 1; 3272 } 3273 break; 3274 default: 3275 break; 3276 } 3277 break; 3278 } else { 3279 // I-Frame 3280 // get control 3281 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3282 uint8_t tx_seq = (control >> 1) & 0x3f; 3283 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3284 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3285 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3286 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3287 if (final){ 3288 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3289 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3290 } 3291 3292 // get SDU 3293 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3294 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3295 3296 // assert SDU size is smaller or equal to our buffers 3297 uint16_t max_payload_size = 0; 3298 switch (sar){ 3299 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 3300 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 3301 // SDU Length + MPS 3302 max_payload_size = l2cap_channel->local_mps + 2; 3303 break; 3304 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 3305 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 3306 max_payload_size = l2cap_channel->local_mps; 3307 break; 3308 } 3309 if (payload_len > max_payload_size){ 3310 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 3311 break; 3312 } 3313 3314 // check ordering 3315 if (l2cap_channel->expected_tx_seq == tx_seq){ 3316 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3317 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3318 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3319 3320 // process SDU 3321 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 3322 3323 // process stored segments 3324 while (1){ 3325 int index = l2cap_channel->rx_store_index; 3326 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3327 if (!rx_state->valid) break; 3328 3329 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3330 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3331 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3332 3333 rx_state->valid = 0; 3334 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3335 3336 // update rx store index 3337 index++; 3338 if (index >= l2cap_channel->num_rx_buffers){ 3339 index = 0; 3340 } 3341 l2cap_channel->rx_store_index = index; 3342 } 3343 3344 // 3345 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3346 3347 } else { 3348 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3349 if (delta < 2){ 3350 // store segment 3351 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 3352 3353 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3354 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3355 } else { 3356 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3357 l2cap_channel->send_supervisor_frame_reject = 1; 3358 } 3359 } 3360 } 3361 break; 3362 } 3363 #endif 3364 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3365 } 3366 break; 3367 } 3368 #else 3369 UNUSED(handle); // ok: no code 3370 UNUSED(packet); // ok: no code 3371 UNUSED(size); // ok: no code 3372 #endif 3373 } 3374 3375 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3376 #ifdef ENABLE_BLE 3377 3378 l2cap_fixed_channel_t * l2cap_fixed_channel; 3379 3380 #ifdef ENABLE_LE_DATA_CHANNELS 3381 l2cap_channel_t * l2cap_channel; 3382 #endif 3383 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3384 switch (channel_id) { 3385 3386 case L2CAP_CID_SIGNALING_LE: { 3387 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3388 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3389 if (COMPLETE_L2CAP_HEADER + 4 + len > size) break; 3390 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3391 if (!valid){ 3392 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3393 } 3394 break; 3395 } 3396 3397 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3398 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3399 if (!l2cap_fixed_channel) break; 3400 if (!l2cap_fixed_channel->packet_handler) break; 3401 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3402 break; 3403 3404 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3405 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3406 if (!l2cap_fixed_channel) break; 3407 if (!l2cap_fixed_channel->packet_handler) break; 3408 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3409 break; 3410 3411 default: 3412 3413 #ifdef ENABLE_LE_DATA_CHANNELS 3414 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 3415 if (l2cap_channel) { 3416 // credit counting 3417 if (l2cap_channel->credits_incoming == 0){ 3418 log_error("LE Data Channel packet received but no incoming credits"); 3419 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3420 break; 3421 } 3422 l2cap_channel->credits_incoming--; 3423 3424 // automatic credits 3425 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 3426 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3427 } 3428 3429 // first fragment 3430 uint16_t pos = 0; 3431 if (!l2cap_channel->receive_sdu_len){ 3432 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3433 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3434 l2cap_channel->receive_sdu_len = sdu_len; 3435 l2cap_channel->receive_sdu_pos = 0; 3436 pos += 2; 3437 size -= 2; 3438 } 3439 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3440 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3441 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3442 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], fragment_size); 3443 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3444 // done? 3445 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3446 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3447 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3448 l2cap_channel->receive_sdu_len = 0; 3449 } 3450 } else { 3451 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 3452 } 3453 #endif 3454 break; 3455 } 3456 #else 3457 UNUSED(handle); // ok: no code 3458 UNUSED(packet); // ok: no code 3459 UNUSED(size); // ok: no code 3460 #endif 3461 } 3462 3463 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3464 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3465 UNUSED(channel); // ok: there is no channel 3466 3467 // Assert full L2CAP header present 3468 if (size < COMPLETE_L2CAP_HEADER) return; 3469 3470 // Dispatch to Classic or LE handler 3471 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3472 hci_connection_t *conn = hci_connection_for_handle(handle); 3473 if (!conn) return; 3474 if (conn->address_type == BD_ADDR_TYPE_CLASSIC){ 3475 l2cap_acl_classic_handler(handle, packet, size); 3476 } else { 3477 l2cap_acl_le_handler(handle, packet, size); 3478 } 3479 3480 l2cap_run(); 3481 } 3482 3483 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3484 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 3485 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3486 if (!channel) return; 3487 channel->packet_handler = the_packet_handler; 3488 } 3489 3490 #ifdef ENABLE_CLASSIC 3491 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3492 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3493 channel->state = L2CAP_STATE_CLOSED; 3494 l2cap_handle_channel_closed(channel); 3495 // discard channel 3496 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3497 l2cap_free_channel_entry(channel); 3498 } 3499 #endif 3500 3501 #ifdef L2CAP_USES_CHANNELS 3502 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3503 btstack_linked_list_iterator_t it; 3504 btstack_linked_list_iterator_init(&it, services); 3505 while (btstack_linked_list_iterator_has_next(&it)){ 3506 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3507 if ( service->psm == psm){ 3508 return service; 3509 }; 3510 } 3511 return NULL; 3512 } 3513 #endif 3514 3515 #ifdef ENABLE_CLASSIC 3516 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3517 return l2cap_get_service_internal(&l2cap_services, psm); 3518 } 3519 3520 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3521 3522 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3523 3524 // check for alread registered psm 3525 l2cap_service_t *service = l2cap_get_service(psm); 3526 if (service) { 3527 log_error("l2cap_register_service: PSM %u already registered", psm); 3528 return L2CAP_SERVICE_ALREADY_REGISTERED; 3529 } 3530 3531 // alloc structure 3532 service = btstack_memory_l2cap_service_get(); 3533 if (!service) { 3534 log_error("l2cap_register_service: no memory for l2cap_service_t"); 3535 return BTSTACK_MEMORY_ALLOC_FAILED; 3536 } 3537 3538 // fill in 3539 service->psm = psm; 3540 service->mtu = mtu; 3541 service->packet_handler = service_packet_handler; 3542 service->required_security_level = security_level; 3543 3544 // add to services list 3545 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3546 3547 // enable page scan 3548 gap_connectable_control(1); 3549 3550 return 0; 3551 } 3552 3553 uint8_t l2cap_unregister_service(uint16_t psm){ 3554 3555 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 3556 3557 l2cap_service_t *service = l2cap_get_service(psm); 3558 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3559 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 3560 btstack_memory_l2cap_service_free(service); 3561 3562 // disable page scan when no services registered 3563 if (btstack_linked_list_empty(&l2cap_services)) { 3564 gap_connectable_control(0); 3565 } 3566 return 0; 3567 } 3568 #endif 3569 3570 3571 #ifdef ENABLE_LE_DATA_CHANNELS 3572 3573 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 3574 if (!channel->waiting_for_can_send_now) return; 3575 if (channel->send_sdu_buffer) return; 3576 channel->waiting_for_can_send_now = 0; 3577 log_debug("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 3578 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 3579 } 3580 3581 // 1BH2222 3582 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 3583 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", 3584 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 3585 uint8_t event[19]; 3586 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 3587 event[1] = sizeof(event) - 2; 3588 event[2] = channel->address_type; 3589 reverse_bd_addr(channel->address, &event[3]); 3590 little_endian_store_16(event, 9, channel->con_handle); 3591 little_endian_store_16(event, 11, channel->psm); 3592 little_endian_store_16(event, 13, channel->local_cid); 3593 little_endian_store_16(event, 15, channel->remote_cid); 3594 little_endian_store_16(event, 17, channel->remote_mtu); 3595 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3596 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3597 } 3598 // 11BH22222 3599 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 3600 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", 3601 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 3602 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 3603 uint8_t event[23]; 3604 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 3605 event[1] = sizeof(event) - 2; 3606 event[2] = status; 3607 event[3] = channel->address_type; 3608 reverse_bd_addr(channel->address, &event[4]); 3609 little_endian_store_16(event, 10, channel->con_handle); 3610 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 3611 little_endian_store_16(event, 13, channel->psm); 3612 little_endian_store_16(event, 15, channel->local_cid); 3613 little_endian_store_16(event, 17, channel->remote_cid); 3614 little_endian_store_16(event, 19, channel->local_mtu); 3615 little_endian_store_16(event, 21, channel->remote_mtu); 3616 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3617 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3618 } 3619 // 2 3620 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 3621 log_info("L2CAP_EVENT_LE_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 3622 uint8_t event[4]; 3623 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 3624 event[1] = sizeof(event) - 2; 3625 little_endian_store_16(event, 2, channel->local_cid); 3626 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3627 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3628 } 3629 3630 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3631 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 3632 channel->state = L2CAP_STATE_CLOSED; 3633 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 3634 // discard channel 3635 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3636 l2cap_free_channel_entry(channel); 3637 } 3638 3639 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 3640 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 3641 } 3642 3643 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 3644 3645 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 3646 3647 // check for alread registered psm 3648 l2cap_service_t *service = l2cap_le_get_service(psm); 3649 if (service) { 3650 return L2CAP_SERVICE_ALREADY_REGISTERED; 3651 } 3652 3653 // alloc structure 3654 service = btstack_memory_l2cap_service_get(); 3655 if (!service) { 3656 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 3657 return BTSTACK_MEMORY_ALLOC_FAILED; 3658 } 3659 3660 // fill in 3661 service->psm = psm; 3662 service->mtu = 0; 3663 service->packet_handler = packet_handler; 3664 service->required_security_level = security_level; 3665 3666 // add to services list 3667 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 3668 3669 // done 3670 return 0; 3671 } 3672 3673 uint8_t l2cap_le_unregister_service(uint16_t psm) { 3674 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 3675 l2cap_service_t *service = l2cap_le_get_service(psm); 3676 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3677 3678 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 3679 btstack_memory_l2cap_service_free(service); 3680 return 0; 3681 } 3682 3683 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 3684 // get channel 3685 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3686 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3687 3688 // validate state 3689 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3690 return ERROR_CODE_COMMAND_DISALLOWED; 3691 } 3692 3693 // set state accept connection 3694 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 3695 channel->receive_sdu_buffer = receive_sdu_buffer; 3696 channel->local_mtu = mtu; 3697 channel->new_credits_incoming = initial_credits; 3698 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3699 3700 // test 3701 // channel->new_credits_incoming = 1; 3702 3703 // go 3704 l2cap_run(); 3705 return 0; 3706 } 3707 3708 /** 3709 * @brief Deny incoming LE Data Channel connection due to resource constraints 3710 * @param local_cid L2CAP LE Data Channel Identifier 3711 */ 3712 3713 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 3714 // get channel 3715 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3716 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3717 3718 // validate state 3719 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3720 return ERROR_CODE_COMMAND_DISALLOWED; 3721 } 3722 3723 // set state decline connection 3724 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 3725 channel->reason = 0x04; // no resources available 3726 l2cap_run(); 3727 return 0; 3728 } 3729 3730 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 3731 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 3732 uint16_t * out_local_cid) { 3733 3734 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 3735 3736 3737 hci_connection_t * connection = hci_connection_for_handle(con_handle); 3738 if (!connection) { 3739 log_error("no hci_connection for handle 0x%04x", con_handle); 3740 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 3741 } 3742 3743 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); 3744 if (!channel) { 3745 return BTSTACK_MEMORY_ALLOC_FAILED; 3746 } 3747 log_info("l2cap_le_create_channel %p", channel); 3748 3749 // store local_cid 3750 if (out_local_cid){ 3751 *out_local_cid = channel->local_cid; 3752 } 3753 3754 // provide buffer 3755 channel->con_handle = con_handle; 3756 channel->receive_sdu_buffer = receive_sdu_buffer; 3757 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 3758 channel->new_credits_incoming = initial_credits; 3759 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3760 3761 // add to connections list 3762 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 3763 3764 // go 3765 l2cap_run(); 3766 return 0; 3767 } 3768 3769 /** 3770 * @brief Provide credtis for LE Data Channel 3771 * @param local_cid L2CAP LE Data Channel Identifier 3772 * @param credits Number additional credits for peer 3773 */ 3774 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 3775 3776 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3777 if (!channel) { 3778 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3779 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3780 } 3781 3782 // check state 3783 if (channel->state != L2CAP_STATE_OPEN){ 3784 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 3785 } 3786 3787 // assert incoming credits + credits <= 0xffff 3788 uint32_t total_credits = channel->credits_incoming; 3789 total_credits += channel->new_credits_incoming; 3790 total_credits += credits; 3791 if (total_credits > 0xffff){ 3792 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 3793 channel->new_credits_incoming, credits); 3794 } 3795 3796 // set credits_granted 3797 channel->new_credits_incoming += credits; 3798 3799 // go 3800 l2cap_run(); 3801 return 0; 3802 } 3803 3804 /** 3805 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 3806 * @param local_cid L2CAP LE Data Channel Identifier 3807 */ 3808 int l2cap_le_can_send_now(uint16_t local_cid){ 3809 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3810 if (!channel) { 3811 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3812 return 0; 3813 } 3814 3815 // check state 3816 if (channel->state != L2CAP_STATE_OPEN) return 0; 3817 3818 // check queue 3819 if (channel->send_sdu_buffer) return 0; 3820 3821 // fine, go ahead 3822 return 1; 3823 } 3824 3825 /** 3826 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 3827 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 3828 * so packet handler should be ready to handle it 3829 * @param local_cid L2CAP LE Data Channel Identifier 3830 */ 3831 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 3832 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3833 if (!channel) { 3834 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 3835 return 0; 3836 } 3837 channel->waiting_for_can_send_now = 1; 3838 l2cap_le_notify_channel_can_send(channel); 3839 return 0; 3840 } 3841 3842 /** 3843 * @brief Send data via LE Data Channel 3844 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 3845 * @param local_cid L2CAP LE Data Channel Identifier 3846 * @param data data to send 3847 * @param size data size 3848 */ 3849 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 3850 3851 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3852 if (!channel) { 3853 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3854 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3855 } 3856 3857 if (len > channel->remote_mtu){ 3858 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 3859 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 3860 } 3861 3862 if (channel->send_sdu_buffer){ 3863 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 3864 return BTSTACK_ACL_BUFFERS_FULL; 3865 } 3866 3867 channel->send_sdu_buffer = data; 3868 channel->send_sdu_len = len; 3869 channel->send_sdu_pos = 0; 3870 3871 l2cap_run(); 3872 return 0; 3873 } 3874 3875 /** 3876 * @brief Disconnect from LE Data Channel 3877 * @param local_cid L2CAP LE Data Channel Identifier 3878 */ 3879 uint8_t l2cap_le_disconnect(uint16_t local_cid) 3880 { 3881 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3882 if (!channel) { 3883 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3884 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3885 } 3886 3887 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3888 l2cap_run(); 3889 return 0; 3890 } 3891 3892 #endif 3893