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