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