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