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