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