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