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