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