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