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