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