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