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