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