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