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