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