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