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