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