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