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