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 // log_info("l2cap_send_signaling_packet type %u", cmd); 1291 hci_reserve_packet_buffer(); 1292 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1293 va_list argptr; 1294 va_start(argptr, identifier); 1295 uint16_t len = l2cap_create_signaling_classic(acl_buffer, handle, cmd, identifier, argptr); 1296 va_end(argptr); 1297 // log_info("l2cap_send_signaling_packet con %u!", handle); 1298 return hci_send_acl_packet_buffer(len); 1299 } 1300 1301 // assumption - only on Classic connections 1302 // cannot be used for L2CAP ERTM 1303 uint8_t l2cap_send_prepared(uint16_t local_cid, uint16_t len){ 1304 1305 if (!hci_is_packet_buffer_reserved()){ 1306 log_error("l2cap_send_prepared called without reserving packet first"); 1307 return BTSTACK_ACL_BUFFERS_FULL; 1308 } 1309 1310 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1311 if (!channel) { 1312 log_error("l2cap_send_prepared no channel for cid 0x%02x", local_cid); 1313 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 1314 } 1315 1316 if (!hci_can_send_prepared_acl_packet_now(channel->con_handle)){ 1317 log_info("l2cap_send_prepared cid 0x%02x, cannot send", local_cid); 1318 return BTSTACK_ACL_BUFFERS_FULL; 1319 } 1320 1321 log_debug("l2cap_send_prepared cid 0x%02x, handle %u, 1 credit used", local_cid, channel->con_handle); 1322 1323 int fcs_size = 0; 1324 1325 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1326 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->fcs_option){ 1327 fcs_size = 2; 1328 } 1329 #endif 1330 1331 // set non-flushable packet boundary flag if supported on Controller 1332 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1333 uint8_t packet_boundary_flag = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 1334 l2cap_setup_header(acl_buffer, channel->con_handle, packet_boundary_flag, channel->remote_cid, len + fcs_size); 1335 1336 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1337 if (fcs_size){ 1338 // calculate FCS over l2cap data 1339 uint16_t fcs = crc16_calc(acl_buffer + 4, 4 + len); 1340 log_info("I-Frame: fcs 0x%04x", fcs); 1341 little_endian_store_16(acl_buffer, 8 + len, fcs); 1342 } 1343 #endif 1344 1345 // send 1346 return hci_send_acl_packet_buffer(len+8+fcs_size); 1347 } 1348 1349 // assumption - only on Classic connections 1350 uint8_t l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){ 1351 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1352 if (!channel) { 1353 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 1354 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 1355 } 1356 1357 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1358 // send in ERTM 1359 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1360 return l2cap_ertm_send(channel, data, len); 1361 } 1362 #endif 1363 1364 if (len > channel->remote_mtu){ 1365 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 1366 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 1367 } 1368 1369 if (!hci_can_send_acl_packet_now(channel->con_handle)){ 1370 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 1371 return BTSTACK_ACL_BUFFERS_FULL; 1372 } 1373 1374 hci_reserve_packet_buffer(); 1375 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1376 (void)memcpy(&acl_buffer[8], data, len); 1377 return l2cap_send_prepared(local_cid, len); 1378 } 1379 1380 int l2cap_send_echo_request(hci_con_handle_t con_handle, uint8_t *data, uint16_t len){ 1381 return l2cap_send_signaling_packet(con_handle, ECHO_REQUEST, l2cap_next_sig_id(), len, data); 1382 } 1383 1384 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, uint16_t flag){ 1385 channel->state_var = channel->state_var | flag; 1386 } 1387 1388 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, uint16_t flag){ 1389 channel->state_var = channel->state_var & ~flag; 1390 } 1391 #endif 1392 1393 1394 #ifdef ENABLE_BLE 1395 static int l2cap_send_le_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, int identifier, ...){ 1396 1397 if (!hci_can_send_acl_packet_now(handle)){ 1398 log_info("l2cap_send_le_signaling_packet, cannot send"); 1399 return BTSTACK_ACL_BUFFERS_FULL; 1400 } 1401 1402 // log_info("l2cap_send_le_signaling_packet type %u", cmd); 1403 hci_reserve_packet_buffer(); 1404 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1405 va_list argptr; 1406 va_start(argptr, identifier); 1407 uint16_t len = l2cap_create_signaling_le(acl_buffer, handle, cmd, identifier, argptr); 1408 va_end(argptr); 1409 // log_info("l2cap_send_le_signaling_packet con %u!", handle); 1410 return hci_send_acl_packet_buffer(len); 1411 } 1412 #endif 1413 1414 uint16_t l2cap_max_mtu(void){ 1415 return HCI_ACL_PAYLOAD_SIZE - L2CAP_HEADER_SIZE; 1416 } 1417 1418 #ifdef ENABLE_BLE 1419 uint16_t l2cap_max_le_mtu(void){ 1420 if (l2cap_le_custom_max_mtu != 0u) return l2cap_le_custom_max_mtu; 1421 return l2cap_max_mtu(); 1422 } 1423 1424 void l2cap_set_max_le_mtu(uint16_t max_mtu){ 1425 if (max_mtu < l2cap_max_mtu()){ 1426 l2cap_le_custom_max_mtu = max_mtu; 1427 } 1428 } 1429 #endif 1430 1431 #ifdef ENABLE_CLASSIC 1432 1433 static uint16_t l2cap_setup_options_mtu(uint8_t * config_options, uint16_t mtu){ 1434 config_options[0] = L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT; // MTU 1435 config_options[1] = 2; // len param 1436 little_endian_store_16(config_options, 2, mtu); 1437 return 4; 1438 } 1439 1440 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1441 static int l2cap_ertm_mode(l2cap_channel_t * channel){ 1442 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1443 return ((connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_DONE) 1444 && (connection->l2cap_state.extended_feature_mask & 0x08)); 1445 } 1446 #endif 1447 1448 static uint16_t l2cap_setup_options_request(l2cap_channel_t * channel, uint8_t * config_options){ 1449 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1450 // use ERTM options if supported by remote and channel ready to use it 1451 if (l2cap_ertm_mode(channel) && channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1452 return l2cap_setup_options_ertm_request(channel, config_options); 1453 } 1454 #endif 1455 uint16_t mtu = channel->local_mtu; 1456 return l2cap_setup_options_mtu(config_options, mtu); 1457 } 1458 1459 static uint16_t l2cap_setup_options_mtu_response(l2cap_channel_t * channel, uint8_t * config_options){ 1460 uint16_t mtu = btstack_min(channel->local_mtu, channel->remote_mtu); 1461 return l2cap_setup_options_mtu(config_options, mtu); 1462 } 1463 1464 static uint32_t l2cap_extended_features_mask(void){ 1465 // extended features request supported, features: fixed channels, unicast connectionless data reception 1466 uint32_t features = 0x280; 1467 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1468 features |= 0x0028; 1469 #endif 1470 return features; 1471 } 1472 #endif 1473 1474 // 1475 #ifdef ENABLE_CLASSIC 1476 1477 // returns true if channel was finalized 1478 static bool l2cap_run_for_classic_channel(l2cap_channel_t * channel){ 1479 1480 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1481 uint8_t config_options[18]; 1482 #else 1483 uint8_t config_options[10]; 1484 #endif 1485 1486 switch (channel->state){ 1487 1488 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1489 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 1490 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1491 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 1492 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 1493 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONN_RESP_PEND); 1494 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0); 1495 } 1496 break; 1497 1498 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1499 if (!hci_can_send_command_packet_now()) break; 1500 // send connection request - set state first 1501 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 1502 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 1503 (void)memcpy(l2cap_outgoing_classic_addr, channel->address, 6); 1504 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, hci_get_allow_role_switch()); 1505 break; 1506 1507 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 1508 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1509 channel->state = L2CAP_STATE_INVALID; 1510 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0); 1511 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1512 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1513 l2cap_free_channel_entry(channel); 1514 channel = NULL; 1515 break; 1516 1517 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 1518 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1519 channel->state = L2CAP_STATE_CONFIG; 1520 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1521 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 1522 break; 1523 1524 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 1525 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1526 // success, start l2cap handshake 1527 channel->local_sig_id = l2cap_next_sig_id(); 1528 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 1529 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 1530 l2cap_start_rtx(channel); 1531 break; 1532 1533 case L2CAP_STATE_CONFIG: 1534 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1535 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1536 // fallback to basic mode if ERTM requested but not not supported by remote 1537 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1538 if (!l2cap_ertm_mode(channel)){ 1539 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 1540 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1541 } 1542 } 1543 #endif 1544 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 1545 uint16_t flags = 0; 1546 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1547 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 1548 flags = 1; 1549 } else { 1550 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1551 } 1552 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 1553 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1554 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); 1555 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1556 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 1557 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1558 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1559 uint16_t options_size = l2cap_setup_options_ertm_response(channel, config_options); 1560 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); 1561 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM){ 1562 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 1563 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1564 uint16_t options_size = l2cap_setup_options_ertm_response(channel, config_options); 1565 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); 1566 #endif 1567 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 1568 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1569 uint16_t options_size = l2cap_setup_options_mtu_response(channel, config_options); 1570 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); 1571 } else { 1572 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 1573 } 1574 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1575 } 1576 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 1577 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1578 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 1579 channel->local_sig_id = l2cap_next_sig_id(); 1580 uint16_t options_size = l2cap_setup_options_request(channel, config_options); 1581 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, options_size, &config_options); 1582 l2cap_start_rtx(channel); 1583 } 1584 if (l2cap_channel_ready_for_open(channel)){ 1585 channel->state = L2CAP_STATE_OPEN; 1586 l2cap_emit_channel_opened(channel, 0); // success 1587 } 1588 break; 1589 1590 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1591 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1592 channel->state = L2CAP_STATE_INVALID; 1593 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1594 // 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 :) 1595 l2cap_finialize_channel_close(channel); // -- remove from list 1596 channel = NULL; 1597 break; 1598 1599 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1600 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1601 channel->local_sig_id = l2cap_next_sig_id(); 1602 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1603 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1604 break; 1605 default: 1606 break; 1607 } 1608 1609 // handle channel finalize on L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE and L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE 1610 return channel == NULL; 1611 } 1612 1613 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1614 static void l2cap_run_for_classic_channel_ertm(l2cap_channel_t * channel){ 1615 1616 // ERTM mode 1617 if (channel->mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) return; 1618 1619 // check if we can still send 1620 if (channel->con_handle == HCI_CON_HANDLE_INVALID) return; 1621 if (!hci_can_send_acl_packet_now(channel->con_handle)) return; 1622 1623 if (channel->send_supervisor_frame_receiver_ready){ 1624 channel->send_supervisor_frame_receiver_ready = 0; 1625 log_info("Send S-Frame: RR %u, final %u", channel->req_seq, channel->set_final_bit_after_packet_with_poll_bit_set); 1626 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); 1627 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1628 l2cap_ertm_send_supervisor_frame(channel, control); 1629 return; 1630 } 1631 if (channel->send_supervisor_frame_receiver_ready_poll){ 1632 channel->send_supervisor_frame_receiver_ready_poll = 0; 1633 log_info("Send S-Frame: RR %u with poll=1 ", channel->req_seq); 1634 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 1, 0, channel->req_seq); 1635 l2cap_ertm_send_supervisor_frame(channel, control); 1636 return; 1637 } 1638 if (channel->send_supervisor_frame_receiver_not_ready){ 1639 channel->send_supervisor_frame_receiver_not_ready = 0; 1640 log_info("Send S-Frame: RNR %u", channel->req_seq); 1641 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 1642 l2cap_ertm_send_supervisor_frame(channel, control); 1643 return; 1644 } 1645 if (channel->send_supervisor_frame_reject){ 1646 channel->send_supervisor_frame_reject = 0; 1647 log_info("Send S-Frame: REJ %u", channel->req_seq); 1648 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT, 0, 0, channel->req_seq); 1649 l2cap_ertm_send_supervisor_frame(channel, control); 1650 return; 1651 } 1652 if (channel->send_supervisor_frame_selective_reject){ 1653 channel->send_supervisor_frame_selective_reject = 0; 1654 log_info("Send S-Frame: SREJ %u", channel->expected_tx_seq); 1655 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); 1656 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1657 l2cap_ertm_send_supervisor_frame(channel, control); 1658 return; 1659 } 1660 1661 if (channel->srej_active){ 1662 int i; 1663 for (i=0;i<channel->num_tx_buffers;i++){ 1664 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[i]; 1665 if (tx_state->retransmission_requested) { 1666 tx_state->retransmission_requested = 0; 1667 uint8_t final = channel->set_final_bit_after_packet_with_poll_bit_set; 1668 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1669 l2cap_ertm_send_information_frame(channel, i, final); 1670 break; 1671 } 1672 } 1673 if (i == channel->num_tx_buffers){ 1674 // no retransmission request found 1675 channel->srej_active = 0; 1676 } else { 1677 // packet was sent 1678 return; 1679 } 1680 } 1681 } 1682 #endif /* ERTM */ 1683 #endif /* Classic */ 1684 1685 static void l2cap_run_signaling_response(void) { 1686 1687 // check pending signaling responses 1688 while (l2cap_signaling_responses_pending){ 1689 1690 hci_con_handle_t handle = l2cap_signaling_responses[0].handle; 1691 1692 if (!hci_can_send_acl_packet_now(handle)) break; 1693 1694 uint8_t sig_id = l2cap_signaling_responses[0].sig_id; 1695 uint8_t response_code = l2cap_signaling_responses[0].code; 1696 uint16_t result = l2cap_signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT, REJECT_SM_PAIRING 1697 uint8_t buffer[2]; // REJECT_SM_PAIRING 1698 #ifdef ENABLE_CLASSIC 1699 uint16_t info_type = l2cap_signaling_responses[0].data; // INFORMATION_REQUEST 1700 #endif 1701 uint16_t source_cid = l2cap_signaling_responses[0].cid; // CONNECTION_REQUEST, REJECT_SM_PAIRING 1702 1703 // remove first item before sending (to avoid sending response mutliple times) 1704 l2cap_signaling_responses_pending--; 1705 int i; 1706 for (i=0; i < l2cap_signaling_responses_pending; i++){ 1707 (void)memcpy(&l2cap_signaling_responses[i], 1708 &l2cap_signaling_responses[i + 1], 1709 sizeof(l2cap_signaling_response_t)); 1710 } 1711 1712 switch (response_code){ 1713 #ifdef ENABLE_CLASSIC 1714 case CONNECTION_REQUEST: 1715 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 1716 break; 1717 case ECHO_REQUEST: 1718 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 1719 break; 1720 case INFORMATION_REQUEST: 1721 switch (info_type){ 1722 case L2CAP_INFO_TYPE_CONNECTIONLESS_MTU: { 1723 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 1724 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(connectionless_mtu), &connectionless_mtu); 1725 } 1726 break; 1727 case L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED: { 1728 uint32_t features = l2cap_extended_features_mask(); 1729 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(features), &features); 1730 } 1731 break; 1732 case L2CAP_INFO_TYPE_FIXED_CHANNELS_SUPPORTED: { 1733 uint8_t map[8]; 1734 memset(map, 0, 8); 1735 // L2CAP Signaling Channel (bit 1) + Connectionless reception (bit 2) 1736 map[0] = (1 << 1) | (1 << 2); 1737 #ifdef ENABLE_BLE 1738 // BR/EDR Security Manager (bit 7) 1739 map[0] |= (1 << 7); 1740 #endif 1741 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(map), &map); 1742 } 1743 break; 1744 default: 1745 // all other types are not supported 1746 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 1, 0, NULL); 1747 break; 1748 } 1749 break; 1750 case COMMAND_REJECT: 1751 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1752 break; 1753 #endif 1754 #ifdef ENABLE_BLE 1755 case LE_CREDIT_BASED_CONNECTION_REQUEST: 1756 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 1757 break; 1758 case COMMAND_REJECT_LE: 1759 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1760 break; 1761 #endif 1762 case SM_PAIRING_FAILED: 1763 buffer[0] = SM_CODE_PAIRING_FAILED; 1764 buffer[1] = result; 1765 l2cap_send_connectionless(handle, source_cid, buffer, 2); 1766 break; 1767 default: 1768 // should not happen 1769 break; 1770 } 1771 } 1772 } 1773 1774 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1775 static bool l2ap_run_ertm(void){ 1776 // send l2cap information request if neccessary 1777 btstack_linked_list_iterator_t it; 1778 hci_connections_get_iterator(&it); 1779 while(btstack_linked_list_iterator_has_next(&it)){ 1780 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1781 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST){ 1782 if (!hci_can_send_acl_packet_now(connection->con_handle)) break; 1783 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE; 1784 uint8_t sig_id = l2cap_next_sig_id(); 1785 uint8_t info_type = L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED; 1786 l2cap_send_signaling_packet(connection->con_handle, INFORMATION_REQUEST, sig_id, info_type); 1787 return true; 1788 } 1789 } 1790 return false; 1791 } 1792 #endif 1793 1794 #ifdef ENABLE_LE_DATA_CHANNELS 1795 static void l2cap_run_le_data_channels(void){ 1796 btstack_linked_list_iterator_t it; 1797 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1798 while (btstack_linked_list_iterator_has_next(&it)){ 1799 uint16_t mps; 1800 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1801 1802 if (channel->channel_type != L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL) continue; 1803 1804 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1805 switch (channel->state){ 1806 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1807 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1808 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1809 // le psm, source cid, mtu, mps, initial credits 1810 channel->local_sig_id = l2cap_next_sig_id(); 1811 channel->credits_incoming = channel->new_credits_incoming; 1812 channel->new_credits_incoming = 0; 1813 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1814 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); 1815 break; 1816 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1817 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1818 // TODO: support larger MPS 1819 channel->state = L2CAP_STATE_OPEN; 1820 channel->credits_incoming = channel->new_credits_incoming; 1821 channel->new_credits_incoming = 0; 1822 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1823 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); 1824 // notify client 1825 l2cap_emit_le_channel_opened(channel, 0); 1826 break; 1827 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1828 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1829 channel->state = L2CAP_STATE_INVALID; 1830 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1831 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1832 btstack_linked_list_iterator_remove(&it); 1833 l2cap_free_channel_entry(channel); 1834 break; 1835 case L2CAP_STATE_OPEN: 1836 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1837 1838 // send credits 1839 if (channel->new_credits_incoming){ 1840 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1841 channel->local_sig_id = l2cap_next_sig_id(); 1842 uint16_t new_credits = channel->new_credits_incoming; 1843 channel->new_credits_incoming = 0; 1844 channel->credits_incoming += new_credits; 1845 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1846 } 1847 break; 1848 1849 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1850 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1851 channel->local_sig_id = l2cap_next_sig_id(); 1852 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1853 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1854 break; 1855 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1856 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1857 channel->state = L2CAP_STATE_INVALID; 1858 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1859 l2cap_le_finialize_channel_close(channel); // -- remove from list 1860 break; 1861 default: 1862 break; 1863 } 1864 } 1865 } 1866 #endif 1867 1868 // MARK: L2CAP_RUN 1869 // process outstanding signaling tasks 1870 static void l2cap_run(void){ 1871 1872 // log_info("l2cap_run: entered"); 1873 l2cap_run_signaling_response(); 1874 1875 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1876 bool done = l2ap_run_ertm(); 1877 if (done) return; 1878 #endif 1879 1880 #if defined(ENABLE_CLASSIC) || defined(ENABLE_BLE) 1881 btstack_linked_list_iterator_t it; 1882 #endif 1883 1884 #ifdef ENABLE_CLASSIC 1885 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1886 while (btstack_linked_list_iterator_has_next(&it)){ 1887 1888 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1889 1890 if (channel->channel_type != L2CAP_CHANNEL_TYPE_CLASSIC) continue; 1891 1892 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1893 bool finalized = l2cap_run_for_classic_channel(channel); 1894 1895 if (!finalized) { 1896 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1897 l2cap_run_for_classic_channel_ertm(channel); 1898 #endif 1899 } 1900 } 1901 #endif 1902 1903 #ifdef ENABLE_LE_DATA_CHANNELS 1904 l2cap_run_le_data_channels(); 1905 #endif 1906 1907 #ifdef ENABLE_BLE 1908 // send l2cap con paramter update if necessary 1909 hci_connections_get_iterator(&it); 1910 while(btstack_linked_list_iterator_has_next(&it)){ 1911 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1912 if ((connection->address_type != BD_ADDR_TYPE_LE_PUBLIC) && (connection->address_type != BD_ADDR_TYPE_LE_RANDOM)) continue; 1913 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1914 switch (connection->le_con_parameter_update_state){ 1915 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1916 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1917 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, l2cap_next_sig_id(), 1918 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1919 break; 1920 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1921 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1922 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1923 break; 1924 case CON_PARAMETER_UPDATE_DENY: 1925 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1926 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1927 break; 1928 default: 1929 break; 1930 } 1931 } 1932 #endif 1933 1934 // log_info("l2cap_run: exit"); 1935 } 1936 1937 #ifdef ENABLE_CLASSIC 1938 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1939 if ((channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE) || (channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION)) { 1940 log_info("connection complete con_handle %04x - for channel %p cid 0x%04x", (int) con_handle, channel, channel->local_cid); 1941 // success, start l2cap handshake 1942 channel->con_handle = con_handle; 1943 // we require remote features to check SSP 1944 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1945 hci_remote_features_query(con_handle); 1946 } 1947 } 1948 1949 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1950 1951 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1952 // assumption: outgoing connection 1953 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1954 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1955 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1956 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1957 return; 1958 } 1959 #endif 1960 1961 // fine, go ahead 1962 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1963 } 1964 1965 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1966 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1967 // double check if all feature pages are complete 1968 1969 bool security_required = channel->required_security_level > LEVEL_0; 1970 1971 // abort if Secure Connections Only Mode with legacy connection 1972 if (security_required && gap_get_secure_connections_only_mode() && gap_secure_connection(channel->con_handle) == 0){ 1973 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY); 1974 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1975 l2cap_free_channel_entry(channel); 1976 return; 1977 } 1978 1979 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) != 0){ 1980 1981 // Core V5.2, Vol 3, Part C, 5.2.2.2 - Security Mode 4 1982 // When a remote device attempts to access a service offered by a Bluetooth device that is in security mode 4 1983 // and a sufficient link key exists and authentication has not been performed the local device shall authenticate 1984 // the remote device and enable encryption after the channel establishment request is received but before a channel 1985 // establishment confirmation (L2CAP_ConnectRsp with result code of 0x0000 or a higher-level channel establishment 1986 // confirmation such as that of RFCOMM) is sent. 1987 1988 // If the remote device has indicated support for Secure Simple Pairing, a channel establishment request is 1989 // received for a service other than SDP, and encryption has not yet been enabled, then the local device shall 1990 // disconnect the ACL link with error code 0x05 - Authentication Failure. 1991 1992 // => Disconnect if l2cap request received in mode 4 and ssp supported, non-sdp psm, not encrypted, no link key available 1993 if ((gap_get_security_mode() == GAP_SECURITY_MODE_4) 1994 && gap_ssp_supported_on_both_sides(channel->con_handle) 1995 && (channel->psm != PSM_SDP) 1996 && (gap_encryption_key_size(channel->con_handle) == 0) 1997 && (gap_bonded(channel->con_handle) == false)){ 1998 hci_disconnect_security_block(channel->con_handle); 1999 return; 2000 } 2001 2002 // incoming: assert security requirements 2003 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 2004 if (channel->required_security_level <= gap_security_level(channel->con_handle)){ 2005 l2cap_handle_security_level_incoming_sufficient(channel); 2006 } else { 2007 // send connection pending if not already done 2008 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONN_RESP_PEND) == 0){ 2009 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND; 2010 } 2011 gap_request_security_level(channel->con_handle, channel->required_security_level); 2012 } 2013 } else { 2014 // outgoing: we have been waiting for remote supported features 2015 if (security_required){ 2016 // request security level 2017 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 2018 gap_request_security_level(channel->con_handle, channel->required_security_level); 2019 } else { 2020 l2cap_ready_to_connect(channel); 2021 } 2022 } 2023 } 2024 #endif 2025 2026 #ifdef L2CAP_USES_CHANNELS 2027 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, 2028 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 2029 2030 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 2031 if (!channel) { 2032 return NULL; 2033 } 2034 2035 // fill in 2036 channel->packet_handler = packet_handler; 2037 channel->channel_type = channel_type; 2038 bd_addr_copy(channel->address, address); 2039 channel->address_type = address_type; 2040 channel->psm = psm; 2041 channel->local_mtu = local_mtu; 2042 channel->remote_mtu = L2CAP_DEFAULT_MTU; 2043 channel->required_security_level = security_level; 2044 2045 // 2046 channel->local_cid = l2cap_next_local_cid(); 2047 channel->con_handle = HCI_CON_HANDLE_INVALID; 2048 2049 // set initial state 2050 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 2051 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 2052 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 2053 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 2054 2055 log_info("create channel %p, local_cid 0x%04x", channel, channel->local_cid); 2056 2057 return channel; 2058 } 2059 2060 static void l2cap_free_channel_entry(l2cap_channel_t * channel){ 2061 log_info("free channel %p, local_cid 0x%04x", channel, channel->local_cid); 2062 // assert all timers are stopped 2063 l2cap_stop_rtx(channel); 2064 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2065 l2cap_ertm_stop_retransmission_timer(channel); 2066 l2cap_ertm_stop_monitor_timer(channel); 2067 #endif 2068 // free memory 2069 btstack_memory_l2cap_channel_free(channel); 2070 } 2071 #endif 2072 2073 #ifdef ENABLE_CLASSIC 2074 2075 /** 2076 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 2077 * @param packet_handler 2078 * @param address 2079 * @param psm 2080 * @param mtu 2081 * @param local_cid 2082 */ 2083 2084 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){ 2085 // limit MTU to the size of our outgoing HCI buffer 2086 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 2087 2088 // determine security level based on psm 2089 const gap_security_level_t security_level = l2cap_security_level_0_allowed_for_PSM(psm) ? LEVEL_0 : gap_get_security_level(); 2090 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); 2091 2092 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); 2093 if (!channel) { 2094 return BTSTACK_MEMORY_ALLOC_FAILED; 2095 } 2096 2097 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2098 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2099 #endif 2100 2101 // add to connections list 2102 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2103 2104 // store local_cid 2105 if (out_local_cid){ 2106 *out_local_cid = channel->local_cid; 2107 } 2108 2109 // state: L2CAP_STATE_WILL_SEND_CREATE_CONNECTION 2110 2111 // check if hci connection is already usable, 2112 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_ACL); 2113 if (conn && conn->state == OPEN){ 2114 // simulate connection complete 2115 l2cap_handle_connection_complete(conn->con_handle, channel); 2116 2117 // state: L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES 2118 2119 // check if remote supported features are already received 2120 if (hci_remote_features_available(conn->con_handle)) { 2121 // simulate remote features received 2122 l2cap_handle_remote_supported_features_received(channel); 2123 } else { 2124 hci_remote_features_query(conn->con_handle); 2125 } 2126 } 2127 2128 l2cap_run(); 2129 2130 return ERROR_CODE_SUCCESS; 2131 } 2132 2133 void l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 2134 log_info("disconnect local_cid 0x%x reason 0x%x", local_cid, reason); 2135 UNUSED(reason); 2136 // find channel for local_cid 2137 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2138 if (channel) { 2139 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2140 } 2141 // process 2142 l2cap_run(); 2143 } 2144 2145 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 2146 // mark all channels before emitting open events as these could trigger new connetion requests to the same device 2147 btstack_linked_list_iterator_t it; 2148 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2149 while (btstack_linked_list_iterator_has_next(&it)){ 2150 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2151 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2152 if (bd_addr_cmp( channel->address, address) != 0) continue; 2153 // channel for this address found 2154 switch (channel->state){ 2155 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2156 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2157 channel->state = L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD; 2158 break; 2159 default: 2160 break; 2161 } 2162 } 2163 // emit and free marked entries. restart loop to deal with list changes 2164 int done = 0; 2165 while (!done) { 2166 done = 1; 2167 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2168 while (btstack_linked_list_iterator_has_next(&it)){ 2169 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2170 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2171 if (channel->state == L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD){ 2172 done = 0; 2173 // failure, forward error code 2174 l2cap_handle_channel_open_failed(channel, status); 2175 // discard channel 2176 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2177 l2cap_free_channel_entry(channel); 2178 break; 2179 } 2180 } 2181 } 2182 2183 } 2184 2185 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 2186 btstack_linked_list_iterator_t it; 2187 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2188 while (btstack_linked_list_iterator_has_next(&it)){ 2189 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2190 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2191 if ( ! bd_addr_cmp( channel->address, address) ){ 2192 l2cap_handle_connection_complete(handle, channel); 2193 } 2194 } 2195 // process 2196 l2cap_run(); 2197 } 2198 #endif 2199 2200 static bool l2cap_channel_ready_to_send(l2cap_channel_t * channel){ 2201 switch (channel->channel_type){ 2202 #ifdef ENABLE_CLASSIC 2203 case L2CAP_CHANNEL_TYPE_CLASSIC: 2204 if (channel->state != L2CAP_STATE_OPEN) return false; 2205 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2206 // send if we have more data and remote windows isn't full yet 2207 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2208 if (channel->unacked_frames >= btstack_min(channel->num_stored_tx_frames, channel->remote_tx_window_size)) return false; 2209 return hci_can_send_acl_classic_packet_now() != 0; 2210 } 2211 #endif 2212 if (!channel->waiting_for_can_send_now) return false; 2213 return (hci_can_send_acl_classic_packet_now() != 0); 2214 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2215 if (!channel->waiting_for_can_send_now) return false; 2216 return hci_can_send_acl_classic_packet_now() != 0; 2217 #endif 2218 #ifdef ENABLE_BLE 2219 case L2CAP_CHANNEL_TYPE_FIXED: 2220 if (!channel->waiting_for_can_send_now) return false; 2221 return hci_can_send_acl_le_packet_now() != 0; 2222 #ifdef ENABLE_LE_DATA_CHANNELS 2223 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2224 if (channel->state != L2CAP_STATE_OPEN) return false; 2225 if (channel->send_sdu_buffer == NULL) return false; 2226 if (channel->credits_outgoing == 0u) return false; 2227 return hci_can_send_acl_le_packet_now() != 0; 2228 #endif 2229 #endif 2230 default: 2231 return false; 2232 } 2233 } 2234 2235 static void l2cap_channel_trigger_send(l2cap_channel_t * channel){ 2236 switch (channel->channel_type){ 2237 #ifdef ENABLE_CLASSIC 2238 case L2CAP_CHANNEL_TYPE_CLASSIC: 2239 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2240 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2241 l2cap_ertm_channel_send_information_frame(channel); 2242 return; 2243 } 2244 #endif 2245 channel->waiting_for_can_send_now = 0; 2246 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2247 break; 2248 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2249 channel->waiting_for_can_send_now = 0; 2250 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2251 break; 2252 #endif 2253 #ifdef ENABLE_BLE 2254 case L2CAP_CHANNEL_TYPE_FIXED: 2255 channel->waiting_for_can_send_now = 0; 2256 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2257 break; 2258 #ifdef ENABLE_LE_DATA_CHANNELS 2259 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2260 l2cap_le_send_pdu(channel); 2261 break; 2262 #endif 2263 #endif 2264 default: 2265 break; 2266 } 2267 } 2268 2269 static void l2cap_notify_channel_can_send(void){ 2270 bool done = false; 2271 while (!done){ 2272 done = true; 2273 btstack_linked_list_iterator_t it; 2274 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2275 while (btstack_linked_list_iterator_has_next(&it)){ 2276 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2277 bool ready = l2cap_channel_ready_to_send(channel); 2278 if (!ready) continue; 2279 2280 // requeue channel for fairness 2281 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2282 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2283 2284 // trigger sending 2285 l2cap_channel_trigger_send(channel); 2286 2287 // exit inner loop as we just broke the iterator, but try again 2288 done = false; 2289 break; 2290 } 2291 } 2292 } 2293 2294 #ifdef L2CAP_USES_CHANNELS 2295 2296 static int l2cap_send_open_failed_on_hci_disconnect(l2cap_channel_t * channel){ 2297 // open cannot fail for for incoming connections 2298 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) return 0; 2299 2300 // check state 2301 switch (channel->state){ 2302 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2303 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2304 case L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES: 2305 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2306 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 2307 case L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES: 2308 case L2CAP_STATE_WAIT_CONNECT_RSP: 2309 case L2CAP_STATE_CONFIG: 2310 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 2311 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 2312 case L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE: 2313 case L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD: 2314 return 1; 2315 2316 case L2CAP_STATE_OPEN: 2317 case L2CAP_STATE_CLOSED: 2318 case L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES: 2319 case L2CAP_STATE_WAIT_DISCONNECT: 2320 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_INSUFFICIENT_SECURITY: 2321 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 2322 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 2323 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2324 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 2325 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 2326 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 2327 case L2CAP_STATE_INVALID: 2328 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2329 return 0; 2330 2331 default: 2332 // get a "warning" about new states 2333 btstack_assert(false); 2334 return 0; 2335 } 2336 } 2337 #endif 2338 2339 #ifdef ENABLE_CLASSIC 2340 static void l2cap_handle_hci_disconnect_event(l2cap_channel_t * channel){ 2341 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2342 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2343 } else { 2344 l2cap_handle_channel_closed(channel); 2345 } 2346 l2cap_free_channel_entry(channel); 2347 } 2348 #endif 2349 2350 #ifdef ENABLE_LE_DATA_CHANNELS 2351 static void l2cap_handle_hci_le_disconnect_event(l2cap_channel_t * channel){ 2352 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2353 l2cap_emit_le_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2354 } else { 2355 l2cap_emit_le_channel_closed(channel); 2356 } 2357 l2cap_free_channel_entry(channel); 2358 } 2359 #endif 2360 2361 #ifdef ENABLE_CLASSIC 2362 static void l2cap_check_classic_timeout(hci_con_handle_t handle){ 2363 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) { 2364 return; 2365 } 2366 if (hci_authentication_active_for_handle(handle)) { 2367 return; 2368 } 2369 bool hci_con_used = false; 2370 btstack_linked_list_iterator_t it; 2371 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2372 while (btstack_linked_list_iterator_has_next(&it)){ 2373 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2374 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2375 if (channel->con_handle != handle) continue; 2376 hci_con_used = true; 2377 break; 2378 } 2379 if (hci_con_used) { 2380 return; 2381 } 2382 if (!hci_can_send_command_packet_now()) { 2383 return; 2384 } 2385 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 2386 } 2387 2388 static void l2cap_handle_features_complete(hci_con_handle_t handle){ 2389 if (hci_remote_features_available(handle) == false){ 2390 return; 2391 } 2392 btstack_linked_list_iterator_t it; 2393 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2394 while (btstack_linked_list_iterator_has_next(&it)){ 2395 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2396 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2397 if (channel->con_handle != handle) continue; 2398 log_info("remote supported features, channel %p, cid %04x - state %u", channel, channel->local_cid, channel->state); 2399 l2cap_handle_remote_supported_features_received(channel); 2400 } 2401 } 2402 2403 static void l2cap_handle_security_level_incoming_sufficient(l2cap_channel_t * channel){ 2404 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2405 // we need to know if ERTM is supported before sending a config response 2406 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 2407 if (connection->l2cap_state.information_state != L2CAP_INFORMATION_STATE_DONE){ 2408 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2409 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 2410 return; 2411 } 2412 #endif 2413 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2414 l2cap_emit_incoming_connection(channel); 2415 } 2416 2417 static void l2cap_handle_security_level(hci_con_handle_t handle, gap_security_level_t actual_level){ 2418 log_info("security level update for handle 0x%04x", handle); 2419 btstack_linked_list_iterator_t it; 2420 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2421 while (btstack_linked_list_iterator_has_next(&it)){ 2422 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2423 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2424 if (channel->con_handle != handle) continue; 2425 2426 gap_security_level_t required_level = channel->required_security_level; 2427 2428 log_info("channel %p, cid %04x - state %u: actual %u >= required %u?", channel, channel->local_cid, channel->state, actual_level, required_level); 2429 2430 switch (channel->state){ 2431 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2432 if (actual_level >= required_level){ 2433 l2cap_handle_security_level_incoming_sufficient(channel); 2434 } else { 2435 channel->reason = 0x0003; // security block 2436 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2437 } 2438 break; 2439 2440 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2441 if (actual_level >= required_level){ 2442 l2cap_ready_to_connect(channel); 2443 } else { 2444 // security level insufficient, report error and free channel 2445 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY); 2446 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2447 l2cap_free_channel_entry(channel); 2448 } 2449 break; 2450 2451 default: 2452 break; 2453 } 2454 } 2455 } 2456 #endif 2457 2458 #ifdef L2CAP_USES_CHANNELS 2459 static void l2cap_handle_disconnection_complete(hci_con_handle_t handle){ 2460 // collect channels to close 2461 btstack_linked_list_t channels_to_close = NULL; 2462 btstack_linked_list_iterator_t it; 2463 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2464 while (btstack_linked_list_iterator_has_next(&it)) { 2465 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2466 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2467 if (channel->con_handle != handle) continue; 2468 btstack_linked_list_iterator_remove(&it); 2469 btstack_linked_list_add(&channels_to_close, (btstack_linked_item_t *) channel); 2470 } 2471 // send l2cap open failed or closed events for all channels on this handle and free them 2472 btstack_linked_list_iterator_init(&it, &channels_to_close); 2473 while (btstack_linked_list_iterator_has_next(&it)) { 2474 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2475 btstack_linked_list_iterator_remove(&it); 2476 switch(channel->channel_type){ 2477 #ifdef ENABLE_CLASSIC 2478 case L2CAP_CHANNEL_TYPE_CLASSIC: 2479 l2cap_handle_hci_disconnect_event(channel); 2480 break; 2481 #endif 2482 #ifdef ENABLE_LE_DATA_CHANNELS 2483 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2484 l2cap_handle_hci_le_disconnect_event(channel); 2485 break; 2486 #endif 2487 default: 2488 break; 2489 } 2490 } 2491 } 2492 #endif 2493 2494 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 2495 2496 UNUSED(packet_type); // ok: registered with hci_event_callback_registration 2497 UNUSED(cid); // ok: there is no channel 2498 UNUSED(size); // ok: fixed format events read from HCI buffer 2499 2500 #ifdef ENABLE_CLASSIC 2501 bd_addr_t address; 2502 gap_security_level_t security_level; 2503 #endif 2504 #ifdef L2CAP_USES_CHANNELS 2505 hci_con_handle_t handle; 2506 #endif 2507 2508 switch(hci_event_packet_get_type(packet)){ 2509 2510 // Notify channel packet handler if they can send now 2511 case HCI_EVENT_TRANSPORT_PACKET_SENT: 2512 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 2513 case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED: 2514 l2cap_run(); // try sending signaling packets first 2515 l2cap_notify_channel_can_send(); 2516 break; 2517 2518 case HCI_EVENT_COMMAND_STATUS: 2519 #ifdef ENABLE_CLASSIC 2520 // check command status for create connection for errors 2521 if (HCI_EVENT_IS_COMMAND_STATUS(packet, hci_create_connection)){ 2522 // cache outgoing address and reset 2523 (void)memcpy(address, l2cap_outgoing_classic_addr, 6); 2524 memset(l2cap_outgoing_classic_addr, 0, 6); 2525 // error => outgoing connection failed 2526 uint8_t status = hci_event_command_status_get_status(packet); 2527 if (status){ 2528 l2cap_handle_connection_failed_for_addr(address, status); 2529 } 2530 } 2531 #endif 2532 l2cap_run(); // try sending signaling packets first 2533 break; 2534 2535 #ifdef ENABLE_CLASSIC 2536 // handle connection complete events 2537 case HCI_EVENT_CONNECTION_COMPLETE: 2538 reverse_bd_addr(&packet[5], address); 2539 if (packet[2] == 0){ 2540 handle = little_endian_read_16(packet, 3); 2541 l2cap_handle_connection_success_for_addr(address, handle); 2542 } else { 2543 l2cap_handle_connection_failed_for_addr(address, packet[2]); 2544 } 2545 break; 2546 2547 // handle successful create connection cancel command 2548 case HCI_EVENT_COMMAND_COMPLETE: 2549 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 2550 if (packet[5] == 0){ 2551 reverse_bd_addr(&packet[6], address); 2552 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 2553 l2cap_handle_connection_failed_for_addr(address, 0x16); 2554 } 2555 } 2556 l2cap_run(); // try sending signaling packets first 2557 break; 2558 #endif 2559 2560 #ifdef L2CAP_USES_CHANNELS 2561 // handle disconnection complete events 2562 case HCI_EVENT_DISCONNECTION_COMPLETE: 2563 handle = little_endian_read_16(packet, 3); 2564 l2cap_handle_disconnection_complete(handle); 2565 break; 2566 #endif 2567 2568 // HCI Connection Timeouts 2569 #ifdef ENABLE_CLASSIC 2570 case L2CAP_EVENT_TIMEOUT_CHECK: 2571 handle = little_endian_read_16(packet, 2); 2572 l2cap_check_classic_timeout(handle); 2573 break; 2574 2575 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 2576 case HCI_EVENT_READ_REMOTE_EXTENDED_FEATURES_COMPLETE: 2577 handle = little_endian_read_16(packet, 3); 2578 l2cap_handle_features_complete(handle); 2579 break; 2580 2581 case GAP_EVENT_SECURITY_LEVEL: 2582 handle = little_endian_read_16(packet, 2); 2583 security_level = (gap_security_level_t) packet[4]; 2584 l2cap_handle_security_level(handle, security_level); 2585 break; 2586 #endif 2587 default: 2588 break; 2589 } 2590 2591 l2cap_run(); 2592 } 2593 2594 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 2595 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 2596 if (l2cap_signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 2597 l2cap_signaling_responses[l2cap_signaling_responses_pending].handle = handle; 2598 l2cap_signaling_responses[l2cap_signaling_responses_pending].code = code; 2599 l2cap_signaling_responses[l2cap_signaling_responses_pending].sig_id = sig_id; 2600 l2cap_signaling_responses[l2cap_signaling_responses_pending].cid = cid; 2601 l2cap_signaling_responses[l2cap_signaling_responses_pending].data = data; 2602 l2cap_signaling_responses_pending++; 2603 l2cap_run(); 2604 } 2605 } 2606 2607 #ifdef ENABLE_CLASSIC 2608 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 2609 switch (channel->state){ 2610 case L2CAP_STATE_CONFIG: 2611 case L2CAP_STATE_OPEN: 2612 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2613 case L2CAP_STATE_WAIT_DISCONNECT: 2614 break; 2615 default: 2616 // ignore in other states 2617 return; 2618 } 2619 2620 channel->remote_sig_id = identifier; 2621 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2622 l2cap_run(); 2623 } 2624 2625 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 2626 2627 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 2628 l2cap_service_t *service = l2cap_get_service(psm); 2629 if (!service) { 2630 // 0x0002 PSM not supported 2631 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2632 return; 2633 } 2634 2635 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 2636 if (!hci_connection) { 2637 // 2638 log_error("no hci_connection for handle %u", handle); 2639 return; 2640 } 2641 2642 // alloc structure 2643 gap_security_level_t required_level = service->required_security_level; 2644 if (gap_get_secure_connections_only_mode() && (required_level != LEVEL_0)){ 2645 required_level = LEVEL_4; 2646 } 2647 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CLASSIC, hci_connection->address, BD_ADDR_TYPE_ACL, 2648 psm, service->mtu, required_level); 2649 if (!channel){ 2650 // 0x0004 No resources available 2651 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2652 return; 2653 } 2654 2655 channel->con_handle = handle; 2656 channel->remote_cid = source_cid; 2657 channel->remote_sig_id = sig_id; 2658 2659 // limit local mtu to max acl packet length - l2cap header 2660 if (channel->local_mtu > l2cap_max_mtu()) { 2661 channel->local_mtu = l2cap_max_mtu(); 2662 } 2663 2664 // set initial state 2665 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 2666 channel->state_var = L2CAP_CHANNEL_STATE_VAR_INCOMING; 2667 2668 // add to connections list 2669 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2670 2671 // send conn resp pending if remote supported features have not been received yet 2672 if (hci_remote_features_available(handle)) { 2673 l2cap_handle_remote_supported_features_received(channel); 2674 } else { 2675 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND; 2676 hci_remote_features_query(handle); 2677 } 2678 } 2679 2680 void l2cap_accept_connection(uint16_t local_cid){ 2681 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 2682 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2683 if (!channel) { 2684 log_error("accept called but local_cid 0x%x not found", local_cid); 2685 return; 2686 } 2687 2688 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2689 // configure L2CAP Basic mode 2690 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2691 #endif 2692 2693 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 2694 2695 // process 2696 l2cap_run(); 2697 } 2698 2699 void l2cap_decline_connection(uint16_t local_cid){ 2700 log_info("decline local_cid 0x%x", local_cid); 2701 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 2702 if (!channel) { 2703 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 2704 return; 2705 } 2706 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2707 channel->reason = 0x04; // no resources available 2708 l2cap_run(); 2709 } 2710 2711 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2712 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 2713 2714 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2715 uint8_t use_fcs = 1; 2716 #endif 2717 2718 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2719 2720 uint16_t flags = little_endian_read_16(command, 6); 2721 if (flags & 1) { 2722 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 2723 } 2724 2725 // accept the other's configuration options 2726 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2727 uint16_t pos = 8; 2728 while (pos < end_pos){ 2729 uint8_t option_hint = command[pos] >> 7; 2730 uint8_t option_type = command[pos] & 0x7f; 2731 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2732 pos++; 2733 uint8_t length = command[pos++]; 2734 // MTU { type(8): 1, len(8):2, MTU(16) } 2735 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) && (length == 2)){ 2736 channel->remote_mtu = little_endian_read_16(command, pos); 2737 log_info("Remote MTU %u", channel->remote_mtu); 2738 if (channel->remote_mtu > l2cap_max_mtu()){ 2739 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 2740 channel->remote_mtu = l2cap_max_mtu(); 2741 } 2742 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2743 } 2744 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2745 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_FLUSH_TIMEOUT) && (length == 2)){ 2746 channel->flush_timeout = little_endian_read_16(command, pos); 2747 log_info("Flush timeout: %u ms", channel->flush_timeout); 2748 } 2749 2750 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2751 // Retransmission and Flow Control Option 2752 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2753 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2754 switch(channel->mode){ 2755 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2756 // Store remote config 2757 channel->remote_tx_window_size = command[pos+1]; 2758 channel->remote_max_transmit = command[pos+2]; 2759 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2760 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2761 channel->remote_mps = little_endian_read_16(command, pos + 7); 2762 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2763 channel->remote_tx_window_size, 2764 channel->remote_max_transmit, 2765 channel->remote_retransmission_timeout_ms, 2766 channel->remote_monitor_timeout_ms, 2767 channel->remote_mps); 2768 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2769 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2770 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2771 } else { 2772 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2773 } 2774 break; 2775 case L2CAP_CHANNEL_MODE_BASIC: 2776 switch (mode){ 2777 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2778 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2779 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2780 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2781 } 2782 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2783 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2784 break; 2785 default: // case L2CAP_CHANNEL_MODE_BASIC: 2786 // TODO store and evaluate configuration 2787 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2788 break; 2789 } 2790 break; 2791 default: 2792 break; 2793 } 2794 } 2795 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE && length == 1){ 2796 use_fcs = command[pos]; 2797 } 2798 #endif 2799 // check for unknown options 2800 if ((option_hint == 0) && ((option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) || (option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE))){ 2801 log_info("l2cap cid %u, unknown option 0x%02x", channel->local_cid, option_type); 2802 channel->unknown_option = option_type; 2803 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2804 } 2805 pos += length; 2806 } 2807 2808 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2809 // "FCS" has precedence over "No FCS" 2810 uint8_t update = channel->fcs_option || use_fcs; 2811 log_info("local fcs: %u, remote fcs: %u -> %u", channel->fcs_option, use_fcs, update); 2812 channel->fcs_option = update; 2813 // If ERTM mandatory, but remote didn't send Retransmission and Flowcontrol options -> disconnect 2814 if (((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM) == 0) & (channel->ertm_mandatory)){ 2815 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2816 } 2817 #endif 2818 } 2819 2820 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2821 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2822 log_info("l2cap_signaling_handle_configure_response"); 2823 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2824 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2825 uint16_t pos = 10; 2826 while (pos < end_pos){ 2827 uint8_t option_hint = command[pos] >> 7; 2828 uint8_t option_type = command[pos] & 0x7f; 2829 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2830 pos++; 2831 uint8_t length = command[pos++]; 2832 2833 // Retransmission and Flow Control Option 2834 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2835 switch (channel->mode){ 2836 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2837 if (channel->ertm_mandatory){ 2838 // ?? 2839 } else { 2840 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2841 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2842 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2843 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2844 } 2845 } 2846 break; 2847 case L2CAP_CHANNEL_MODE_BASIC: 2848 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2849 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2850 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2851 } 2852 break; 2853 default: 2854 break; 2855 } 2856 } 2857 2858 // check for unknown options 2859 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2860 log_info("l2cap cid %u, unknown option 0x%02x", channel->local_cid, option_type); 2861 channel->unknown_option = option_type; 2862 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2863 } 2864 2865 pos += length; 2866 } 2867 #else 2868 UNUSED(channel); // ok: no code 2869 UNUSED(result); // ok: no code 2870 UNUSED(command); // ok: no code 2871 #endif 2872 } 2873 2874 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2875 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2876 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2877 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2878 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2879 if (channel->state == L2CAP_STATE_OPEN) return 0; 2880 return 1; 2881 } 2882 2883 2884 // @pre command len is valid, see check in l2cap_signaling_handler_dispatch 2885 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2886 2887 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2888 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2889 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2890 uint16_t result = 0; 2891 2892 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2893 2894 // handle DISCONNECT REQUESTS seperately 2895 if (code == DISCONNECTION_REQUEST){ 2896 l2cap_handle_disconnect_request(channel, identifier); 2897 return; 2898 } 2899 2900 // @STATEMACHINE(l2cap) 2901 switch (channel->state) { 2902 2903 case L2CAP_STATE_WAIT_CONNECT_RSP: 2904 switch (code){ 2905 case CONNECTION_RESPONSE: 2906 if (cmd_len < 8){ 2907 // command imcomplete 2908 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2909 break; 2910 } 2911 l2cap_stop_rtx(channel); 2912 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2913 switch (result) { 2914 case 0: 2915 // successful connection 2916 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2917 channel->state = L2CAP_STATE_CONFIG; 2918 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2919 break; 2920 case 1: 2921 // connection pending. get some coffee, but start the ERTX 2922 l2cap_start_ertx(channel); 2923 break; 2924 default: 2925 // channel closed 2926 channel->state = L2CAP_STATE_CLOSED; 2927 // map l2cap connection response result to BTstack status enumeration 2928 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2929 2930 // drop link key if security block 2931 if ((L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result) == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2932 gap_drop_link_key_for_bd_addr(channel->address); 2933 } 2934 2935 // discard channel 2936 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2937 l2cap_free_channel_entry(channel); 2938 break; 2939 } 2940 break; 2941 2942 default: 2943 //@TODO: implement other signaling packets 2944 break; 2945 } 2946 break; 2947 2948 case L2CAP_STATE_CONFIG: 2949 switch (code) { 2950 case CONFIGURE_REQUEST: 2951 if (cmd_len < 4){ 2952 // command incomplete 2953 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2954 break; 2955 } 2956 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 2957 l2cap_signaling_handle_configure_request(channel, command); 2958 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 2959 // only done if continuation not set 2960 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 2961 } 2962 break; 2963 case CONFIGURE_RESPONSE: 2964 if (cmd_len < 6){ 2965 // command incomplete 2966 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2967 break; 2968 } 2969 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2970 l2cap_stop_rtx(channel); 2971 l2cap_signaling_handle_configure_response(channel, result, command); 2972 switch (result){ 2973 case 0: // success 2974 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 2975 break; 2976 case 4: // pending 2977 l2cap_start_ertx(channel); 2978 break; 2979 default: 2980 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2981 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 2982 // remote does not offer ertm but it's required 2983 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2984 break; 2985 } 2986 #endif 2987 // retry on negative result 2988 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2989 break; 2990 } 2991 break; 2992 default: 2993 break; 2994 } 2995 if (l2cap_channel_ready_for_open(channel)){ 2996 2997 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2998 // assert that packet can be stored in fragment buffers in ertm 2999 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3000 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 3001 uint16_t usable_mtu = channel->num_tx_buffers == 1 ? effective_mps : channel->num_tx_buffers * effective_mps - 2; 3002 if (usable_mtu < channel->remote_mtu){ 3003 log_info("Remote MTU %u > max storable ERTM packet, only using MTU = %u", channel->remote_mtu, usable_mtu); 3004 channel->remote_mtu = usable_mtu; 3005 } 3006 } 3007 #endif 3008 // for open: 3009 channel->state = L2CAP_STATE_OPEN; 3010 l2cap_emit_channel_opened(channel, 0); 3011 } 3012 break; 3013 3014 case L2CAP_STATE_WAIT_DISCONNECT: 3015 switch (code) { 3016 case DISCONNECTION_RESPONSE: 3017 l2cap_finialize_channel_close(channel); 3018 break; 3019 default: 3020 //@TODO: implement other signaling packets 3021 break; 3022 } 3023 break; 3024 3025 case L2CAP_STATE_CLOSED: 3026 // @TODO handle incoming requests 3027 break; 3028 3029 case L2CAP_STATE_OPEN: 3030 //@TODO: implement other signaling packets, e.g. re-configure 3031 break; 3032 default: 3033 break; 3034 } 3035 // log_info("new state %u", channel->state); 3036 } 3037 3038 3039 // @pre command len is valid, see check in l2cap_acl_classic_handler 3040 static void l2cap_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command){ 3041 3042 btstack_linked_list_iterator_t it; 3043 3044 // get code, signalind identifier and command len 3045 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3046 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 3047 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3048 3049 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 3050 if ((code < 1) || (code == ECHO_RESPONSE) || (code > INFORMATION_RESPONSE)){ 3051 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3052 return; 3053 } 3054 3055 // general commands without an assigned channel 3056 switch(code) { 3057 3058 case CONNECTION_REQUEST: 3059 if (cmd_len == 4){ 3060 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3061 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3062 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 3063 } else { 3064 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3065 } 3066 return; 3067 3068 case ECHO_REQUEST: 3069 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 3070 return; 3071 3072 case INFORMATION_REQUEST: 3073 if (cmd_len == 2) { 3074 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3075 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 3076 } else { 3077 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3078 } 3079 return; 3080 3081 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3082 case INFORMATION_RESPONSE: { 3083 hci_connection_t * connection = hci_connection_for_handle(handle); 3084 if (!connection) return; 3085 if (connection->l2cap_state.information_state != L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE) return; 3086 3087 // get extended features from response if valid 3088 connection->l2cap_state.extended_feature_mask = 0; 3089 if (cmd_len >= 6) { 3090 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3091 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3092 if (result == 0 && info_type == L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) { 3093 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3094 } 3095 } 3096 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 3097 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 3098 3099 // trigger connection request 3100 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3101 while (btstack_linked_list_iterator_has_next(&it)){ 3102 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3103 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3104 if (channel->con_handle != handle) continue; 3105 3106 // incoming connection: ask user for channel configuration, esp. if ertm will be mandatory 3107 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 3108 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3109 l2cap_emit_incoming_connection(channel); 3110 continue; 3111 } 3112 3113 // outgoing connection 3114 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 3115 3116 // if ERTM was requested, but is not listed in extended feature mask: 3117 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 3118 3119 if (channel->ertm_mandatory){ 3120 // bail if ERTM is mandatory 3121 channel->state = L2CAP_STATE_CLOSED; 3122 // map l2cap connection response result to BTstack status enumeration 3123 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 3124 // discard channel 3125 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3126 l2cap_free_channel_entry(channel); 3127 continue; 3128 3129 } else { 3130 // fallback to Basic mode 3131 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 3132 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 3133 } 3134 } 3135 3136 // respond to connection request 3137 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 3138 continue; 3139 } 3140 } 3141 return; 3142 } 3143 #endif 3144 3145 default: 3146 break; 3147 } 3148 3149 // Get potential destination CID 3150 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3151 3152 // Find channel for this sig_id and connection handle 3153 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3154 while (btstack_linked_list_iterator_has_next(&it)){ 3155 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3156 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3157 if (channel->con_handle != handle) continue; 3158 if (code & 1) { 3159 // match odd commands (responses) by previous signaling identifier 3160 if (channel->local_sig_id == sig_id) { 3161 l2cap_signaling_handler_channel(channel, command); 3162 break; 3163 } 3164 } else { 3165 // match even commands (requests) by local channel id 3166 if (channel->local_cid == dest_cid) { 3167 l2cap_signaling_handler_channel(channel, command); 3168 break; 3169 } 3170 } 3171 } 3172 } 3173 #endif 3174 3175 #ifdef ENABLE_BLE 3176 3177 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 3178 uint8_t event[6]; 3179 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 3180 event[1] = 4; 3181 little_endian_store_16(event, 2, con_handle); 3182 little_endian_store_16(event, 4, result); 3183 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3184 if (!l2cap_event_packet_handler) return; 3185 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 3186 } 3187 3188 // @returns valid 3189 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 3190 hci_connection_t * connection; 3191 uint16_t result; 3192 uint8_t event[12]; 3193 3194 #ifdef ENABLE_LE_DATA_CHANNELS 3195 btstack_linked_list_iterator_t it; 3196 l2cap_channel_t * channel; 3197 uint16_t local_cid; 3198 uint16_t le_psm; 3199 uint16_t new_credits; 3200 uint16_t credits_before; 3201 l2cap_service_t * service; 3202 uint16_t source_cid; 3203 #endif 3204 3205 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3206 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3207 log_info("le dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 3208 3209 switch (code){ 3210 3211 case CONNECTION_PARAMETER_UPDATE_REQUEST: 3212 // check size 3213 if (len < 8u) return 0u; 3214 connection = hci_connection_for_handle(handle); 3215 if (connection != NULL){ 3216 if (connection->role != HCI_ROLE_MASTER){ 3217 // reject command without notifying upper layer when not in master role 3218 return 0; 3219 } 3220 le_connection_parameter_range_t existing_range; 3221 gap_get_connection_parameter_range(&existing_range); 3222 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3223 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3224 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3225 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 3226 3227 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 3228 if (update_parameter){ 3229 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 3230 connection->le_conn_interval_min = le_conn_interval_min; 3231 connection->le_conn_interval_max = le_conn_interval_max; 3232 connection->le_conn_latency = le_conn_latency; 3233 connection->le_supervision_timeout = le_supervision_timeout; 3234 } else { 3235 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 3236 } 3237 connection->le_con_param_update_identifier = sig_id; 3238 } 3239 3240 if (!l2cap_event_packet_handler) break; 3241 3242 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 3243 event[1] = 8; 3244 little_endian_store_16(event, 2, handle); 3245 (void)memcpy(&event[4], &command[4], 8); 3246 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3247 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3248 break; 3249 3250 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 3251 // check size 3252 if (len < 2u) return 0u; 3253 result = little_endian_read_16(command, 4); 3254 l2cap_emit_connection_parameter_update_response(handle, result); 3255 break; 3256 3257 #ifdef ENABLE_LE_DATA_CHANNELS 3258 3259 case COMMAND_REJECT: 3260 // Find channel for this sig_id and connection handle 3261 channel = NULL; 3262 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3263 while (btstack_linked_list_iterator_has_next(&it)){ 3264 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3265 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3266 if (a_channel->con_handle != handle) continue; 3267 if (a_channel->local_sig_id != sig_id) continue; 3268 channel = a_channel; 3269 break; 3270 } 3271 if (!channel) break; 3272 3273 // if received while waiting for le connection response, assume legacy device 3274 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 3275 channel->state = L2CAP_STATE_CLOSED; 3276 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 3277 l2cap_emit_le_channel_opened(channel, 0x0002); 3278 3279 // discard channel 3280 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3281 l2cap_free_channel_entry(channel); 3282 break; 3283 } 3284 break; 3285 3286 case LE_CREDIT_BASED_CONNECTION_REQUEST: 3287 // check size 3288 if (len < 10u) return 0u; 3289 3290 // get hci connection, bail if not found (must not happen) 3291 connection = hci_connection_for_handle(handle); 3292 if (!connection) return 0; 3293 3294 // check if service registered 3295 le_psm = little_endian_read_16(command, 4); 3296 service = l2cap_le_get_service(le_psm); 3297 source_cid = little_endian_read_16(command, 6); 3298 3299 if (service){ 3300 if (source_cid < 0x40u){ 3301 // 0x0009 Connection refused - Invalid Source CID 3302 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 3303 return 1; 3304 } 3305 3306 // go through list of channels for this ACL connection and check if we get a match 3307 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3308 while (btstack_linked_list_iterator_has_next(&it)){ 3309 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3310 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3311 if (a_channel->con_handle != handle) continue; 3312 if (a_channel->remote_cid != source_cid) continue; 3313 // 0x000a Connection refused - Source CID already allocated 3314 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 3315 return 1; 3316 } 3317 3318 // security: check encryption 3319 if (service->required_security_level >= LEVEL_2){ 3320 if (gap_encryption_key_size(handle) == 0){ 3321 // 0x0008 Connection refused - insufficient encryption 3322 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 3323 return 1; 3324 } 3325 // anything less than 16 byte key size is insufficient 3326 if (gap_encryption_key_size(handle) < 16){ 3327 // 0x0007 Connection refused – insufficient encryption key size 3328 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 3329 return 1; 3330 } 3331 } 3332 3333 // security: check authencation 3334 if (service->required_security_level >= LEVEL_3){ 3335 if (!gap_authenticated(handle)){ 3336 // 0x0005 Connection refused – insufficient authentication 3337 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 3338 return 1; 3339 } 3340 } 3341 3342 // security: check authorization 3343 if (service->required_security_level >= LEVEL_4){ 3344 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 3345 // 0x0006 Connection refused – insufficient authorization 3346 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 3347 return 1; 3348 } 3349 } 3350 3351 // allocate channel 3352 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 3353 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 3354 if (!channel){ 3355 // 0x0004 Connection refused – no resources available 3356 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 3357 return 1; 3358 } 3359 3360 channel->con_handle = handle; 3361 channel->remote_cid = source_cid; 3362 channel->remote_sig_id = sig_id; 3363 channel->remote_mtu = little_endian_read_16(command, 8); 3364 channel->remote_mps = little_endian_read_16(command, 10); 3365 channel->credits_outgoing = little_endian_read_16(command, 12); 3366 3367 // set initial state 3368 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3369 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3370 3371 // add to connections list 3372 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 3373 3374 // post connection request event 3375 l2cap_emit_le_incoming_connection(channel); 3376 3377 } else { 3378 // Connection refused – LE_PSM not supported 3379 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 3380 } 3381 break; 3382 3383 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 3384 // check size 3385 if (len < 10u) return 0u; 3386 3387 // Find channel for this sig_id and connection handle 3388 channel = NULL; 3389 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3390 while (btstack_linked_list_iterator_has_next(&it)){ 3391 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3392 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3393 if (a_channel->con_handle != handle) continue; 3394 if (a_channel->local_sig_id != sig_id) continue; 3395 channel = a_channel; 3396 break; 3397 } 3398 if (!channel) break; 3399 3400 // cid + 0 3401 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 3402 if (result){ 3403 channel->state = L2CAP_STATE_CLOSED; 3404 // map l2cap connection response result to BTstack status enumeration 3405 l2cap_emit_le_channel_opened(channel, result); 3406 3407 // discard channel 3408 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3409 l2cap_free_channel_entry(channel); 3410 break; 3411 } 3412 3413 // success 3414 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3415 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3416 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3417 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3418 channel->state = L2CAP_STATE_OPEN; 3419 l2cap_emit_le_channel_opened(channel, result); 3420 break; 3421 3422 case LE_FLOW_CONTROL_CREDIT: 3423 // check size 3424 if (len < 4u) return 0u; 3425 3426 // find channel 3427 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3428 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3429 if (!channel) { 3430 log_error("credit: no channel for cid 0x%02x", local_cid); 3431 break; 3432 } 3433 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3434 credits_before = channel->credits_outgoing; 3435 channel->credits_outgoing += new_credits; 3436 // check for credit overrun 3437 if (credits_before > channel->credits_outgoing){ 3438 log_error("credit: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 3439 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3440 break; 3441 } 3442 log_info("credit: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 3443 break; 3444 3445 case DISCONNECTION_REQUEST: 3446 3447 // check size 3448 if (len < 4u) return 0u; 3449 3450 // find channel 3451 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3452 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3453 if (!channel) { 3454 log_error("credit: no channel for cid 0x%02x", local_cid); 3455 break; 3456 } 3457 channel->remote_sig_id = sig_id; 3458 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3459 break; 3460 3461 #endif 3462 3463 case DISCONNECTION_RESPONSE: 3464 break; 3465 3466 default: 3467 // command unknown -> reject command 3468 return 0; 3469 } 3470 return 1; 3471 } 3472 #endif 3473 3474 #ifdef ENABLE_CLASSIC 3475 static void l2cap_acl_classic_handler_for_channel(l2cap_channel_t * l2cap_channel, uint8_t * packet, uint16_t size){ 3476 3477 // forward data only in OPEN state 3478 if (l2cap_channel->state != L2CAP_STATE_OPEN) return; 3479 3480 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3481 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3482 3483 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3484 3485 // assert control + FCS fields are inside 3486 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) return; 3487 3488 if (l2cap_channel->fcs_option){ 3489 // verify FCS (required if one side requested it) 3490 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3491 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3492 3493 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 3494 // simulate fcs error 3495 static int counter = 0; 3496 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 3497 log_info("Simulate fcs error"); 3498 fcs_calculated++; 3499 counter = 0; 3500 } 3501 #endif 3502 3503 if (fcs_calculated == fcs_packet){ 3504 log_info("Packet FCS 0x%04x verified", fcs_packet); 3505 } else { 3506 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3507 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 3508 return; 3509 } 3510 } 3511 3512 // switch on packet type 3513 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3514 uint8_t req_seq = (control >> 8) & 0x3f; 3515 int final = (control >> 7) & 0x01; 3516 if (control & 1){ 3517 // S-Frame 3518 int poll = (control >> 4) & 0x01; 3519 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3520 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3521 l2cap_ertm_tx_packet_state_t * tx_state; 3522 switch (s){ 3523 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3524 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3525 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3526 if (poll && final){ 3527 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3528 log_error("P=F=1 in S-Frame"); 3529 break; 3530 } 3531 if (poll){ 3532 // check if we did request selective retransmission before <==> we have stored SDU segments 3533 int i; 3534 int num_stored_out_of_order_packets = 0; 3535 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3536 int index = l2cap_channel->rx_store_index + i; 3537 if (index >= l2cap_channel->num_rx_buffers){ 3538 index -= l2cap_channel->num_rx_buffers; 3539 } 3540 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3541 if (!rx_state->valid) continue; 3542 num_stored_out_of_order_packets++; 3543 } 3544 if (num_stored_out_of_order_packets){ 3545 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3546 } else { 3547 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3548 } 3549 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3550 } 3551 if (final){ 3552 // Stop-MonitorTimer 3553 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3554 // If UnackedFrames > 0 then Start-RetransTimer 3555 if (l2cap_channel->unacked_frames){ 3556 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3557 } 3558 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3559 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3560 } 3561 break; 3562 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3563 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3564 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3565 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3566 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3567 break; 3568 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3569 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3570 break; 3571 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3572 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3573 if (poll){ 3574 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3575 } 3576 // find requested i-frame 3577 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3578 if (tx_state){ 3579 log_info("Retransmission for tx_seq %u requested", req_seq); 3580 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3581 tx_state->retransmission_requested = 1; 3582 l2cap_channel->srej_active = 1; 3583 } 3584 break; 3585 default: 3586 break; 3587 } 3588 } else { 3589 // I-Frame 3590 // get control 3591 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3592 uint8_t tx_seq = (control >> 1) & 0x3f; 3593 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3594 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3595 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3596 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3597 if (final){ 3598 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3599 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3600 } 3601 3602 // get SDU 3603 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3604 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3605 3606 // assert SDU size is smaller or equal to our buffers 3607 uint16_t max_payload_size = 0; 3608 switch (sar){ 3609 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 3610 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 3611 // SDU Length + MPS 3612 max_payload_size = l2cap_channel->local_mps + 2; 3613 break; 3614 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 3615 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 3616 max_payload_size = l2cap_channel->local_mps; 3617 break; 3618 default: 3619 btstack_assert(false); 3620 break; 3621 } 3622 if (payload_len > max_payload_size){ 3623 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 3624 return; 3625 } 3626 3627 // check ordering 3628 if (l2cap_channel->expected_tx_seq == tx_seq){ 3629 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3630 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3631 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3632 3633 // process SDU 3634 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 3635 3636 // process stored segments 3637 while (true){ 3638 int index = l2cap_channel->rx_store_index; 3639 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3640 if (!rx_state->valid) break; 3641 3642 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3643 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3644 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3645 3646 rx_state->valid = 0; 3647 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3648 3649 // update rx store index 3650 index++; 3651 if (index >= l2cap_channel->num_rx_buffers){ 3652 index = 0; 3653 } 3654 l2cap_channel->rx_store_index = index; 3655 } 3656 3657 // 3658 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3659 3660 } else { 3661 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3662 if (delta < 2){ 3663 // store segment 3664 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 3665 3666 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3667 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3668 } else { 3669 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3670 l2cap_channel->send_supervisor_frame_reject = 1; 3671 } 3672 } 3673 } 3674 return; 3675 } 3676 #endif 3677 3678 // check size 3679 uint16_t payload_size = size - COMPLETE_L2CAP_HEADER; 3680 if (l2cap_channel->local_mtu < payload_size) return; 3681 3682 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], payload_size); 3683 } 3684 #endif 3685 3686 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3687 #ifdef ENABLE_CLASSIC 3688 l2cap_channel_t * l2cap_channel; 3689 l2cap_fixed_channel_t * l2cap_fixed_channel; 3690 3691 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3692 uint8_t broadcast_flag = READ_ACL_FLAGS(packet) >> 2; 3693 switch (channel_id) { 3694 3695 case L2CAP_CID_SIGNALING: { 3696 if (broadcast_flag != 0) break; 3697 uint32_t command_offset = 8; 3698 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 3699 // assert signaling command is fully inside packet 3700 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3701 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3702 if (next_command_offset > size){ 3703 log_error("l2cap signaling command len invalid -> drop"); 3704 break; 3705 } 3706 // handle signaling command 3707 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3708 // go to next command 3709 command_offset = next_command_offset; 3710 } 3711 break; 3712 } 3713 3714 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3715 if (broadcast_flag == 0) break; 3716 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3717 if (!l2cap_fixed_channel) break; 3718 if (!l2cap_fixed_channel->packet_handler) break; 3719 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3720 break; 3721 3722 #ifdef ENABLE_BLE 3723 case L2CAP_CID_BR_EDR_SECURITY_MANAGER: 3724 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3725 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 3726 // Pairing Failed 3727 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_BR_EDR_SECURITY_MANAGER, SM_REASON_PAIRING_NOT_SUPPORTED); 3728 } else { 3729 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3730 } 3731 break; 3732 #endif 3733 3734 default: 3735 if (broadcast_flag != 0) break; 3736 // Find channel for this channel_id and connection handle 3737 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3738 if (l2cap_channel != NULL){ 3739 l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size); 3740 } 3741 break; 3742 } 3743 #else 3744 UNUSED(handle); // ok: no code 3745 UNUSED(packet); // ok: no code 3746 UNUSED(size); // ok: no code 3747 #endif 3748 } 3749 3750 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3751 #ifdef ENABLE_BLE 3752 3753 l2cap_fixed_channel_t * l2cap_fixed_channel; 3754 3755 #ifdef ENABLE_LE_DATA_CHANNELS 3756 l2cap_channel_t * l2cap_channel; 3757 #endif 3758 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3759 switch (channel_id) { 3760 3761 case L2CAP_CID_SIGNALING_LE: { 3762 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3763 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3764 if ((COMPLETE_L2CAP_HEADER + 4u + len) > size) break; 3765 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3766 if (!valid){ 3767 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3768 } 3769 break; 3770 } 3771 3772 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3773 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3774 if (!l2cap_fixed_channel) break; 3775 if (!l2cap_fixed_channel->packet_handler) break; 3776 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3777 break; 3778 3779 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3780 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3781 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 3782 // Pairing Failed 3783 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, SM_REASON_PAIRING_NOT_SUPPORTED); 3784 } else { 3785 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3786 } 3787 break; 3788 3789 default: 3790 3791 #ifdef ENABLE_LE_DATA_CHANNELS 3792 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3793 if (l2cap_channel != NULL) { 3794 // credit counting 3795 if (l2cap_channel->credits_incoming == 0u){ 3796 log_info("LE Data Channel packet received but no incoming credits"); 3797 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3798 break; 3799 } 3800 l2cap_channel->credits_incoming--; 3801 3802 // automatic credits 3803 if ((l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK) && l2cap_channel->automatic_credits){ 3804 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3805 } 3806 3807 // first fragment 3808 uint16_t pos = 0; 3809 if (!l2cap_channel->receive_sdu_len){ 3810 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3811 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3812 l2cap_channel->receive_sdu_len = sdu_len; 3813 l2cap_channel->receive_sdu_pos = 0; 3814 pos += 2u; 3815 size -= 2u; 3816 } 3817 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3818 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3819 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3820 (void)memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], 3821 &packet[COMPLETE_L2CAP_HEADER + pos], 3822 fragment_size); 3823 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3824 // done? 3825 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3826 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3827 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3828 l2cap_channel->receive_sdu_len = 0; 3829 } 3830 } 3831 #endif 3832 break; 3833 } 3834 #else 3835 UNUSED(handle); // ok: no code 3836 UNUSED(packet); // ok: no code 3837 UNUSED(size); // ok: no code 3838 #endif 3839 } 3840 3841 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3842 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3843 UNUSED(channel); // ok: there is no channel 3844 3845 // Assert full L2CAP header present 3846 if (size < COMPLETE_L2CAP_HEADER) return; 3847 3848 // Dispatch to Classic or LE handler (SCO packets are not dispatched to L2CAP) 3849 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3850 hci_connection_t *conn = hci_connection_for_handle(handle); 3851 if (!conn) return; 3852 if (conn->address_type == BD_ADDR_TYPE_ACL){ 3853 l2cap_acl_classic_handler(handle, packet, size); 3854 } else { 3855 l2cap_acl_le_handler(handle, packet, size); 3856 } 3857 3858 l2cap_run(); 3859 } 3860 3861 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3862 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 3863 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3864 if (!channel) return; 3865 channel->packet_handler = packet_handler; 3866 } 3867 3868 #ifdef ENABLE_CLASSIC 3869 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3870 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3871 channel->state = L2CAP_STATE_CLOSED; 3872 l2cap_handle_channel_closed(channel); 3873 // discard channel 3874 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3875 l2cap_free_channel_entry(channel); 3876 } 3877 #endif 3878 3879 #ifdef L2CAP_USES_CHANNELS 3880 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3881 btstack_linked_list_iterator_t it; 3882 btstack_linked_list_iterator_init(&it, services); 3883 while (btstack_linked_list_iterator_has_next(&it)){ 3884 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3885 if ( service->psm == psm){ 3886 return service; 3887 }; 3888 } 3889 return NULL; 3890 } 3891 #endif 3892 3893 #ifdef ENABLE_CLASSIC 3894 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3895 return l2cap_get_service_internal(&l2cap_services, psm); 3896 } 3897 3898 static void l2cap_update_minimal_security_level(void){ 3899 // update minimal service security level 3900 gap_security_level_t minimal_level = LEVEL_1; 3901 btstack_linked_list_iterator_t it; 3902 btstack_linked_list_iterator_init(&it, &l2cap_services); 3903 while (btstack_linked_list_iterator_has_next(&it)){ 3904 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3905 if (service->required_security_level > minimal_level){ 3906 minimal_level = service->required_security_level; 3907 }; 3908 } 3909 gap_set_minimal_service_security_level(minimal_level); 3910 } 3911 3912 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3913 3914 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3915 3916 // check for alread registered psm 3917 l2cap_service_t *service = l2cap_get_service(psm); 3918 if (service) { 3919 log_error("register: PSM %u already registered", psm); 3920 return L2CAP_SERVICE_ALREADY_REGISTERED; 3921 } 3922 3923 // alloc structure 3924 service = btstack_memory_l2cap_service_get(); 3925 if (!service) { 3926 log_error("register: no memory for l2cap_service_t"); 3927 return BTSTACK_MEMORY_ALLOC_FAILED; 3928 } 3929 3930 // fill in 3931 service->psm = psm; 3932 service->mtu = mtu; 3933 service->packet_handler = service_packet_handler; 3934 service->required_security_level = security_level; 3935 3936 // add to services list 3937 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3938 3939 l2cap_update_minimal_security_level(); 3940 3941 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 3942 // enable page scan 3943 gap_connectable_control(1); 3944 #endif 3945 3946 3947 return ERROR_CODE_SUCCESS; 3948 } 3949 3950 uint8_t l2cap_unregister_service(uint16_t psm){ 3951 3952 log_info("unregister psm 0x%x", psm); 3953 3954 l2cap_service_t *service = l2cap_get_service(psm); 3955 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3956 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 3957 btstack_memory_l2cap_service_free(service); 3958 3959 l2cap_update_minimal_security_level(); 3960 3961 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 3962 // disable page scan when no services registered 3963 if (btstack_linked_list_empty(&l2cap_services)) { 3964 gap_connectable_control(0); 3965 } 3966 #endif 3967 3968 return ERROR_CODE_SUCCESS; 3969 } 3970 #endif 3971 3972 3973 #ifdef ENABLE_LE_DATA_CHANNELS 3974 3975 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 3976 if (!channel->waiting_for_can_send_now) return; 3977 if (channel->send_sdu_buffer) return; 3978 channel->waiting_for_can_send_now = 0; 3979 log_debug("le can send now, local_cid 0x%x", channel->local_cid); 3980 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 3981 } 3982 3983 // 1BH2222 3984 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 3985 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", 3986 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 3987 uint8_t event[19]; 3988 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 3989 event[1] = sizeof(event) - 2u; 3990 event[2] = channel->address_type; 3991 reverse_bd_addr(channel->address, &event[3]); 3992 little_endian_store_16(event, 9, channel->con_handle); 3993 little_endian_store_16(event, 11, channel->psm); 3994 little_endian_store_16(event, 13, channel->local_cid); 3995 little_endian_store_16(event, 15, channel->remote_cid); 3996 little_endian_store_16(event, 17, channel->remote_mtu); 3997 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3998 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3999 } 4000 // 11BH22222 4001 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 4002 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", 4003 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 4004 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 4005 uint8_t event[23]; 4006 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 4007 event[1] = sizeof(event) - 2u; 4008 event[2] = status; 4009 event[3] = channel->address_type; 4010 reverse_bd_addr(channel->address, &event[4]); 4011 little_endian_store_16(event, 10, channel->con_handle); 4012 event[12] = (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) ? 1 : 0; 4013 little_endian_store_16(event, 13, channel->psm); 4014 little_endian_store_16(event, 15, channel->local_cid); 4015 little_endian_store_16(event, 17, channel->remote_cid); 4016 little_endian_store_16(event, 19, channel->local_mtu); 4017 little_endian_store_16(event, 21, channel->remote_mtu); 4018 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4019 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4020 } 4021 // 2 4022 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 4023 log_info("closed local_cid 0x%x", channel->local_cid); 4024 uint8_t event[4]; 4025 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 4026 event[1] = sizeof(event) - 2u; 4027 little_endian_store_16(event, 2, channel->local_cid); 4028 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4029 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4030 } 4031 4032 static void l2cap_le_send_pdu(l2cap_channel_t *channel){ 4033 btstack_assert(channel != NULL); 4034 btstack_assert(channel->send_sdu_buffer != NULL); 4035 btstack_assert(channel->credits_outgoing > 0); 4036 4037 // send part of SDU 4038 hci_reserve_packet_buffer(); 4039 uint8_t * acl_buffer = hci_get_outgoing_packet_buffer(); 4040 uint8_t * l2cap_payload = acl_buffer + 8; 4041 uint16_t pos = 0; 4042 if (!channel->send_sdu_pos){ 4043 // store SDU len 4044 channel->send_sdu_pos += 2u; 4045 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 4046 pos += 2u; 4047 } 4048 uint16_t payload_size = btstack_min(channel->send_sdu_len + 2u - channel->send_sdu_pos, channel->remote_mps - pos); 4049 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 4050 (void)memcpy(&l2cap_payload[pos], 4051 &channel->send_sdu_buffer[channel->send_sdu_pos - 2u], 4052 payload_size); // -2 for virtual SDU len 4053 pos += payload_size; 4054 channel->send_sdu_pos += payload_size; 4055 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 4056 4057 channel->credits_outgoing--; 4058 4059 hci_send_acl_packet_buffer(8u + pos); 4060 4061 if (channel->send_sdu_pos >= (channel->send_sdu_len + 2u)){ 4062 channel->send_sdu_buffer = NULL; 4063 // send done event 4064 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 4065 // inform about can send now 4066 l2cap_le_notify_channel_can_send(channel); 4067 } 4068 } 4069 4070 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 4071 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 4072 channel->state = L2CAP_STATE_CLOSED; 4073 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 4074 // discard channel 4075 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4076 l2cap_free_channel_entry(channel); 4077 } 4078 4079 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 4080 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 4081 } 4082 4083 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 4084 4085 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 4086 4087 // check for alread registered psm 4088 l2cap_service_t *service = l2cap_le_get_service(psm); 4089 if (service) { 4090 return L2CAP_SERVICE_ALREADY_REGISTERED; 4091 } 4092 4093 // alloc structure 4094 service = btstack_memory_l2cap_service_get(); 4095 if (!service) { 4096 log_error("register: no memory for l2cap_service_t"); 4097 return BTSTACK_MEMORY_ALLOC_FAILED; 4098 } 4099 4100 // fill in 4101 service->psm = psm; 4102 service->mtu = 0; 4103 service->packet_handler = packet_handler; 4104 service->required_security_level = security_level; 4105 4106 // add to services list 4107 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 4108 4109 // done 4110 return ERROR_CODE_SUCCESS; 4111 } 4112 4113 uint8_t l2cap_le_unregister_service(uint16_t psm) { 4114 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 4115 l2cap_service_t *service = l2cap_le_get_service(psm); 4116 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 4117 4118 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 4119 btstack_memory_l2cap_service_free(service); 4120 return ERROR_CODE_SUCCESS; 4121 } 4122 4123 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 4124 // get channel 4125 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4126 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4127 4128 // validate state 4129 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 4130 return ERROR_CODE_COMMAND_DISALLOWED; 4131 } 4132 4133 // set state accept connection 4134 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 4135 channel->receive_sdu_buffer = receive_sdu_buffer; 4136 channel->local_mtu = mtu; 4137 channel->new_credits_incoming = initial_credits; 4138 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4139 4140 // test 4141 // channel->new_credits_incoming = 1; 4142 4143 // go 4144 l2cap_run(); 4145 return ERROR_CODE_SUCCESS; 4146 } 4147 4148 /** 4149 * @brief Deny incoming LE Data Channel connection due to resource constraints 4150 * @param local_cid L2CAP LE Data Channel Identifier 4151 */ 4152 4153 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 4154 // get channel 4155 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4156 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4157 4158 // validate state 4159 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 4160 return ERROR_CODE_COMMAND_DISALLOWED; 4161 } 4162 4163 // set state decline connection 4164 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 4165 channel->reason = 0x04; // no resources available 4166 l2cap_run(); 4167 return ERROR_CODE_SUCCESS; 4168 } 4169 4170 static gap_security_level_t l2cap_le_security_level_for_connection(hci_con_handle_t con_handle){ 4171 uint8_t encryption_key_size = gap_encryption_key_size(con_handle); 4172 if (encryption_key_size == 0) return LEVEL_0; 4173 4174 uint8_t authenticated = gap_authenticated(con_handle); 4175 if (!authenticated) return LEVEL_2; 4176 4177 return encryption_key_size == 16 ? LEVEL_4 : LEVEL_3; 4178 } 4179 4180 // used to handle pairing complete after triggering to increase 4181 static void l2cap_sm_packet_handler(uint8_t packet_type, uint16_t channel_nr, uint8_t *packet, uint16_t size) { 4182 UNUSED(channel_nr); 4183 UNUSED(size); 4184 UNUSED(packet_type); 4185 btstack_assert(packet_type == HCI_EVENT_PACKET); 4186 if (hci_event_packet_get_type(packet) != SM_EVENT_PAIRING_COMPLETE) return; 4187 hci_con_handle_t con_handle = sm_event_pairing_complete_get_handle(packet); 4188 btstack_linked_list_iterator_t it; 4189 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4190 while (btstack_linked_list_iterator_has_next(&it)) { 4191 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4192 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 4193 if (channel->con_handle != con_handle) continue; 4194 if (channel->state != L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE) continue; 4195 4196 // found channel, check security level 4197 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4198 // pairing failed or wasn't good enough, inform user 4199 l2cap_emit_le_channel_opened(channel, ERROR_CODE_INSUFFICIENT_SECURITY); 4200 // discard channel 4201 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4202 l2cap_free_channel_entry(channel); 4203 } else { 4204 // send conn request now 4205 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4206 l2cap_run(); 4207 } 4208 } 4209 } 4210 4211 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 4212 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 4213 uint16_t * out_local_cid) { 4214 4215 static btstack_packet_callback_registration_t sm_event_callback_registration; 4216 static bool sm_callback_registered = false; 4217 4218 log_info("create, handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 4219 4220 hci_connection_t * connection = hci_connection_for_handle(con_handle); 4221 if (!connection) { 4222 log_error("no hci_connection for handle 0x%04x", con_handle); 4223 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 4224 } 4225 4226 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); 4227 if (!channel) { 4228 return BTSTACK_MEMORY_ALLOC_FAILED; 4229 } 4230 log_info("created %p", channel); 4231 4232 // store local_cid 4233 if (out_local_cid){ 4234 *out_local_cid = channel->local_cid; 4235 } 4236 4237 // setup channel entry 4238 channel->con_handle = con_handle; 4239 channel->receive_sdu_buffer = receive_sdu_buffer; 4240 channel->new_credits_incoming = initial_credits; 4241 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4242 4243 // add to connections list 4244 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 4245 4246 // check security level 4247 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4248 if (!sm_callback_registered){ 4249 sm_callback_registered = true; 4250 // lazy registration for SM events 4251 sm_event_callback_registration.callback = &l2cap_sm_packet_handler; 4252 sm_add_event_handler(&sm_event_callback_registration); 4253 } 4254 4255 // start pairing 4256 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 4257 sm_request_pairing(con_handle); 4258 } else { 4259 // send conn request right away 4260 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4261 l2cap_run(); 4262 } 4263 4264 return ERROR_CODE_SUCCESS; 4265 } 4266 4267 /** 4268 * @brief Provide credtis for LE Data Channel 4269 * @param local_cid L2CAP LE Data Channel Identifier 4270 * @param credits Number additional credits for peer 4271 */ 4272 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 4273 4274 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4275 if (!channel) { 4276 log_error("le credits no channel for cid 0x%02x", local_cid); 4277 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4278 } 4279 4280 // check state 4281 if (channel->state != L2CAP_STATE_OPEN){ 4282 log_error("le credits but channel 0x%02x not open yet", local_cid); 4283 } 4284 4285 // assert incoming credits + credits <= 0xffff 4286 uint32_t total_credits = channel->credits_incoming; 4287 total_credits += channel->new_credits_incoming; 4288 total_credits += credits; 4289 if (total_credits > 0xffffu){ 4290 log_error("le credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 4291 channel->new_credits_incoming, credits); 4292 } 4293 4294 // set credits_granted 4295 channel->new_credits_incoming += credits; 4296 4297 // go 4298 l2cap_run(); 4299 return ERROR_CODE_SUCCESS; 4300 } 4301 4302 /** 4303 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 4304 * @param local_cid L2CAP LE Data Channel Identifier 4305 */ 4306 bool l2cap_le_can_send_now(uint16_t local_cid){ 4307 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4308 if (!channel) { 4309 log_error("le can send now, no channel for cid 0x%02x", local_cid); 4310 return false; 4311 } 4312 4313 // check state 4314 if (channel->state != L2CAP_STATE_OPEN) return false; 4315 4316 // check queue 4317 if (channel->send_sdu_buffer) return false; 4318 4319 // fine, go ahead 4320 return true; 4321 } 4322 4323 /** 4324 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 4325 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 4326 * so packet handler should be ready to handle it 4327 * @param local_cid L2CAP LE Data Channel Identifier 4328 */ 4329 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 4330 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4331 if (!channel) { 4332 log_error("can send now, no channel for cid 0x%02x", local_cid); 4333 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4334 } 4335 channel->waiting_for_can_send_now = 1; 4336 l2cap_le_notify_channel_can_send(channel); 4337 return ERROR_CODE_SUCCESS; 4338 } 4339 4340 /** 4341 * @brief Send data via LE Data Channel 4342 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 4343 * @param local_cid L2CAP LE Data Channel Identifier 4344 * @param data data to send 4345 * @param size data size 4346 */ 4347 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t size){ 4348 4349 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4350 if (!channel) { 4351 log_error("l2cap send, no channel for cid 0x%02x", local_cid); 4352 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4353 } 4354 4355 if (size > channel->remote_mtu){ 4356 log_error("l2cap send, cid 0x%02x, data length exceeds remote MTU.", local_cid); 4357 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 4358 } 4359 4360 if (channel->send_sdu_buffer){ 4361 log_info("l2cap send, cid 0x%02x, cannot send", local_cid); 4362 return BTSTACK_ACL_BUFFERS_FULL; 4363 } 4364 4365 channel->send_sdu_buffer = data; 4366 channel->send_sdu_len = size; 4367 channel->send_sdu_pos = 0; 4368 4369 l2cap_notify_channel_can_send(); 4370 return ERROR_CODE_SUCCESS; 4371 } 4372 4373 /** 4374 * @brief Disconnect from LE Data Channel 4375 * @param local_cid L2CAP LE Data Channel Identifier 4376 */ 4377 uint8_t l2cap_le_disconnect(uint16_t local_cid) 4378 { 4379 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4380 if (!channel) { 4381 log_error("l2cap send, no channel for cid 0x%02x", local_cid); 4382 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4383 } 4384 4385 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 4386 l2cap_run(); 4387 return ERROR_CODE_SUCCESS; 4388 } 4389 4390 #endif 4391