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