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 2353 bool security_required = channel->required_security_level > LEVEL_0; 2354 2355 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) != 0){ 2356 2357 // Core V5.2, Vol 3, Part C, 5.2.2.2 - Security Mode 4 2358 // When a remote device attempts to access a service offered by a Bluetooth device that is in security mode 4 2359 // and a sufficient link key exists and authentication has not been performed the local device shall authenticate 2360 // the remote device and enable encryption after the channel establishment request is received but before a channel 2361 // establishment confirmation (L2CAP_ConnectRsp with result code of 0x0000 or a higher-level channel establishment 2362 // confirmation such as that of RFCOMM) is sent. 2363 2364 // If the remote device has indicated support for Secure Simple Pairing, a channel establishment request is 2365 // received for a service other than SDP, and encryption has not yet been enabled, then the local device shall 2366 // disconnect the ACL link with error code 0x05 - Authentication Failure. 2367 2368 // => Disconnect if l2cap request received in mode 4 and ssp supported, non-sdp psm, not encrypted, no link key available 2369 if ((gap_get_security_mode() == GAP_SECURITY_MODE_4) 2370 && gap_ssp_supported_on_both_sides(channel->con_handle) 2371 && (channel->psm != PSM_SDP) 2372 && (gap_encryption_key_size(channel->con_handle) == 0)){ 2373 hci_disconnect_security_block(channel->con_handle); 2374 return; 2375 } 2376 2377 // incoming: assert security requirements 2378 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 2379 if (channel->required_security_level <= gap_security_level(channel->con_handle)){ 2380 l2cap_handle_security_level_incoming_sufficient(channel); 2381 } else { 2382 // send connection pending if not already done 2383 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONN_RESP_PEND) == 0){ 2384 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND; 2385 } 2386 gap_request_security_level(channel->con_handle, channel->required_security_level); 2387 } 2388 } else { 2389 // outgoing: we have been waiting for remote supported features 2390 if (security_required){ 2391 // request security level 2392 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 2393 gap_request_security_level(channel->con_handle, channel->required_security_level); 2394 } else { 2395 l2cap_ready_to_connect(channel); 2396 } 2397 } 2398 } 2399 #endif 2400 2401 #ifdef L2CAP_USES_CHANNELS 2402 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, 2403 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 2404 2405 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 2406 if (!channel) { 2407 return NULL; 2408 } 2409 2410 // fill in 2411 channel->packet_handler = packet_handler; 2412 channel->channel_type = channel_type; 2413 bd_addr_copy(channel->address, address); 2414 channel->address_type = address_type; 2415 channel->psm = psm; 2416 channel->local_mtu = local_mtu; 2417 channel->remote_mtu = L2CAP_DEFAULT_MTU; 2418 channel->required_security_level = security_level; 2419 2420 // 2421 channel->local_cid = l2cap_next_local_cid(); 2422 channel->con_handle = HCI_CON_HANDLE_INVALID; 2423 2424 // set initial state 2425 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 2426 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 2427 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 2428 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 2429 2430 log_info("create channel %p, local_cid 0x%04x", channel, channel->local_cid); 2431 2432 return channel; 2433 } 2434 2435 static void l2cap_free_channel_entry(l2cap_channel_t * channel){ 2436 log_info("free channel %p, local_cid 0x%04x", channel, channel->local_cid); 2437 // assert all timers are stopped 2438 l2cap_stop_rtx(channel); 2439 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2440 l2cap_ertm_stop_retransmission_timer(channel); 2441 l2cap_ertm_stop_monitor_timer(channel); 2442 #endif 2443 // free memory 2444 btstack_memory_l2cap_channel_free(channel); 2445 } 2446 #endif 2447 2448 #ifdef ENABLE_CLASSIC 2449 2450 /** 2451 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 2452 * @param packet_handler 2453 * @param address 2454 * @param psm 2455 * @param mtu 2456 * @param local_cid 2457 */ 2458 2459 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){ 2460 // limit MTU to the size of our outgoing HCI buffer 2461 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 2462 2463 // determine security level based on psm 2464 const gap_security_level_t security_level = l2cap_security_level_0_allowed_for_PSM(psm) ? LEVEL_0 : gap_get_security_level(); 2465 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); 2466 2467 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); 2468 if (!channel) { 2469 return BTSTACK_MEMORY_ALLOC_FAILED; 2470 } 2471 2472 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2473 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2474 #endif 2475 2476 // add to connections list 2477 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2478 2479 // store local_cid 2480 if (out_local_cid){ 2481 *out_local_cid = channel->local_cid; 2482 } 2483 2484 // state: L2CAP_STATE_WILL_SEND_CREATE_CONNECTION 2485 2486 // check if hci connection is already usable, 2487 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_ACL); 2488 if (conn && conn->state == OPEN){ 2489 // simulate connection complete 2490 l2cap_handle_connection_complete(conn->con_handle, channel); 2491 2492 // state: L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES or L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST 2493 2494 // simulate if remote supported features if requested and already received 2495 if ((channel->state == L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) && hci_remote_features_available(conn->con_handle)) { 2496 // simulate remote features received 2497 l2cap_handle_remote_supported_features_received(channel); 2498 } 2499 } 2500 2501 l2cap_run(); 2502 2503 return ERROR_CODE_SUCCESS; 2504 } 2505 2506 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 2507 // mark all channels before emitting open events as these could trigger new connetion requests to the same device 2508 btstack_linked_list_iterator_t it; 2509 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2510 while (btstack_linked_list_iterator_has_next(&it)){ 2511 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2512 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2513 if (bd_addr_cmp( channel->address, address) != 0) continue; 2514 // channel for this address found 2515 switch (channel->state){ 2516 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2517 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2518 channel->state = L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD; 2519 break; 2520 default: 2521 break; 2522 } 2523 } 2524 // emit and free marked entries. restart loop to deal with list changes 2525 int done = 0; 2526 while (!done) { 2527 done = 1; 2528 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2529 while (btstack_linked_list_iterator_has_next(&it)){ 2530 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2531 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2532 if (channel->state == L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD){ 2533 done = 0; 2534 // failure, forward error code 2535 l2cap_handle_channel_open_failed(channel, status); 2536 // discard channel 2537 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2538 l2cap_free_channel_entry(channel); 2539 break; 2540 } 2541 } 2542 } 2543 2544 } 2545 2546 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 2547 btstack_linked_list_iterator_t it; 2548 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2549 while (btstack_linked_list_iterator_has_next(&it)){ 2550 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2551 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2552 if ( ! bd_addr_cmp( channel->address, address) ){ 2553 l2cap_handle_connection_complete(handle, channel); 2554 } 2555 } 2556 // process 2557 l2cap_run(); 2558 } 2559 #endif 2560 2561 static bool l2cap_channel_ready_to_send(l2cap_channel_t * channel){ 2562 switch (channel->channel_type){ 2563 #ifdef ENABLE_CLASSIC 2564 case L2CAP_CHANNEL_TYPE_CLASSIC: 2565 if (channel->state != L2CAP_STATE_OPEN) return false; 2566 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2567 // send if we have more data and remote windows isn't full yet 2568 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2569 if (channel->unacked_frames >= btstack_min(channel->num_stored_tx_frames, channel->remote_tx_window_size)) return false; 2570 return hci_can_send_acl_classic_packet_now() != 0; 2571 } 2572 #endif 2573 if (!channel->waiting_for_can_send_now) return false; 2574 return (hci_can_send_acl_classic_packet_now() != 0); 2575 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2576 if (!channel->waiting_for_can_send_now) return false; 2577 return hci_can_send_acl_classic_packet_now() != 0; 2578 #endif 2579 #ifdef ENABLE_BLE 2580 case L2CAP_CHANNEL_TYPE_FIXED: 2581 if (!channel->waiting_for_can_send_now) return false; 2582 return hci_can_send_acl_le_packet_now() != 0; 2583 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 2584 case L2CAP_CHANNEL_TYPE_CHANNEL_CBM: 2585 if (channel->state != L2CAP_STATE_OPEN) return false; 2586 if (channel->send_sdu_buffer == NULL) return false; 2587 if (channel->credits_outgoing == 0u) return false; 2588 return hci_can_send_acl_le_packet_now() != 0; 2589 #endif 2590 #endif 2591 #ifdef ENABLE_L2CAP_ENHANCED_CREDIT_BASED_FLOW_CONTROL_MODE 2592 case L2CAP_CHANNEL_TYPE_CHANNEL_ECBM: 2593 if (channel->state != L2CAP_STATE_OPEN) return false; 2594 if (channel->send_sdu_buffer == NULL) return false; 2595 if (channel->credits_outgoing == 0u) return false; 2596 if (channel->address_type == BD_ADDR_TYPE_ACL) { 2597 return hci_can_send_acl_classic_packet_now() != 0; 2598 } else { 2599 return hci_can_send_acl_le_packet_now() != 0; 2600 } 2601 #endif 2602 default: 2603 return false; 2604 } 2605 } 2606 2607 static void l2cap_channel_trigger_send(l2cap_channel_t * channel){ 2608 switch (channel->channel_type){ 2609 #ifdef ENABLE_CLASSIC 2610 case L2CAP_CHANNEL_TYPE_CLASSIC: 2611 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2612 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2613 l2cap_ertm_channel_send_information_frame(channel); 2614 return; 2615 } 2616 #endif 2617 channel->waiting_for_can_send_now = 0; 2618 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2619 break; 2620 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2621 channel->waiting_for_can_send_now = 0; 2622 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2623 break; 2624 #endif 2625 #ifdef ENABLE_BLE 2626 case L2CAP_CHANNEL_TYPE_FIXED: 2627 channel->waiting_for_can_send_now = 0; 2628 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2629 break; 2630 #endif 2631 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 2632 case L2CAP_CHANNEL_TYPE_CHANNEL_CBM: 2633 l2cap_credit_based_send_pdu(channel); 2634 break; 2635 #endif 2636 #ifdef ENABLE_L2CAP_ENHANCED_CREDIT_BASED_FLOW_CONTROL_MODE 2637 case L2CAP_CHANNEL_TYPE_CHANNEL_ECBM: 2638 l2cap_credit_based_send_pdu(channel); 2639 break; 2640 #endif 2641 default: 2642 break; 2643 } 2644 } 2645 2646 static void l2cap_notify_channel_can_send(void){ 2647 bool done = false; 2648 while (!done){ 2649 done = true; 2650 btstack_linked_list_iterator_t it; 2651 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2652 while (btstack_linked_list_iterator_has_next(&it)){ 2653 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2654 bool ready = l2cap_channel_ready_to_send(channel); 2655 if (!ready) continue; 2656 2657 // requeue channel for fairness 2658 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2659 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2660 2661 // trigger sending 2662 l2cap_channel_trigger_send(channel); 2663 2664 // exit inner loop as we just broke the iterator, but try again 2665 done = false; 2666 break; 2667 } 2668 } 2669 } 2670 2671 #ifdef L2CAP_USES_CHANNELS 2672 2673 uint8_t l2cap_disconnect(uint16_t local_cid){ 2674 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2675 if (!channel) { 2676 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2677 } 2678 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2679 l2cap_run(); 2680 return ERROR_CODE_SUCCESS; 2681 } 2682 2683 static int l2cap_send_open_failed_on_hci_disconnect(l2cap_channel_t * channel){ 2684 // open cannot fail for for incoming connections 2685 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) return 0; 2686 2687 // check state 2688 switch (channel->state){ 2689 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2690 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2691 case L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES: 2692 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2693 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 2694 case L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES: 2695 case L2CAP_STATE_WAIT_CONNECT_RSP: 2696 case L2CAP_STATE_CONFIG: 2697 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 2698 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 2699 case L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE: 2700 case L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD: 2701 return 1; 2702 2703 case L2CAP_STATE_OPEN: 2704 case L2CAP_STATE_CLOSED: 2705 case L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES: 2706 case L2CAP_STATE_WAIT_DISCONNECT: 2707 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_INSUFFICIENT_SECURITY: 2708 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 2709 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 2710 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2711 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 2712 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 2713 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 2714 case L2CAP_STATE_INVALID: 2715 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2716 return 0; 2717 2718 default: 2719 // get a "warning" about new states 2720 btstack_assert(false); 2721 return 0; 2722 } 2723 } 2724 #endif 2725 2726 #ifdef ENABLE_CLASSIC 2727 static void l2cap_handle_hci_disconnect_event(l2cap_channel_t * channel){ 2728 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2729 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2730 } else { 2731 l2cap_handle_channel_closed(channel); 2732 } 2733 l2cap_free_channel_entry(channel); 2734 } 2735 #endif 2736 2737 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 2738 static void l2cap_handle_hci_le_disconnect_event(l2cap_channel_t * channel){ 2739 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2740 l2cap_cbm_emit_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2741 } else { 2742 l2cap_emit_channel_closed(channel); 2743 } 2744 l2cap_free_channel_entry(channel); 2745 } 2746 #endif 2747 2748 #ifdef ENABLE_CLASSIC 2749 static void l2cap_check_classic_timeout(hci_con_handle_t handle){ 2750 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) { 2751 return; 2752 } 2753 if (hci_authentication_active_for_handle(handle)) { 2754 return; 2755 } 2756 bool hci_con_used = false; 2757 btstack_linked_list_iterator_t it; 2758 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2759 while (btstack_linked_list_iterator_has_next(&it)){ 2760 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2761 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2762 if (channel->con_handle != handle) continue; 2763 hci_con_used = true; 2764 break; 2765 } 2766 if (hci_con_used) { 2767 return; 2768 } 2769 if (!hci_can_send_command_packet_now()) { 2770 return; 2771 } 2772 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 2773 } 2774 2775 static void l2cap_handle_features_complete(hci_con_handle_t handle){ 2776 if (hci_remote_features_available(handle) == false){ 2777 return; 2778 } 2779 btstack_linked_list_iterator_t it; 2780 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2781 while (btstack_linked_list_iterator_has_next(&it)){ 2782 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2783 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2784 if (channel->con_handle != handle) continue; 2785 log_info("remote supported features, channel %p, cid %04x - state %u", channel, channel->local_cid, channel->state); 2786 l2cap_handle_remote_supported_features_received(channel); 2787 } 2788 } 2789 2790 static void l2cap_handle_security_level_incoming_sufficient(l2cap_channel_t * channel){ 2791 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2792 l2cap_emit_incoming_connection(channel); 2793 } 2794 2795 static void l2cap_handle_security_level(hci_con_handle_t handle, gap_security_level_t actual_level){ 2796 log_info("security level update for handle 0x%04x", handle); 2797 2798 // trigger l2cap information requests 2799 if (actual_level > LEVEL_0){ 2800 hci_connection_t * hci_connection = hci_connection_for_handle(handle); 2801 btstack_assert(hci_connection != NULL); 2802 if (hci_connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 2803 hci_connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2804 } 2805 } 2806 2807 btstack_linked_list_iterator_t it; 2808 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2809 while (btstack_linked_list_iterator_has_next(&it)){ 2810 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2811 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2812 if (channel->con_handle != handle) continue; 2813 2814 gap_security_level_t required_level = channel->required_security_level; 2815 2816 log_info("channel %p, cid %04x - state %u: actual %u >= required %u?", channel, channel->local_cid, channel->state, actual_level, required_level); 2817 2818 switch (channel->state){ 2819 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2820 if (actual_level >= required_level){ 2821 l2cap_handle_security_level_incoming_sufficient(channel); 2822 } else { 2823 channel->reason = L2CAP_CONNECTION_RESULT_SECURITY_BLOCK; 2824 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2825 } 2826 break; 2827 2828 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2829 if (actual_level >= required_level){ 2830 l2cap_ready_to_connect(channel); 2831 } else { 2832 // security level insufficient, report error and free channel 2833 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY); 2834 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2835 l2cap_free_channel_entry(channel); 2836 } 2837 break; 2838 2839 default: 2840 break; 2841 } 2842 } 2843 } 2844 #endif 2845 2846 #ifdef L2CAP_USES_CHANNELS 2847 static void l2cap_handle_disconnection_complete(hci_con_handle_t handle){ 2848 // collect channels to close 2849 btstack_linked_list_t channels_to_close = NULL; 2850 btstack_linked_list_iterator_t it; 2851 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2852 while (btstack_linked_list_iterator_has_next(&it)) { 2853 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2854 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2855 if (channel->con_handle != handle) continue; 2856 btstack_linked_list_iterator_remove(&it); 2857 btstack_linked_list_add(&channels_to_close, (btstack_linked_item_t *) channel); 2858 } 2859 // send l2cap open failed or closed events for all channels on this handle and free them 2860 btstack_linked_list_iterator_init(&it, &channels_to_close); 2861 while (btstack_linked_list_iterator_has_next(&it)) { 2862 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2863 btstack_linked_list_iterator_remove(&it); 2864 switch(channel->channel_type){ 2865 #ifdef ENABLE_CLASSIC 2866 case L2CAP_CHANNEL_TYPE_CLASSIC: 2867 l2cap_handle_hci_disconnect_event(channel); 2868 break; 2869 #endif 2870 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 2871 case L2CAP_CHANNEL_TYPE_CHANNEL_CBM: 2872 l2cap_handle_hci_le_disconnect_event(channel); 2873 break; 2874 #endif 2875 #ifdef ENABLE_L2CAP_ENHANCED_CREDIT_BASED_FLOW_CONTROL_MODE 2876 case L2CAP_CHANNEL_TYPE_CHANNEL_ECBM: 2877 switch (channel->state) { 2878 case L2CAP_STATE_WILL_SEND_ENHANCED_CONNECTION_REQUEST: 2879 case L2CAP_STATE_WAIT_ENHANCED_CONNECTION_RESPONSE: 2880 // emit open failed if disconnected before connection complete 2881 l2cap_ecbm_emit_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2882 break; 2883 case L2CAP_STATE_WILL_SEND_EHNANCED_RENEGOTIATION_REQUEST: 2884 case L2CAP_STATE_WAIT_ENHANCED_RENEGOTIATION_RESPONSE: 2885 // emit reconfigure failure - result = 0xffff 2886 l2cap_ecbm_emit_reconfigure_complete(channel, 0xffff); 2887 break; 2888 default: 2889 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 2890 break; 2891 } 2892 l2cap_free_channel_entry(channel); 2893 break; 2894 #endif 2895 default: 2896 break; 2897 } 2898 } 2899 } 2900 #endif 2901 2902 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 2903 2904 UNUSED(packet_type); // ok: registered with hci_event_callback_registration 2905 UNUSED(cid); // ok: there is no channel 2906 UNUSED(size); // ok: fixed format events read from HCI buffer 2907 2908 #ifdef ENABLE_CLASSIC 2909 bd_addr_t address; 2910 gap_security_level_t security_level; 2911 #endif 2912 #ifdef L2CAP_USES_CHANNELS 2913 hci_con_handle_t handle; 2914 #endif 2915 2916 switch(hci_event_packet_get_type(packet)){ 2917 2918 // Notify channel packet handler if they can send now 2919 case HCI_EVENT_TRANSPORT_PACKET_SENT: 2920 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 2921 case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED: 2922 l2cap_call_notify_channel_in_run = true; 2923 break; 2924 2925 case HCI_EVENT_COMMAND_STATUS: 2926 #ifdef ENABLE_CLASSIC 2927 // check command status for create connection for errors 2928 if (HCI_EVENT_IS_COMMAND_STATUS(packet, hci_create_connection)){ 2929 // cache outgoing address and reset 2930 (void)memcpy(address, l2cap_outgoing_classic_addr, 6); 2931 memset(l2cap_outgoing_classic_addr, 0, 6); 2932 // error => outgoing connection failed 2933 uint8_t status = hci_event_command_status_get_status(packet); 2934 if (status){ 2935 l2cap_handle_connection_failed_for_addr(address, status); 2936 } 2937 } 2938 #endif 2939 l2cap_run(); // try sending signaling packets first 2940 break; 2941 2942 #ifdef ENABLE_CLASSIC 2943 // handle connection complete events 2944 case HCI_EVENT_CONNECTION_COMPLETE: 2945 reverse_bd_addr(&packet[5], address); 2946 if (packet[2] == 0){ 2947 handle = little_endian_read_16(packet, 3); 2948 l2cap_handle_connection_success_for_addr(address, handle); 2949 } else { 2950 l2cap_handle_connection_failed_for_addr(address, packet[2]); 2951 } 2952 break; 2953 2954 // handle successful create connection cancel command 2955 case HCI_EVENT_COMMAND_COMPLETE: 2956 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 2957 if (packet[5] == 0){ 2958 reverse_bd_addr(&packet[6], address); 2959 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 2960 l2cap_handle_connection_failed_for_addr(address, 0x16); 2961 } 2962 } 2963 l2cap_run(); // try sending signaling packets first 2964 break; 2965 #endif 2966 2967 #ifdef L2CAP_USES_CHANNELS 2968 // handle disconnection complete events 2969 case HCI_EVENT_DISCONNECTION_COMPLETE: 2970 handle = little_endian_read_16(packet, 3); 2971 l2cap_handle_disconnection_complete(handle); 2972 break; 2973 #endif 2974 2975 // HCI Connection Timeouts 2976 #ifdef ENABLE_CLASSIC 2977 case L2CAP_EVENT_TIMEOUT_CHECK: 2978 handle = little_endian_read_16(packet, 2); 2979 l2cap_check_classic_timeout(handle); 2980 break; 2981 2982 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 2983 case HCI_EVENT_READ_REMOTE_EXTENDED_FEATURES_COMPLETE: 2984 handle = little_endian_read_16(packet, 3); 2985 l2cap_handle_features_complete(handle); 2986 break; 2987 2988 case GAP_EVENT_SECURITY_LEVEL: 2989 handle = little_endian_read_16(packet, 2); 2990 security_level = (gap_security_level_t) packet[4]; 2991 l2cap_handle_security_level(handle, security_level); 2992 break; 2993 #endif 2994 default: 2995 break; 2996 } 2997 2998 l2cap_run(); 2999 } 3000 3001 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 3002 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indicates the connection was refused." 3003 if (l2cap_signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 3004 l2cap_signaling_responses[l2cap_signaling_responses_pending].handle = handle; 3005 l2cap_signaling_responses[l2cap_signaling_responses_pending].code = code; 3006 l2cap_signaling_responses[l2cap_signaling_responses_pending].sig_id = sig_id; 3007 l2cap_signaling_responses[l2cap_signaling_responses_pending].cid = cid; 3008 l2cap_signaling_responses[l2cap_signaling_responses_pending].data = data; 3009 l2cap_signaling_responses_pending++; 3010 l2cap_run(); 3011 } 3012 } 3013 3014 #ifdef ENABLE_CLASSIC 3015 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 3016 switch (channel->state){ 3017 case L2CAP_STATE_CONFIG: 3018 case L2CAP_STATE_OPEN: 3019 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 3020 case L2CAP_STATE_WAIT_DISCONNECT: 3021 break; 3022 default: 3023 // ignore in other states 3024 return; 3025 } 3026 3027 channel->remote_sig_id = identifier; 3028 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3029 l2cap_run(); 3030 } 3031 3032 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 3033 3034 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 3035 l2cap_service_t *service = l2cap_get_service(psm); 3036 if (!service) { 3037 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CONNECTION_RESULT_PSM_NOT_SUPPORTED); 3038 return; 3039 } 3040 3041 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 3042 if (!hci_connection) { 3043 // 3044 log_error("no hci_connection for handle %u", handle); 3045 return; 3046 } 3047 3048 // alloc structure 3049 gap_security_level_t required_level = service->required_security_level; 3050 if (gap_get_secure_connections_only_mode() && (required_level != LEVEL_0)){ 3051 required_level = LEVEL_4; 3052 } 3053 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CLASSIC, hci_connection->address, BD_ADDR_TYPE_ACL, 3054 psm, service->mtu, required_level); 3055 if (!channel){ 3056 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CONNECTION_RESULT_NO_RESOURCES_AVAILABLE); 3057 return; 3058 } 3059 3060 channel->con_handle = handle; 3061 channel->remote_cid = source_cid; 3062 channel->remote_sig_id = sig_id; 3063 3064 // limit local mtu to max acl packet length - l2cap header 3065 if (channel->local_mtu > l2cap_max_mtu()) { 3066 channel->local_mtu = l2cap_max_mtu(); 3067 } 3068 3069 // set initial state 3070 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 3071 channel->state_var = L2CAP_CHANNEL_STATE_VAR_INCOMING; 3072 3073 // add to connections list 3074 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 3075 3076 // 3077 if (required_level > LEVEL_0){ 3078 // send conn resp pending if remote supported features have not been received yet 3079 if (hci_remote_features_available(handle)) { 3080 l2cap_handle_remote_supported_features_received(channel); 3081 } else { 3082 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND; 3083 hci_remote_features_query(handle); 3084 } 3085 } else { 3086 l2cap_handle_security_level_incoming_sufficient(channel); 3087 } 3088 } 3089 3090 void l2cap_accept_connection(uint16_t local_cid){ 3091 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 3092 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3093 if (!channel) { 3094 log_error("accept called but local_cid 0x%x not found", local_cid); 3095 return; 3096 } 3097 3098 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3099 // configure L2CAP Basic mode 3100 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 3101 #endif 3102 3103 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 3104 3105 // process 3106 l2cap_run(); 3107 } 3108 3109 void l2cap_decline_connection(uint16_t local_cid){ 3110 log_info("decline local_cid 0x%x", local_cid); 3111 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 3112 if (!channel) { 3113 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 3114 return; 3115 } 3116 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 3117 channel->reason = L2CAP_CONNECTION_RESULT_NO_RESOURCES_AVAILABLE; 3118 l2cap_run(); 3119 } 3120 3121 // @pre command len is valid, see check in l2cap_signaling_handler_channel 3122 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 3123 3124 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3125 uint8_t use_fcs = 1; 3126 #endif 3127 3128 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 3129 3130 uint16_t flags = little_endian_read_16(command, 6); 3131 if (flags & 1) { 3132 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 3133 } 3134 3135 // accept the other's configuration options 3136 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3137 uint16_t pos = 8; 3138 while (pos < end_pos){ 3139 uint8_t option_hint = command[pos] >> 7; 3140 uint8_t option_type = command[pos] & 0x7f; 3141 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 3142 pos++; 3143 uint8_t length = command[pos++]; 3144 // MTU { type(8): 1, len(8):2, MTU(16) } 3145 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) && (length == 2)){ 3146 channel->remote_mtu = little_endian_read_16(command, pos); 3147 log_info("Remote MTU %u", channel->remote_mtu); 3148 if (channel->remote_mtu > l2cap_max_mtu()){ 3149 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 3150 channel->remote_mtu = l2cap_max_mtu(); 3151 } 3152 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 3153 } 3154 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 3155 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_FLUSH_TIMEOUT) && (length == 2)){ 3156 channel->flush_timeout = little_endian_read_16(command, pos); 3157 log_info("Flush timeout: %u ms", channel->flush_timeout); 3158 } 3159 3160 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3161 // Retransmission and Flow Control Option 3162 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 3163 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 3164 switch(channel->mode){ 3165 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 3166 // Store remote config 3167 channel->remote_tx_window_size = command[pos+1]; 3168 channel->remote_max_transmit = command[pos+2]; 3169 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 3170 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 3171 channel->remote_mps = little_endian_read_16(command, pos + 7); 3172 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 3173 channel->remote_tx_window_size, 3174 channel->remote_max_transmit, 3175 channel->remote_retransmission_timeout_ms, 3176 channel->remote_monitor_timeout_ms, 3177 channel->remote_mps); 3178 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 3179 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3180 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3181 } else { 3182 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 3183 } 3184 break; 3185 case L2CAP_CHANNEL_MODE_BASIC: 3186 switch (mode){ 3187 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 3188 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 3189 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 3190 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3191 } 3192 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 3193 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 3194 break; 3195 default: // case L2CAP_CHANNEL_MODE_BASIC: 3196 // TODO store and evaluate configuration 3197 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 3198 break; 3199 } 3200 break; 3201 default: 3202 break; 3203 } 3204 } 3205 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE && length == 1){ 3206 use_fcs = command[pos]; 3207 } 3208 #endif 3209 // check for unknown options 3210 if ((option_hint == 0) && ((option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) || (option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE))){ 3211 log_info("l2cap cid %u, unknown option 0x%02x", channel->local_cid, option_type); 3212 channel->unknown_option = option_type; 3213 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 3214 } 3215 pos += length; 3216 } 3217 3218 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3219 // "FCS" has precedence over "No FCS" 3220 uint8_t update = channel->fcs_option || use_fcs; 3221 log_info("local fcs: %u, remote fcs: %u -> %u", channel->fcs_option, use_fcs, update); 3222 channel->fcs_option = update; 3223 // If ERTM mandatory, but remote didn't send Retransmission and Flowcontrol options -> disconnect 3224 if (((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM) == 0) & (channel->ertm_mandatory)){ 3225 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3226 } 3227 #endif 3228 } 3229 3230 // @pre command len is valid, see check in l2cap_signaling_handler_channel 3231 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 3232 log_info("l2cap_signaling_handle_configure_response"); 3233 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3234 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3235 uint16_t pos = 10; 3236 while (pos < end_pos){ 3237 uint8_t option_hint = command[pos] >> 7; 3238 uint8_t option_type = command[pos] & 0x7f; 3239 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 3240 pos++; 3241 uint8_t length = command[pos++]; 3242 3243 // Retransmission and Flow Control Option 3244 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 3245 switch (channel->mode){ 3246 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 3247 if (channel->ertm_mandatory){ 3248 // ?? 3249 } else { 3250 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 3251 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 3252 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 3253 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 3254 } 3255 } 3256 break; 3257 case L2CAP_CHANNEL_MODE_BASIC: 3258 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 3259 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 3260 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3261 } 3262 break; 3263 default: 3264 break; 3265 } 3266 } 3267 3268 // check for unknown options 3269 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 3270 log_info("l2cap cid %u, unknown option 0x%02x", channel->local_cid, option_type); 3271 channel->unknown_option = option_type; 3272 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 3273 } 3274 3275 pos += length; 3276 } 3277 #else 3278 UNUSED(channel); // ok: no code 3279 UNUSED(result); // ok: no code 3280 UNUSED(command); // ok: no code 3281 #endif 3282 } 3283 3284 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 3285 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 3286 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 3287 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 3288 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 3289 if (channel->state == L2CAP_STATE_OPEN) return 0; 3290 return 1; 3291 } 3292 3293 3294 // @pre command len is valid, see check in l2cap_signaling_handler_dispatch 3295 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 3296 3297 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3298 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 3299 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3300 uint16_t result = 0; 3301 3302 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 3303 3304 // handle DISCONNECT REQUESTS seperately 3305 if (code == DISCONNECTION_REQUEST){ 3306 l2cap_handle_disconnect_request(channel, identifier); 3307 return; 3308 } 3309 3310 // @STATEMACHINE(l2cap) 3311 switch (channel->state) { 3312 3313 case L2CAP_STATE_WAIT_CONNECT_RSP: 3314 switch (code){ 3315 case CONNECTION_RESPONSE: 3316 if (cmd_len < 8){ 3317 // command imcomplete 3318 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 3319 break; 3320 } 3321 l2cap_stop_rtx(channel); 3322 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3323 switch (result) { 3324 case 0: 3325 // successful connection 3326 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3327 channel->state = L2CAP_STATE_CONFIG; 3328 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 3329 break; 3330 case 1: 3331 // connection pending. get some coffee, but start the ERTX 3332 l2cap_start_ertx(channel); 3333 break; 3334 default: 3335 // channel closed 3336 channel->state = L2CAP_STATE_CLOSED; 3337 // map l2cap connection response result to BTstack status enumeration 3338 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 3339 3340 // drop link key if security block 3341 if ((L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result) == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 3342 gap_drop_link_key_for_bd_addr(channel->address); 3343 } 3344 3345 // discard channel 3346 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3347 l2cap_free_channel_entry(channel); 3348 break; 3349 } 3350 break; 3351 3352 default: 3353 //@TODO: implement other signaling packets 3354 break; 3355 } 3356 break; 3357 3358 case L2CAP_STATE_CONFIG: 3359 switch (code) { 3360 case CONFIGURE_REQUEST: 3361 if (cmd_len < 4){ 3362 // command incomplete 3363 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 3364 break; 3365 } 3366 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 3367 l2cap_signaling_handle_configure_request(channel, command); 3368 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 3369 // only done if continuation not set 3370 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 3371 } 3372 break; 3373 case CONFIGURE_RESPONSE: 3374 if (cmd_len < 6){ 3375 // command incomplete 3376 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 3377 break; 3378 } 3379 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3380 l2cap_stop_rtx(channel); 3381 l2cap_signaling_handle_configure_response(channel, result, command); 3382 switch (result){ 3383 case 0: // success 3384 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 3385 break; 3386 case 4: // pending 3387 l2cap_start_ertx(channel); 3388 break; 3389 default: 3390 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3391 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 3392 // remote does not offer ertm but it's required 3393 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3394 break; 3395 } 3396 #endif 3397 // retry on negative result 3398 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 3399 break; 3400 } 3401 break; 3402 default: 3403 break; 3404 } 3405 if (l2cap_channel_ready_for_open(channel)){ 3406 3407 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3408 // assert that packet can be stored in fragment buffers in ertm 3409 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3410 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 3411 uint16_t usable_mtu = channel->num_tx_buffers == 1 ? effective_mps : channel->num_tx_buffers * effective_mps - 2; 3412 if (usable_mtu < channel->remote_mtu){ 3413 log_info("Remote MTU %u > max storable ERTM packet, only using MTU = %u", channel->remote_mtu, usable_mtu); 3414 channel->remote_mtu = usable_mtu; 3415 } 3416 } 3417 #endif 3418 // for open: 3419 channel->state = L2CAP_STATE_OPEN; 3420 l2cap_emit_channel_opened(channel, 0); 3421 } 3422 break; 3423 3424 case L2CAP_STATE_WAIT_DISCONNECT: 3425 switch (code) { 3426 case DISCONNECTION_RESPONSE: 3427 l2cap_finalize_channel_close(channel); 3428 break; 3429 default: 3430 //@TODO: implement other signaling packets 3431 break; 3432 } 3433 break; 3434 3435 case L2CAP_STATE_CLOSED: 3436 // @TODO handle incoming requests 3437 break; 3438 3439 case L2CAP_STATE_OPEN: 3440 //@TODO: implement other signaling packets, e.g. re-configure 3441 break; 3442 default: 3443 break; 3444 } 3445 // log_info("new state %u", channel->state); 3446 } 3447 3448 #ifdef ENABLE_CLASSIC 3449 static void l2cap_handle_information_request_complete(hci_connection_t * connection){ 3450 3451 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 3452 3453 // emit event 3454 uint8_t event[8]; 3455 event[0] = L2CAP_EVENT_INFORMATION_RESPONSE; 3456 event[1] = sizeof(event) - 2; 3457 little_endian_store_16(event, 2, connection->con_handle); 3458 little_endian_store_16(event, 4, connection->l2cap_state.extended_feature_mask); 3459 little_endian_store_16(event, 6, connection->l2cap_state.fixed_channels_supported); 3460 l2cap_emit_event(event, sizeof(event)); 3461 3462 // trigger connection request 3463 btstack_linked_list_iterator_t it; 3464 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3465 while (btstack_linked_list_iterator_has_next(&it)){ 3466 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3467 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3468 if (channel->con_handle != connection->con_handle) continue; 3469 3470 // incoming connection: information request was triggered after user has accepted connection, 3471 // now: verify channel configuration, esp. if ertm will be mandatory 3472 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 3473 // default: continue 3474 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 3475 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3476 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 3477 // ERTM not possible, select basic mode and release buffer 3478 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 3479 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 3480 3481 // bail if ERTM is mandatory 3482 if (channel->ertm_mandatory){ 3483 // We chose 'no resources available' for "ERTM mandatory but you don't even know ERTM exists" 3484 log_info("ERTM mandatory -> reject connection"); 3485 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 3486 channel->reason = L2CAP_CONNECTION_RESULT_NO_RESOURCES_AVAILABLE; 3487 } else { 3488 log_info("ERTM not supported by remote -> use Basic mode"); 3489 } 3490 } 3491 #endif 3492 continue; 3493 } 3494 3495 // outgoing connection: information request is triggered before connection request 3496 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 3497 3498 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3499 // if ERTM was requested, but is not listed in extended feature mask: 3500 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 3501 3502 if (channel->ertm_mandatory){ 3503 // bail if ERTM is mandatory 3504 channel->state = L2CAP_STATE_CLOSED; 3505 // map l2cap connection response result to BTstack status enumeration 3506 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 3507 // discard channel 3508 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3509 l2cap_free_channel_entry(channel); 3510 continue; 3511 3512 } else { 3513 // fallback to Basic mode 3514 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 3515 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 3516 } 3517 } 3518 #endif 3519 3520 // respond to connection request 3521 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 3522 continue; 3523 } 3524 } 3525 } 3526 #endif 3527 3528 // @pre command len is valid, see check in l2cap_acl_classic_handler 3529 static void l2cap_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command){ 3530 3531 hci_connection_t * connection; 3532 btstack_linked_list_iterator_t it; 3533 3534 // get code, signal identifier and command len 3535 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3536 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 3537 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3538 3539 // general commands without an assigned channel 3540 switch(code) { 3541 3542 case CONNECTION_REQUEST: 3543 if (cmd_len == 4){ 3544 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3545 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3546 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 3547 } else { 3548 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3549 } 3550 return; 3551 3552 case ECHO_REQUEST: 3553 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 3554 return; 3555 3556 case INFORMATION_REQUEST: 3557 if (cmd_len == 2) { 3558 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3559 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 3560 return; 3561 } 3562 break; 3563 3564 case INFORMATION_RESPONSE: 3565 connection = hci_connection_for_handle(handle); 3566 if (!connection) return; 3567 switch (connection->l2cap_state.information_state){ 3568 case L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE: 3569 // get extended features from response if valid 3570 if (cmd_len >= 6) { 3571 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3572 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3573 if (result == 0 && info_type == L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) { 3574 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3575 } 3576 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 3577 if ((connection->l2cap_state.extended_feature_mask & 0x80) != 0){ 3578 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_FIXED_CHANNELS_REQUEST; 3579 } else { 3580 // information request complete 3581 l2cap_handle_information_request_complete(connection); 3582 } 3583 } 3584 break; 3585 case L2CAP_INFORMATION_STATE_W4_FIXED_CHANNELS_RESPONSE: 3586 if (cmd_len >= 12) { 3587 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3588 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3589 if (result == 0 && info_type == L2CAP_INFO_TYPE_FIXED_CHANNELS_SUPPORTED) { 3590 connection->l2cap_state.fixed_channels_supported = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3591 } 3592 log_info("fixed channels mask 0x%02x", connection->l2cap_state.fixed_channels_supported); 3593 // information request complete 3594 l2cap_handle_information_request_complete(connection); 3595 } 3596 break; 3597 default: 3598 break; 3599 } 3600 return; 3601 3602 #ifdef ENABLE_L2CAP_ENHANCED_CREDIT_BASED_FLOW_CONTROL_MODE 3603 case L2CAP_CREDIT_BASED_CONNECTION_REQUEST: 3604 case L2CAP_CREDIT_BASED_CONNECTION_RESPONSE: 3605 case L2CAP_CREDIT_BASED_RECONFIGURE_REQUEST: 3606 case L2CAP_CREDIT_BASED_RECONFIGURE_RESPONSE: 3607 l2cap_ecbm_signaling_handler_dispatch(handle, L2CAP_CID_SIGNALING, command, sig_id); 3608 return; 3609 case L2CAP_FLOW_CONTROL_CREDIT_INDICATION: 3610 // return if valid 3611 if (l2cap_credit_based_handle_credit_indication(handle, command, cmd_len)) return; 3612 break; 3613 3614 case COMMAND_REJECT: 3615 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3616 while (btstack_linked_list_iterator_has_next(&it)) { 3617 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3618 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3619 if (channel->con_handle != handle) continue; 3620 if (channel->local_sig_id != sig_id) continue; 3621 if (channel->state != L2CAP_STATE_WAIT_ENHANCED_CONNECTION_RESPONSE) continue; 3622 3623 // open failed 3624 channel->state = L2CAP_STATE_CLOSED; 3625 l2cap_ecbm_emit_channel_opened(channel, 3626 ERROR_CODE_CONNECTION_REJECTED_DUE_TO_LIMITED_RESOURCES); 3627 // drop failed channel 3628 btstack_linked_list_iterator_remove(&it); 3629 l2cap_free_channel_entry(channel); 3630 } 3631 break; 3632 #endif 3633 3634 default: 3635 break; 3636 } 3637 3638 // Get potential destination CID 3639 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3640 3641 // Find channel for this sig_id and connection handle 3642 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3643 while (btstack_linked_list_iterator_has_next(&it)){ 3644 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3645 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3646 if (channel->con_handle != handle) continue; 3647 if (code & 1) { 3648 // match odd commands (responses) by previous signaling identifier 3649 if (channel->local_sig_id == sig_id) { 3650 l2cap_signaling_handler_channel(channel, command); 3651 return; 3652 } 3653 } else { 3654 // match even commands (requests) by local channel id 3655 if (channel->local_cid == dest_cid) { 3656 l2cap_signaling_handler_channel(channel, command); 3657 return; 3658 } 3659 } 3660 } 3661 3662 // send command reject 3663 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3664 } 3665 #endif 3666 3667 #ifdef L2CAP_USES_CHANNELS 3668 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3669 btstack_linked_list_iterator_t it; 3670 btstack_linked_list_iterator_init(&it, services); 3671 while (btstack_linked_list_iterator_has_next(&it)){ 3672 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3673 if ( service->psm == psm){ 3674 return service; 3675 }; 3676 } 3677 return NULL; 3678 } 3679 #endif 3680 3681 #ifdef ENABLE_L2CAP_ENHANCED_CREDIT_BASED_FLOW_CONTROL_MODE 3682 3683 static uint8_t l2cap_ecbm_setup_channels(btstack_linked_list_t * channels, 3684 btstack_packet_handler_t packet_handler, uint8_t num_channels, 3685 hci_connection_t * connection, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3686 uint8_t i; 3687 uint8_t status = ERROR_CODE_SUCCESS; 3688 for (i=0;i<num_channels;i++){ 3689 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, L2CAP_CHANNEL_TYPE_CHANNEL_ECBM, connection->address, connection->address_type, psm, mtu, security_level); 3690 if (!channel) { 3691 status = BTSTACK_MEMORY_ALLOC_FAILED; 3692 break; 3693 } 3694 channel->con_handle = connection->con_handle; 3695 btstack_linked_list_add_tail(channels, (btstack_linked_item_t *) channel); 3696 } 3697 3698 // free channels if not all allocated 3699 if (status != ERROR_CODE_SUCCESS){ 3700 while (true) { 3701 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_pop(channels); 3702 if (channel == NULL) break; 3703 l2cap_free_channel_entry(channel); 3704 } 3705 } 3706 3707 return status; 3708 } 3709 3710 static inline l2cap_service_t * l2cap_ecbm_get_service(uint16_t spsm){ 3711 return l2cap_get_service_internal(&l2cap_enhanced_services, spsm); 3712 } 3713 3714 static inline uint8_t l2cap_ecbm_status_for_result(uint16_t result) { 3715 switch (result) { 3716 case L2CAP_ECBM_CONNECTION_RESULT_ALL_SUCCESS: 3717 return ERROR_CODE_SUCCESS; 3718 case L2CAP_ECBM_CONNECTION_RESULT_ALL_REFUSED_SPSM_NOT_SUPPORTED: 3719 return L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_PSM; 3720 case L2CAP_ECBM_CONNECTION_RESULT_SOME_REFUSED_INSUFFICIENT_RESOURCES_AVAILABLE: 3721 return L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_RESOURCES; 3722 case L2CAP_ECBM_CONNECTION_RESULT_ALL_REFUSED_INSUFFICIENT_AUTHENTICATION: 3723 case L2CAP_ECBM_CONNECTION_RESULT_ALL_REFUSED_INSUFFICIENT_AUTHORIZATION: 3724 case L2CAP_ECBM_CONNECTION_RESULT_ALL_REFUSED_ENCYRPTION_KEY_SIZE_TOO_SHORT: 3725 case L2CAP_ECBM_CONNECTION_RESULT_ALL_REFUSED_INSUFFICIENT_ENCRYPTION: 3726 return L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY; 3727 default: 3728 // invalid Source CID, Source CID already allocated, unacceptable parameters 3729 return L2CAP_CONNECTION_RESPONSE_UNKNOWN_ERROR; 3730 } 3731 } 3732 3733 static int l2cap_ecbm_signaling_handler_dispatch(hci_con_handle_t handle, uint16_t signaling_cid, uint8_t *command, 3734 uint8_t sig_id) { 3735 3736 hci_connection_t *connection; 3737 btstack_linked_list_iterator_t it; 3738 l2cap_service_t *service; 3739 uint16_t spsm; 3740 uint8_t num_channels; 3741 uint16_t num_channels_and_signaling_cid; 3742 uint8_t i; 3743 uint16_t new_mtu; 3744 uint16_t new_mps; 3745 uint16_t initial_credits; 3746 uint16_t result; 3747 uint8_t status; 3748 3749 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3750 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3751 log_info("enhanced dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 3752 3753 switch (code) { 3754 case L2CAP_CREDIT_BASED_CONNECTION_REQUEST: 3755 // check size 3756 if (len < 10u) return 0u; 3757 3758 // get hci connection, bail if not found (must not happen) 3759 connection = hci_connection_for_handle(handle); 3760 btstack_assert(connection != NULL); 3761 3762 // get num channels to establish 3763 num_channels = (len - 8) / sizeof(uint16_t); 3764 3765 // combine signaling cid and number channels for l2cap_register_signaling_response 3766 num_channels_and_signaling_cid = (num_channels << 8) | signaling_cid; 3767 3768 // check if service registered 3769 spsm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3770 service = l2cap_ecbm_get_service(spsm); 3771 3772 if (service) { 3773 3774 uint16_t remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3775 uint16_t remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3776 uint16_t credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3777 3778 // param: check remote mtu 3779 if (service->mtu > remote_mtu) { 3780 l2cap_register_signaling_response(handle, L2CAP_CREDIT_BASED_CONNECTION_REQUEST, sig_id, 3781 num_channels_and_signaling_cid, L2CAP_ECBM_CONNECTION_RESULT_ALL_REFUSED_INVALID_PARAMETERS); 3782 return 1; 3783 } 3784 3785 // security: check authentication 3786 if (service->required_security_level >= LEVEL_3) { 3787 if (!gap_authenticated(handle)) { 3788 l2cap_register_signaling_response(handle, L2CAP_CREDIT_BASED_CONNECTION_REQUEST, sig_id, 3789 num_channels_and_signaling_cid, L2CAP_ECBM_CONNECTION_RESULT_ALL_REFUSED_INSUFFICIENT_AUTHENTICATION); 3790 return 1; 3791 } 3792 } 3793 3794 // security: check encryption 3795 // L2CAP.TS.p31 does not check for Connection refused - insufficient encryption which might be send for no encryption 3796 if (service->required_security_level >= LEVEL_2) { 3797 if (gap_encryption_key_size(handle) < 16) { 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_ENCYRPTION_KEY_SIZE_TOO_SHORT); 3800 return 1; 3801 } 3802 } 3803 3804 // report the last result code != 0 3805 result = 0; 3806 // store one of the local cids for the event 3807 uint16_t a_local_cid = 0; 3808 for (i = 0; i < num_channels; i++) { 3809 3810 // check source cids 3811 uint16_t source_cid = little_endian_read_16(command, 12 + (i * 2)); 3812 if (source_cid < 0x40u) { 3813 result = L2CAP_ECBM_CONNECTION_RESULT_SOME_REFUSED_INVALID_SOURCE_CID; 3814 continue; 3815 } 3816 3817 // go through list of channels for this ACL connection and check if we get a match 3818 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3819 bool source_cid_allocated = false; 3820 while (btstack_linked_list_iterator_has_next(&it)) { 3821 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3822 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3823 if (channel->con_handle != handle) continue; 3824 if (channel->remote_cid != source_cid) continue; 3825 source_cid_allocated = true; 3826 break; 3827 } 3828 if (source_cid_allocated) { 3829 result = 0x00a; 3830 continue; 3831 } 3832 3833 // setup channel 3834 l2cap_channel_t *channel = l2cap_create_channel_entry(service->packet_handler, 3835 L2CAP_CHANNEL_TYPE_CHANNEL_ECBM, 3836 connection->address, 3837 connection->address_type, spsm, 3838 service->mtu, 3839 service->required_security_level); 3840 3841 if (channel == NULL) { 3842 // Some connections refused – insufficient resources available 3843 result = 0x004; 3844 continue; 3845 } 3846 3847 // setup state 3848 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3849 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3850 channel->con_handle = connection->con_handle; 3851 channel->remote_sig_id = sig_id; 3852 channel->remote_cid = source_cid; 3853 channel->remote_mtu = remote_mtu; 3854 channel->remote_mps = remote_mps; 3855 channel->credits_outgoing = credits_outgoing; 3856 channel->cid_index = i; 3857 channel->num_cids = num_channels; 3858 3859 // if more than one error, we can report any of them 3860 channel->reason = result; 3861 3862 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 3863 3864 a_local_cid = channel->local_cid; 3865 } 3866 3867 // if no channels have been created, all have been refused, and we can respond right away 3868 if (a_local_cid == 0) { 3869 l2cap_register_signaling_response(handle, L2CAP_CREDIT_BASED_CONNECTION_REQUEST, sig_id, 3870 num_channels_and_signaling_cid, result); 3871 return 1; 3872 } 3873 3874 // emit incoming data connection event 3875 uint8_t event[16]; 3876 event[0] = L2CAP_EVENT_ECBM_INCOMING_CONNECTION; 3877 event[1] = sizeof(event) - 2; 3878 event[2] = connection->address_type; 3879 reverse_bd_addr(connection->address, &event[3]); 3880 little_endian_store_16(event, 9, connection->con_handle); 3881 little_endian_store_16(event, 11, spsm); 3882 event[13] = num_channels; 3883 little_endian_store_16(event, 14, a_local_cid); 3884 hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event)); 3885 (*service->packet_handler)(HCI_EVENT_PACKET, a_local_cid, event, sizeof(event)); 3886 3887 } else { 3888 l2cap_register_signaling_response(handle, L2CAP_CREDIT_BASED_CONNECTION_REQUEST, sig_id, 3889 num_channels_and_signaling_cid, L2CAP_ECBM_CONNECTION_RESULT_ALL_REFUSED_SPSM_NOT_SUPPORTED); 3890 } 3891 return 1; 3892 3893 case L2CAP_CREDIT_BASED_CONNECTION_RESPONSE: 3894 // check size 3895 if (len < 10u) return 0u; 3896 3897 // get hci connection, ignore if not found 3898 connection = hci_connection_for_handle(handle); 3899 if (connection == NULL) return 0; 3900 3901 // get channel config: mtu, mps, initial credits, result 3902 new_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3903 new_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3904 initial_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3905 result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3906 status = l2cap_ecbm_status_for_result(result); 3907 3908 // get num channels to modify 3909 num_channels = (len - 8) / sizeof(uint16_t); 3910 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3911 while (btstack_linked_list_iterator_has_next(&it)) { 3912 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3913 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3914 if (channel->con_handle != handle) continue; 3915 if (channel->local_sig_id != sig_id) continue; 3916 if (channel->state != L2CAP_STATE_WAIT_ENHANCED_CONNECTION_RESPONSE) continue; 3917 if (channel->cid_index < num_channels) { 3918 uint16_t remote_cid = little_endian_read_16(command, 12 + channel->cid_index * sizeof(uint16_t)); 3919 if (remote_cid != 0) { 3920 channel->state = L2CAP_STATE_OPEN; 3921 channel->remote_cid = remote_cid; 3922 channel->remote_mtu = new_mtu; 3923 channel->remote_mps = new_mps; 3924 channel->credits_outgoing = initial_credits; 3925 l2cap_ecbm_emit_channel_opened(channel, ERROR_CODE_SUCCESS); 3926 continue; 3927 } 3928 } 3929 // open failed 3930 l2cap_ecbm_emit_channel_opened(channel, status); 3931 // drop failed channel 3932 btstack_linked_list_iterator_remove(&it); 3933 btstack_memory_l2cap_channel_free(channel); 3934 } 3935 return 1; 3936 3937 case L2CAP_CREDIT_BASED_RECONFIGURE_REQUEST: 3938 // check minimal size 3939 if (len < 6) return 0u; 3940 3941 // get hci connection, bail if not found (must not happen) 3942 connection = hci_connection_for_handle(handle); 3943 btstack_assert(connection != NULL); 3944 3945 // get num channels to modify 3946 num_channels = (len - 4) / sizeof(uint16_t); 3947 3948 // get new mtu and mps 3949 new_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3950 new_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3951 3952 // validate request 3953 result = 0; 3954 for (i = 0; i < num_channels; i++) { 3955 // check cid 3956 uint16_t remote_cid = little_endian_read_16(command, 8 + (i * sizeof(uint16_t))); 3957 l2cap_channel_t *channel = l2cap_get_channel_for_remote_handle_and_cid(handle, remote_cid); 3958 if (channel == NULL) { 3959 result = L2CAP_ECBM_RECONFIGURE_FAILED_DESTINATION_CID_INVALID; 3960 break; 3961 } 3962 // check MTU is not reduced 3963 if (channel->remote_mtu > new_mtu) { 3964 result = L2CAP_ECBM_RECONFIGURE_FAILED_MTU_REDUCTION_NOT_ALLOWED; 3965 break; 3966 } 3967 // check MPS reduction 3968 if ((num_channels > 1) && (channel->remote_mps > new_mps)) { 3969 result = L2CAP_ECBM_RECONFIGURE_FAILED_MPS_REDUCTION_MULTIPLE_CHANNELS; 3970 break; 3971 } 3972 // check MPS valid 3973 if (new_mps < l2cap_enhanced_mps_min) { 3974 result = L2CAP_ECBM_RECONFIGURE_FAILED_UNACCEPTABLE_PARAMETERS; 3975 break; 3976 } 3977 } 3978 3979 // send reject 3980 if (result != 0) { 3981 l2cap_register_signaling_response(handle, L2CAP_CREDIT_BASED_RECONFIGURE_REQUEST, sig_id, signaling_cid, 3982 result); 3983 return 1; 3984 } 3985 3986 // update channels 3987 for (i = 0; i < num_channels; i++) { 3988 uint16_t remote_cid = little_endian_read_16(command, 8 + (i * sizeof(uint16_t))); 3989 l2cap_channel_t *channel = l2cap_get_channel_for_remote_handle_and_cid(handle, remote_cid); 3990 channel->remote_mps = new_mps; 3991 channel->remote_mtu = new_mtu; 3992 // emit event 3993 uint8_t event[8]; 3994 event[0] = L2CAP_EVENT_ECBM_RECONFIGURED; 3995 event[1] = sizeof(event) - 2; 3996 little_endian_store_16(event, 2, channel->local_cid); 3997 little_endian_store_16(event, 4, new_mtu); 3998 little_endian_store_16(event, 6, new_mps); 3999 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4000 } 4001 4002 // send accept 4003 l2cap_register_signaling_response(handle, L2CAP_CREDIT_BASED_RECONFIGURE_REQUEST, sig_id, signaling_cid, 0); 4004 return 1; 4005 4006 case L2CAP_CREDIT_BASED_RECONFIGURE_RESPONSE: 4007 // check size 4008 if (len < 2u) return 0u; 4009 4010 result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 4011 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4012 while (btstack_linked_list_iterator_has_next(&it)) { 4013 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4014 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 4015 if (channel->con_handle != handle) continue; 4016 if (channel->local_sig_id != sig_id) continue; 4017 if (channel->state != L2CAP_STATE_WAIT_ENHANCED_RENEGOTIATION_RESPONSE) continue; 4018 // complete request 4019 if (result == 0) { 4020 channel->receive_sdu_buffer = channel->renegotiate_sdu_buffer; 4021 channel->local_mtu = channel->renegotiate_mtu; 4022 } 4023 channel->state = L2CAP_STATE_OPEN; 4024 // emit event 4025 l2cap_ecbm_emit_reconfigure_complete(channel, result); 4026 } 4027 break; 4028 4029 default: 4030 btstack_unreachable(); 4031 break; 4032 } 4033 return 0; 4034 } 4035 4036 #endif 4037 4038 #ifdef ENABLE_BLE 4039 4040 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 4041 uint8_t event[6]; 4042 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 4043 event[1] = 4; 4044 little_endian_store_16(event, 2, con_handle); 4045 little_endian_store_16(event, 4, result); 4046 l2cap_emit_event(event, sizeof(event)); 4047 } 4048 4049 // @return valid 4050 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 4051 hci_connection_t * connection; 4052 uint16_t result; 4053 uint8_t event[12]; 4054 4055 #ifdef L2CAP_USES_CREDIT_BASED_CHANNELS 4056 l2cap_channel_t * channel; 4057 btstack_linked_list_iterator_t it; 4058 #endif 4059 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 4060 uint16_t local_cid; 4061 uint16_t le_psm; 4062 l2cap_service_t * service; 4063 uint16_t source_cid; 4064 #endif 4065 4066 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 4067 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 4068 log_info("le dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 4069 4070 switch (code){ 4071 4072 case CONNECTION_PARAMETER_UPDATE_REQUEST: 4073 // check size 4074 if (len < 8u) return 0u; 4075 connection = hci_connection_for_handle(handle); 4076 if (connection != NULL){ 4077 if (connection->role != HCI_ROLE_MASTER){ 4078 // reject command without notifying upper layer when not in master role 4079 return 0; 4080 } 4081 le_connection_parameter_range_t existing_range; 4082 gap_get_connection_parameter_range(&existing_range); 4083 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 4084 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 4085 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 4086 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 4087 4088 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 4089 if (update_parameter){ 4090 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 4091 connection->le_conn_interval_min = le_conn_interval_min; 4092 connection->le_conn_interval_max = le_conn_interval_max; 4093 connection->le_conn_latency = le_conn_latency; 4094 connection->le_supervision_timeout = le_supervision_timeout; 4095 } else { 4096 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 4097 } 4098 connection->le_con_param_update_identifier = sig_id; 4099 } 4100 4101 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 4102 event[1] = 8; 4103 little_endian_store_16(event, 2, handle); 4104 (void)memcpy(&event[4], &command[4], 8); 4105 l2cap_emit_event(event, sizeof(event)); 4106 break; 4107 4108 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 4109 // check size 4110 if (len < 2u) return 0u; 4111 result = little_endian_read_16(command, 4); 4112 l2cap_emit_connection_parameter_update_response(handle, result); 4113 break; 4114 4115 #ifdef ENABLE_L2CAP_ENHANCED_CREDIT_BASED_FLOW_CONTROL_MODE 4116 case L2CAP_CREDIT_BASED_CONNECTION_REQUEST: 4117 case L2CAP_CREDIT_BASED_CONNECTION_RESPONSE: 4118 case L2CAP_CREDIT_BASED_RECONFIGURE_REQUEST: 4119 case L2CAP_CREDIT_BASED_RECONFIGURE_RESPONSE: 4120 return l2cap_ecbm_signaling_handler_dispatch(handle, L2CAP_CID_SIGNALING_LE, command, sig_id); 4121 #endif 4122 4123 #ifdef L2CAP_USES_CREDIT_BASED_CHANNELS 4124 case COMMAND_REJECT: 4125 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4126 while (btstack_linked_list_iterator_has_next(&it)){ 4127 channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4128 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 4129 if (channel->con_handle != handle) continue; 4130 if (channel->local_sig_id != sig_id) continue; 4131 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 4132 // if received while waiting for le connection response, assume legacy device 4133 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 4134 channel->state = L2CAP_STATE_CLOSED; 4135 // no official value for this, use: Connection refused – LE_PSM not supported 4136 l2cap_cbm_emit_channel_opened(channel, L2CAP_CBM_CONNECTION_RESULT_SPSM_NOT_SUPPORTED); 4137 4138 // discard channel 4139 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4140 l2cap_free_channel_entry(channel); 4141 continue; 4142 } 4143 #endif 4144 #ifdef ENABLE_L2CAP_ENHANCED_CREDIT_BASED_FLOW_CONTROL_MODE 4145 // if received while waiting for le connection response, assume legacy device 4146 if (channel->state == L2CAP_STATE_WAIT_ENHANCED_CONNECTION_RESPONSE){ 4147 channel->state = L2CAP_STATE_CLOSED; 4148 // treat as SPSM not supported 4149 l2cap_ecbm_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_PSM); 4150 4151 // discard channel 4152 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4153 l2cap_free_channel_entry(channel); 4154 continue; 4155 } 4156 #endif 4157 } 4158 break; 4159 #endif 4160 4161 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 4162 case LE_CREDIT_BASED_CONNECTION_REQUEST: 4163 // check size 4164 if (len < 10u) return 0u; 4165 4166 // get hci connection, bail if not found (must not happen) 4167 connection = hci_connection_for_handle(handle); 4168 if (!connection) return 0; 4169 4170 // check if service registered 4171 le_psm = little_endian_read_16(command, 4); 4172 service = l2cap_cbm_get_service(le_psm); 4173 source_cid = little_endian_read_16(command, 6); 4174 4175 if (service){ 4176 if (source_cid < 0x40u){ 4177 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CBM_CONNECTION_RESULT_INVALID_SOURCE_CID); 4178 return 1; 4179 } 4180 4181 // go through list of channels for this ACL connection and check if we get a match 4182 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4183 while (btstack_linked_list_iterator_has_next(&it)){ 4184 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4185 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 4186 if (a_channel->con_handle != handle) continue; 4187 if (a_channel->remote_cid != source_cid) continue; 4188 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CBM_CONNECTION_RESULT_SOURCE_CID_ALREADY_ALLOCATED); 4189 return 1; 4190 } 4191 4192 // security: check encryption 4193 if (service->required_security_level >= LEVEL_2){ 4194 if (gap_encryption_key_size(handle) == 0){ 4195 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CBM_CONNECTION_RESULT_INSUFFICIENT_ENCRYPTION); 4196 return 1; 4197 } 4198 // anything less than 16 byte key size is insufficient 4199 if (gap_encryption_key_size(handle) < 16){ 4200 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CBM_CONNECTION_RESULT_ENCYRPTION_KEY_SIZE_TOO_SHORT); 4201 return 1; 4202 } 4203 } 4204 4205 // security: check authentication 4206 if (service->required_security_level >= LEVEL_3){ 4207 if (!gap_authenticated(handle)){ 4208 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CBM_CONNECTION_RESULT_INSUFFICIENT_AUTHENTICATION); 4209 return 1; 4210 } 4211 } 4212 4213 // security: check authorization 4214 if (service->required_security_level >= LEVEL_4){ 4215 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 4216 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CBM_CONNECTION_RESULT_INSUFFICIENT_AUTHORIZATION); 4217 return 1; 4218 } 4219 } 4220 4221 // allocate channel 4222 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CHANNEL_CBM, connection->address, 4223 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 4224 if (!channel){ 4225 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CBM_CONNECTION_RESULT_NO_RESOURCES_AVAILABLE); 4226 return 1; 4227 } 4228 4229 channel->con_handle = handle; 4230 channel->remote_cid = source_cid; 4231 channel->remote_sig_id = sig_id; 4232 channel->remote_mtu = little_endian_read_16(command, 8); 4233 channel->remote_mps = little_endian_read_16(command, 10); 4234 channel->credits_outgoing = little_endian_read_16(command, 12); 4235 4236 // set initial state 4237 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 4238 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 4239 4240 // add to connections list 4241 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 4242 4243 // post connection request event 4244 l2cap_cbm_emit_incoming_connection(channel); 4245 4246 } else { 4247 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, L2CAP_CBM_CONNECTION_RESULT_SPSM_NOT_SUPPORTED); 4248 } 4249 break; 4250 4251 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 4252 // check size 4253 if (len < 10u) return 0u; 4254 4255 // Find channel for this sig_id and connection handle 4256 channel = NULL; 4257 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4258 while (btstack_linked_list_iterator_has_next(&it)){ 4259 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4260 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 4261 if (a_channel->con_handle != handle) continue; 4262 if (a_channel->local_sig_id != sig_id) continue; 4263 channel = a_channel; 4264 break; 4265 } 4266 if (!channel) break; 4267 4268 // cid + 0 4269 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 4270 if (result){ 4271 channel->state = L2CAP_STATE_CLOSED; 4272 uint8_t status = l2cap_cbm_status_for_result(result); 4273 l2cap_cbm_emit_channel_opened(channel, status); 4274 4275 // discard channel 4276 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4277 l2cap_free_channel_entry(channel); 4278 break; 4279 } 4280 4281 // success 4282 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 4283 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 4284 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 4285 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 4286 channel->state = L2CAP_STATE_OPEN; 4287 l2cap_cbm_emit_channel_opened(channel, ERROR_CODE_SUCCESS); 4288 break; 4289 #endif 4290 4291 #ifdef L2CAP_USES_CREDIT_BASED_CHANNELS 4292 case L2CAP_FLOW_CONTROL_CREDIT_INDICATION: 4293 return l2cap_credit_based_handle_credit_indication(handle, command, len) ? 1 : 0; 4294 4295 case DISCONNECTION_REQUEST: 4296 4297 // check size 4298 if (len < 4u) return 0u; 4299 4300 // find channel 4301 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 4302 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 4303 if (!channel) { 4304 log_error("credit: no channel for cid 0x%02x", local_cid); 4305 break; 4306 } 4307 channel->remote_sig_id = sig_id; 4308 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 4309 break; 4310 4311 case DISCONNECTION_RESPONSE: 4312 // check size 4313 if (len < 4u) return 0u; 4314 4315 // find channel 4316 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 4317 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 4318 if (!channel) break; 4319 if (channel->local_sig_id == sig_id) { 4320 if (channel->state == L2CAP_STATE_WAIT_DISCONNECT){ 4321 l2cap_finalize_channel_close(channel); 4322 } 4323 } 4324 break; 4325 #endif 4326 4327 default: 4328 // command unknown -> reject command 4329 return 0; 4330 } 4331 return 1; 4332 } 4333 #endif 4334 4335 #ifdef ENABLE_CLASSIC 4336 static void l2cap_acl_classic_handler_for_channel(l2cap_channel_t * l2cap_channel, uint8_t * packet, uint16_t size){ 4337 4338 // forward data only in OPEN state 4339 if (l2cap_channel->state != L2CAP_STATE_OPEN) return; 4340 4341 #ifdef L2CAP_USES_CREDIT_BASED_CHANNELS 4342 if (l2cap_channel->channel_type == L2CAP_CHANNEL_TYPE_CHANNEL_ECBM){ 4343 l2cap_credit_based_handle_pdu(l2cap_channel, packet, size); 4344 return; 4345 } 4346 #endif 4347 4348 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 4349 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 4350 4351 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 4352 4353 // assert control + FCS fields are inside 4354 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) return; 4355 4356 if (l2cap_channel->fcs_option){ 4357 // verify FCS (required if one side requested it) 4358 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 4359 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 4360 4361 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 4362 // simulate fcs error 4363 static int counter = 0; 4364 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 4365 log_info("Simulate fcs error"); 4366 fcs_calculated++; 4367 counter = 0; 4368 } 4369 #endif 4370 4371 if (fcs_calculated == fcs_packet){ 4372 log_info("Packet FCS 0x%04x verified", fcs_packet); 4373 } else { 4374 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 4375 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 4376 return; 4377 } 4378 } 4379 4380 // switch on packet type 4381 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 4382 uint8_t req_seq = (control >> 8) & 0x3f; 4383 int final = (control >> 7) & 0x01; 4384 if (control & 1){ 4385 // S-Frame 4386 int poll = (control >> 4) & 0x01; 4387 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 4388 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 4389 l2cap_ertm_tx_packet_state_t * tx_state; 4390 switch (s){ 4391 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 4392 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 4393 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 4394 if (poll && final){ 4395 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 4396 log_error("P=F=1 in S-Frame"); 4397 break; 4398 } 4399 if (poll){ 4400 // check if we did request selective retransmission before <==> we have stored SDU segments 4401 int i; 4402 int num_stored_out_of_order_packets = 0; 4403 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 4404 int index = l2cap_channel->rx_store_index + i; 4405 if (index >= l2cap_channel->num_rx_buffers){ 4406 index -= l2cap_channel->num_rx_buffers; 4407 } 4408 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 4409 if (!rx_state->valid) continue; 4410 num_stored_out_of_order_packets++; 4411 } 4412 if (num_stored_out_of_order_packets){ 4413 l2cap_channel->send_supervisor_frame_selective_reject = 1; 4414 } else { 4415 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 4416 } 4417 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 4418 } 4419 if (final){ 4420 // Stop-MonitorTimer 4421 l2cap_ertm_stop_monitor_timer(l2cap_channel); 4422 // If UnackedFrames > 0 then Start-RetransTimer 4423 if (l2cap_channel->unacked_frames){ 4424 l2cap_ertm_start_retransmission_timer(l2cap_channel); 4425 } 4426 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 4427 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 4428 } 4429 break; 4430 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 4431 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 4432 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 4433 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 4434 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 4435 break; 4436 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 4437 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 4438 break; 4439 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 4440 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 4441 if (poll){ 4442 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 4443 } 4444 // find requested i-frame 4445 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 4446 if (tx_state){ 4447 log_info("Retransmission for tx_seq %u requested", req_seq); 4448 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 4449 tx_state->retransmission_requested = 1; 4450 l2cap_channel->srej_active = 1; 4451 } 4452 break; 4453 default: 4454 break; 4455 } 4456 } else { 4457 // I-Frame 4458 // get control 4459 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 4460 uint8_t tx_seq = (control >> 1) & 0x3f; 4461 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 4462 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 4463 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 4464 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 4465 if (final){ 4466 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 4467 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 4468 } 4469 4470 // get SDU 4471 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 4472 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 4473 4474 // assert SDU size is smaller or equal to our buffers 4475 uint16_t max_payload_size = 0; 4476 switch (sar){ 4477 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 4478 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 4479 // SDU Length + MPS 4480 max_payload_size = l2cap_channel->local_mps + 2; 4481 break; 4482 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 4483 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 4484 max_payload_size = l2cap_channel->local_mps; 4485 break; 4486 default: 4487 btstack_assert(false); 4488 break; 4489 } 4490 if (payload_len > max_payload_size){ 4491 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 4492 return; 4493 } 4494 4495 // check ordering 4496 if (l2cap_channel->expected_tx_seq == tx_seq){ 4497 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 4498 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 4499 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 4500 4501 // process SDU 4502 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 4503 4504 // process stored segments 4505 while (true){ 4506 int index = l2cap_channel->rx_store_index; 4507 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 4508 if (!rx_state->valid) break; 4509 4510 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 4511 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 4512 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 4513 4514 rx_state->valid = 0; 4515 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 4516 4517 // update rx store index 4518 index++; 4519 if (index >= l2cap_channel->num_rx_buffers){ 4520 index = 0; 4521 } 4522 l2cap_channel->rx_store_index = index; 4523 } 4524 4525 // 4526 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 4527 4528 } else { 4529 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 4530 if (delta < 2){ 4531 // store segment 4532 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 4533 4534 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 4535 l2cap_channel->send_supervisor_frame_selective_reject = 1; 4536 } else { 4537 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 4538 l2cap_channel->send_supervisor_frame_reject = 1; 4539 } 4540 } 4541 } 4542 return; 4543 } 4544 #endif 4545 4546 // check size 4547 uint16_t payload_size = size - COMPLETE_L2CAP_HEADER; 4548 if (l2cap_channel->local_mtu < payload_size) return; 4549 4550 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], payload_size); 4551 } 4552 #endif 4553 4554 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 4555 #ifdef ENABLE_CLASSIC 4556 l2cap_channel_t * l2cap_channel; 4557 l2cap_fixed_channel_t * l2cap_fixed_channel; 4558 4559 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 4560 uint8_t broadcast_flag = READ_ACL_FLAGS(packet) >> 2; 4561 switch (channel_id) { 4562 4563 case L2CAP_CID_SIGNALING: { 4564 if (broadcast_flag != 0) break; 4565 uint32_t command_offset = 8; 4566 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 4567 // assert signaling command is fully inside packet 4568 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 4569 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 4570 if (next_command_offset > size){ 4571 log_error("l2cap signaling command len invalid -> drop"); 4572 break; 4573 } 4574 // handle signaling command 4575 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 4576 // go to next command 4577 command_offset = next_command_offset; 4578 } 4579 break; 4580 } 4581 4582 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 4583 if (broadcast_flag == 0) break; 4584 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 4585 if (!l2cap_fixed_channel) break; 4586 if (!l2cap_fixed_channel->packet_handler) break; 4587 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 4588 break; 4589 4590 #ifdef ENABLE_BLE 4591 case L2CAP_CID_BR_EDR_SECURITY_MANAGER: 4592 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 4593 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 4594 // Pairing Failed 4595 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_BR_EDR_SECURITY_MANAGER, SM_REASON_PAIRING_NOT_SUPPORTED); 4596 } else { 4597 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 4598 } 4599 break; 4600 #endif 4601 4602 default: 4603 if (broadcast_flag != 0) break; 4604 // Find channel for this channel_id and connection handle 4605 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 4606 if (l2cap_channel != NULL){ 4607 l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size); 4608 } 4609 break; 4610 } 4611 #else 4612 UNUSED(handle); // ok: no code 4613 UNUSED(packet); // ok: no code 4614 UNUSED(size); // ok: no code 4615 #endif 4616 } 4617 4618 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 4619 #ifdef ENABLE_BLE 4620 4621 l2cap_fixed_channel_t * l2cap_fixed_channel; 4622 4623 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 4624 l2cap_channel_t * l2cap_channel; 4625 #endif 4626 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 4627 switch (channel_id) { 4628 4629 case L2CAP_CID_SIGNALING_LE: { 4630 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 4631 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 4632 if ((COMPLETE_L2CAP_HEADER + 4u + len) > size) break; 4633 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 4634 if (!valid){ 4635 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 4636 } 4637 break; 4638 } 4639 4640 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 4641 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 4642 if (!l2cap_fixed_channel) break; 4643 if (!l2cap_fixed_channel->packet_handler) break; 4644 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 4645 break; 4646 4647 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 4648 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 4649 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 4650 // Pairing Failed 4651 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, SM_REASON_PAIRING_NOT_SUPPORTED); 4652 } else { 4653 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 4654 } 4655 break; 4656 4657 default: 4658 4659 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 4660 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 4661 if (l2cap_channel != NULL) { 4662 l2cap_credit_based_handle_pdu(l2cap_channel, packet, size); 4663 } 4664 #endif 4665 break; 4666 } 4667 #else 4668 UNUSED(handle); // ok: no code 4669 UNUSED(packet); // ok: no code 4670 UNUSED(size); // ok: no code 4671 #endif 4672 } 4673 4674 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 4675 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 4676 UNUSED(channel); // ok: there is no channel 4677 4678 // Assert full L2CAP header present 4679 if (size < COMPLETE_L2CAP_HEADER) return; 4680 4681 // Dispatch to Classic or LE handler (SCO packets are not dispatched to L2CAP) 4682 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 4683 hci_connection_t *conn = hci_connection_for_handle(handle); 4684 if (!conn) return; 4685 if (conn->address_type == BD_ADDR_TYPE_ACL){ 4686 l2cap_acl_classic_handler(handle, packet, size); 4687 } else { 4688 l2cap_acl_le_handler(handle, packet, size); 4689 } 4690 4691 l2cap_run(); 4692 } 4693 4694 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 4695 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 4696 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 4697 if (!channel) return; 4698 channel->packet_handler = packet_handler; 4699 } 4700 4701 #ifdef L2CAP_USES_CHANNELS 4702 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 4703 void l2cap_finalize_channel_close(l2cap_channel_t * channel){ 4704 channel->state = L2CAP_STATE_CLOSED; 4705 l2cap_handle_channel_closed(channel); 4706 // discard channel 4707 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4708 l2cap_free_channel_entry(channel); 4709 } 4710 #endif 4711 4712 #ifdef ENABLE_CLASSIC 4713 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 4714 return l2cap_get_service_internal(&l2cap_services, psm); 4715 } 4716 4717 static void l2cap_update_minimal_security_level(void){ 4718 // update minimal service security level 4719 gap_security_level_t minimal_level = LEVEL_1; 4720 btstack_linked_list_iterator_t it; 4721 btstack_linked_list_iterator_init(&it, &l2cap_services); 4722 while (btstack_linked_list_iterator_has_next(&it)){ 4723 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 4724 if (service->required_security_level > minimal_level){ 4725 minimal_level = service->required_security_level; 4726 }; 4727 } 4728 gap_set_minimal_service_security_level(minimal_level); 4729 } 4730 4731 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 4732 4733 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 4734 4735 // check for alread registered psm 4736 l2cap_service_t *service = l2cap_get_service(psm); 4737 if (service) { 4738 log_error("register: PSM %u already registered", psm); 4739 return L2CAP_SERVICE_ALREADY_REGISTERED; 4740 } 4741 4742 // alloc structure 4743 service = btstack_memory_l2cap_service_get(); 4744 if (!service) { 4745 log_error("register: no memory for l2cap_service_t"); 4746 return BTSTACK_MEMORY_ALLOC_FAILED; 4747 } 4748 4749 // fill in 4750 service->psm = psm; 4751 service->mtu = mtu; 4752 service->packet_handler = service_packet_handler; 4753 service->required_security_level = security_level; 4754 4755 // add to services list 4756 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 4757 4758 l2cap_update_minimal_security_level(); 4759 4760 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 4761 // enable page scan 4762 gap_connectable_control(1); 4763 #endif 4764 4765 4766 return ERROR_CODE_SUCCESS; 4767 } 4768 4769 uint8_t l2cap_unregister_service(uint16_t psm){ 4770 4771 log_info("unregister psm 0x%x", psm); 4772 4773 l2cap_service_t *service = l2cap_get_service(psm); 4774 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 4775 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 4776 btstack_memory_l2cap_service_free(service); 4777 4778 l2cap_update_minimal_security_level(); 4779 4780 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 4781 // disable page scan when no services registered 4782 if (btstack_linked_list_empty(&l2cap_services)) { 4783 gap_connectable_control(0); 4784 } 4785 #endif 4786 4787 return ERROR_CODE_SUCCESS; 4788 } 4789 #endif 4790 4791 4792 #ifdef L2CAP_USES_CREDIT_BASED_CHANNELS 4793 4794 static void l2cap_credit_based_send_pdu(l2cap_channel_t *channel) { 4795 btstack_assert(channel != NULL); 4796 btstack_assert(channel->send_sdu_buffer != NULL); 4797 btstack_assert(channel->credits_outgoing > 0); 4798 4799 // send part of SDU 4800 hci_reserve_packet_buffer(); 4801 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 4802 uint8_t *l2cap_payload = acl_buffer + 8; 4803 uint16_t pos = 0; 4804 if (!channel->send_sdu_pos) { 4805 // store SDU len 4806 channel->send_sdu_pos += 2u; 4807 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 4808 pos += 2u; 4809 } 4810 uint16_t payload_size = btstack_min(channel->send_sdu_len + 2u - channel->send_sdu_pos, channel->remote_mps - pos); 4811 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, 4812 channel->credits_outgoing); 4813 (void) memcpy(&l2cap_payload[pos], 4814 &channel->send_sdu_buffer[channel->send_sdu_pos - 2u], 4815 payload_size); // -2 for virtual SDU len 4816 pos += payload_size; 4817 channel->send_sdu_pos += payload_size; 4818 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 4819 4820 channel->credits_outgoing--; 4821 4822 // update state (mark SDU as done) before calling hci_send_acl_packet_buffer (trigger l2cap_le_send_pdu again) 4823 bool done = channel->send_sdu_pos >= (channel->send_sdu_len + 2u); 4824 if (done) { 4825 channel->send_sdu_buffer = NULL; 4826 } 4827 4828 hci_send_acl_packet_buffer(8u + pos); 4829 4830 if (done) { 4831 // send done event 4832 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_PACKET_SENT); 4833 // inform about can send now 4834 l2cap_credit_based_notify_channel_can_send(channel); 4835 } 4836 } 4837 4838 static uint8_t l2cap_credit_based_send_data(l2cap_channel_t * channel, const uint8_t * data, uint16_t size){ 4839 4840 if (size > channel->remote_mtu){ 4841 log_error("l2cap send, cid 0x%02x, data length exceeds remote MTU.", channel->local_cid); 4842 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 4843 } 4844 4845 if (channel->send_sdu_buffer){ 4846 log_info("l2cap send, cid 0x%02x, cannot send", channel->local_cid); 4847 return BTSTACK_ACL_BUFFERS_FULL; 4848 } 4849 4850 channel->send_sdu_buffer = data; 4851 channel->send_sdu_len = size; 4852 channel->send_sdu_pos = 0; 4853 4854 l2cap_notify_channel_can_send(); 4855 return ERROR_CODE_SUCCESS; 4856 } 4857 4858 static uint8_t l2cap_credit_based_provide_credits(uint16_t local_cid, uint16_t credits){ 4859 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4860 if (!channel) { 4861 log_error("le credits no channel for cid 0x%02x", local_cid); 4862 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4863 } 4864 4865 // check state 4866 if (channel->state != L2CAP_STATE_OPEN){ 4867 log_error("le credits but channel 0x%02x not open yet", local_cid); 4868 } 4869 4870 // ignore if set to automatic credits 4871 if (channel->automatic_credits) return ERROR_CODE_SUCCESS; 4872 4873 // assert incoming credits + credits <= 0xffff 4874 uint32_t total_credits = channel->credits_incoming; 4875 total_credits += channel->new_credits_incoming; 4876 total_credits += credits; 4877 if (total_credits > 0xffffu){ 4878 log_error("le credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 4879 channel->new_credits_incoming, credits); 4880 } 4881 4882 // set credits_granted 4883 channel->new_credits_incoming += credits; 4884 4885 // go 4886 l2cap_run(); 4887 return ERROR_CODE_SUCCESS; 4888 } 4889 4890 static void l2cap_credit_based_send_credits(l2cap_channel_t *channel) { 4891 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 4892 channel->local_sig_id = l2cap_next_sig_id(); 4893 uint16_t new_credits = channel->new_credits_incoming; 4894 channel->new_credits_incoming = 0; 4895 channel->credits_incoming += new_credits; 4896 uint16_t signaling_cid = channel->address_type == BD_ADDR_TYPE_ACL ? L2CAP_CID_SIGNALING : L2CAP_CID_SIGNALING_LE; 4897 l2cap_send_general_signaling_packet(channel->con_handle, signaling_cid, L2CAP_FLOW_CONTROL_CREDIT_INDICATION, channel->local_sig_id, channel->local_cid, new_credits); 4898 } 4899 4900 // @return valid 4901 static bool l2cap_credit_based_handle_credit_indication(hci_con_handle_t handle, const uint8_t * command, uint16_t len){ 4902 // check size 4903 if (len < 4u) return false; 4904 4905 // find channel 4906 uint16_t remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 4907 l2cap_channel_t * channel = l2cap_get_channel_for_remote_handle_and_cid(handle, remote_cid); 4908 if (!channel) { 4909 log_error("credit: no channel for handle 0x%04x/cid 0x%02x", handle, remote_cid); 4910 return true; 4911 } 4912 uint16_t new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 4913 uint16_t credits_before = channel->credits_outgoing; 4914 channel->credits_outgoing += new_credits; 4915 // check for credit overrun 4916 if (credits_before > channel->credits_outgoing) { 4917 log_error("credit: new credits caused overrrun for cid 0x%02x, disconnecting", channel->local_cid); 4918 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 4919 return true; 4920 } 4921 log_info("credit: %u credits for 0x%02x, now %u", new_credits, channel->local_cid, channel->credits_outgoing); 4922 l2cap_call_notify_channel_in_run = true; 4923 return true; 4924 } 4925 4926 static void l2cap_credit_based_handle_pdu(l2cap_channel_t * l2cap_channel, const uint8_t * packet, uint16_t size){ 4927 // ignore empty packets 4928 if (size == COMPLETE_L2CAP_HEADER) return; 4929 4930 // credit counting 4931 if (l2cap_channel->credits_incoming == 0u){ 4932 log_info("(e)CBM: packet received but no incoming credits"); 4933 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 4934 return; 4935 } 4936 l2cap_channel->credits_incoming--; 4937 4938 // automatic credits 4939 if ((l2cap_channel->credits_incoming < L2CAP_CREDIT_BASED_FLOW_CONTROL_MODE_AUTOMATIC_CREDITS_WATERMARK) && l2cap_channel->automatic_credits){ 4940 l2cap_channel->new_credits_incoming = L2CAP_CREDIT_BASED_FLOW_CONTROL_MODE_AUTOMATIC_CREDITS_INCREMENT; 4941 } 4942 4943 // first fragment 4944 uint16_t pos = 0; 4945 if (!l2cap_channel->receive_sdu_len){ 4946 if (size < (COMPLETE_L2CAP_HEADER + 2)) return; 4947 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 4948 if(sdu_len > l2cap_channel->local_mtu) return; // SDU would be larger than our buffer 4949 l2cap_channel->receive_sdu_len = sdu_len; 4950 l2cap_channel->receive_sdu_pos = 0; 4951 pos += 2u; 4952 size -= 2u; 4953 } 4954 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 4955 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 4956 if (fragment_size > remaining_space) return; // SDU would cause buffer overrun 4957 (void)memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], 4958 &packet[COMPLETE_L2CAP_HEADER + pos], 4959 fragment_size); 4960 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 4961 // done? 4962 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 4963 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 4964 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 4965 l2cap_channel->receive_sdu_len = 0; 4966 } 4967 } 4968 4969 static void l2cap_credit_based_notify_channel_can_send(l2cap_channel_t *channel){ 4970 if (!channel->waiting_for_can_send_now) return; 4971 if (channel->send_sdu_buffer) return; 4972 channel->waiting_for_can_send_now = 0; 4973 log_debug("le can send now, local_cid 0x%x", channel->local_cid); 4974 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CAN_SEND_NOW); 4975 } 4976 #endif 4977 4978 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 4979 // 1BH2222 4980 static void l2cap_cbm_emit_incoming_connection(l2cap_channel_t *channel) { 4981 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", 4982 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 4983 uint8_t event[19]; 4984 event[0] = L2CAP_EVENT_CBM_INCOMING_CONNECTION; 4985 event[1] = sizeof(event) - 2u; 4986 event[2] = channel->address_type; 4987 reverse_bd_addr(channel->address, &event[3]); 4988 little_endian_store_16(event, 9, channel->con_handle); 4989 little_endian_store_16(event, 11, channel->psm); 4990 little_endian_store_16(event, 13, channel->local_cid); 4991 little_endian_store_16(event, 15, channel->remote_cid); 4992 little_endian_store_16(event, 17, channel->remote_mtu); 4993 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4994 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4995 } 4996 // 11BH22222 4997 static void l2cap_cbm_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 4998 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", 4999 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 5000 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 5001 uint8_t event[23]; 5002 event[0] = L2CAP_EVENT_CBM_CHANNEL_OPENED; 5003 event[1] = sizeof(event) - 2u; 5004 event[2] = status; 5005 event[3] = channel->address_type; 5006 reverse_bd_addr(channel->address, &event[4]); 5007 little_endian_store_16(event, 10, channel->con_handle); 5008 event[12] = (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) ? 1 : 0; 5009 little_endian_store_16(event, 13, channel->psm); 5010 little_endian_store_16(event, 15, channel->local_cid); 5011 little_endian_store_16(event, 17, channel->remote_cid); 5012 little_endian_store_16(event, 19, channel->local_mtu); 5013 little_endian_store_16(event, 21, channel->remote_mtu); 5014 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 5015 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 5016 } 5017 5018 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 5019 void l2cap_cbm_finialize_channel_close(l2cap_channel_t * channel){ 5020 channel->state = L2CAP_STATE_CLOSED; 5021 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 5022 // discard channel 5023 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 5024 l2cap_free_channel_entry(channel); 5025 } 5026 5027 static inline l2cap_service_t * l2cap_cbm_get_service(uint16_t le_psm){ 5028 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 5029 } 5030 5031 uint8_t l2cap_cbm_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 5032 5033 log_info("l2cap_cbm_register_service psm 0x%x", psm); 5034 5035 // check for alread registered psm 5036 l2cap_service_t *service = l2cap_cbm_get_service(psm); 5037 if (service) { 5038 return L2CAP_SERVICE_ALREADY_REGISTERED; 5039 } 5040 5041 // alloc structure 5042 service = btstack_memory_l2cap_service_get(); 5043 if (!service) { 5044 log_error("register: no memory for l2cap_service_t"); 5045 return BTSTACK_MEMORY_ALLOC_FAILED; 5046 } 5047 5048 // fill in 5049 service->psm = psm; 5050 service->mtu = 0; 5051 service->packet_handler = packet_handler; 5052 service->required_security_level = security_level; 5053 5054 // add to services list 5055 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 5056 5057 // done 5058 return ERROR_CODE_SUCCESS; 5059 } 5060 5061 uint8_t l2cap_cbm_unregister_service(uint16_t psm) { 5062 log_info("l2cap_cbm_unregister_service psm 0x%x", psm); 5063 l2cap_service_t *service = l2cap_cbm_get_service(psm); 5064 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 5065 5066 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 5067 btstack_memory_l2cap_service_free(service); 5068 return ERROR_CODE_SUCCESS; 5069 } 5070 5071 uint8_t l2cap_cbm_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 5072 // get channel 5073 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 5074 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 5075 5076 // validate state 5077 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 5078 return ERROR_CODE_COMMAND_DISALLOWED; 5079 } 5080 5081 // set state accept connection 5082 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 5083 channel->receive_sdu_buffer = receive_sdu_buffer; 5084 channel->local_mtu = mtu; 5085 channel->new_credits_incoming = initial_credits; 5086 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 5087 5088 // go 5089 l2cap_run(); 5090 return ERROR_CODE_SUCCESS; 5091 } 5092 5093 uint8_t l2cap_cbm_decline_connection(uint16_t local_cid, uint16_t result) { 5094 // get channel 5095 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 5096 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 5097 5098 // validate state 5099 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 5100 return ERROR_CODE_COMMAND_DISALLOWED; 5101 } 5102 5103 // set state decline connection 5104 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 5105 channel->reason = result; 5106 l2cap_run(); 5107 return ERROR_CODE_SUCCESS; 5108 } 5109 5110 static gap_security_level_t l2cap_cbm_security_level_for_connection(hci_con_handle_t con_handle){ 5111 uint8_t encryption_key_size = gap_encryption_key_size(con_handle); 5112 if (encryption_key_size == 0) return LEVEL_0; 5113 5114 bool authenticated = gap_authenticated(con_handle); 5115 if (!authenticated) return LEVEL_2; 5116 5117 return encryption_key_size == 16 ? LEVEL_4 : LEVEL_3; 5118 } 5119 5120 // used to handle pairing complete after triggering to increase 5121 static void l2cap_cbm_sm_packet_handler(uint8_t packet_type, uint16_t channel_nr, uint8_t *packet, uint16_t size) { 5122 UNUSED(channel_nr); 5123 UNUSED(size); 5124 UNUSED(packet_type); 5125 btstack_assert(packet_type == HCI_EVENT_PACKET); 5126 if (hci_event_packet_get_type(packet) != SM_EVENT_PAIRING_COMPLETE) return; 5127 hci_con_handle_t con_handle = sm_event_pairing_complete_get_handle(packet); 5128 btstack_linked_list_iterator_t it; 5129 btstack_linked_list_iterator_init(&it, &l2cap_channels); 5130 while (btstack_linked_list_iterator_has_next(&it)) { 5131 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 5132 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 5133 if (channel->con_handle != con_handle) continue; 5134 if (channel->state != L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE) continue; 5135 5136 // found channel, check security level 5137 if (l2cap_cbm_security_level_for_connection(con_handle) < channel->required_security_level){ 5138 // pairing failed or wasn't good enough, inform user 5139 l2cap_cbm_emit_channel_opened(channel, ERROR_CODE_INSUFFICIENT_SECURITY); 5140 // discard channel 5141 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 5142 l2cap_free_channel_entry(channel); 5143 } else { 5144 // send conn request now 5145 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 5146 l2cap_run(); 5147 } 5148 } 5149 } 5150 5151 uint8_t l2cap_cbm_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 5152 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 5153 uint16_t * out_local_cid) { 5154 5155 static btstack_packet_callback_registration_t sm_event_callback_registration; 5156 static bool sm_callback_registered = false; 5157 5158 log_info("create, handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 5159 5160 hci_connection_t * connection = hci_connection_for_handle(con_handle); 5161 if (!connection) { 5162 log_error("no hci_connection for handle 0x%04x", con_handle); 5163 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 5164 } 5165 5166 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, L2CAP_CHANNEL_TYPE_CHANNEL_CBM, connection->address, connection->address_type, psm, mtu, security_level); 5167 if (!channel) { 5168 return BTSTACK_MEMORY_ALLOC_FAILED; 5169 } 5170 log_info("created %p", channel); 5171 5172 // store local_cid 5173 if (out_local_cid){ 5174 *out_local_cid = channel->local_cid; 5175 } 5176 5177 // setup channel entry 5178 channel->con_handle = con_handle; 5179 channel->receive_sdu_buffer = receive_sdu_buffer; 5180 channel->new_credits_incoming = initial_credits; 5181 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 5182 5183 // add to connections list 5184 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 5185 5186 // check security level 5187 if (l2cap_cbm_security_level_for_connection(con_handle) < channel->required_security_level){ 5188 if (!sm_callback_registered){ 5189 sm_callback_registered = true; 5190 // lazy registration for SM events 5191 sm_event_callback_registration.callback = &l2cap_cbm_sm_packet_handler; 5192 sm_add_event_handler(&sm_event_callback_registration); 5193 } 5194 5195 // start pairing 5196 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 5197 sm_request_pairing(con_handle); 5198 } else { 5199 // send conn request right away 5200 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 5201 l2cap_run(); 5202 } 5203 5204 return ERROR_CODE_SUCCESS; 5205 } 5206 5207 uint8_t l2cap_cbm_provide_credits(uint16_t local_cid, uint16_t credits){ 5208 return l2cap_credit_based_provide_credits(local_cid, credits); 5209 } 5210 #endif 5211 5212 #ifdef ENABLE_L2CAP_ENHANCED_CREDIT_BASED_FLOW_CONTROL_MODE 5213 5214 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){ 5215 5216 // check for already registered psm 5217 l2cap_service_t *service = l2cap_cbm_get_service(psm); 5218 if (service) { 5219 return L2CAP_SERVICE_ALREADY_REGISTERED; 5220 } 5221 5222 // alloc structure 5223 service = btstack_memory_l2cap_service_get(); 5224 if (!service) { 5225 log_error("register: no memory for l2cap_service_t"); 5226 return BTSTACK_MEMORY_ALLOC_FAILED; 5227 } 5228 5229 // fill in 5230 service->psm = psm; 5231 service->mtu = min_remote_mtu; 5232 service->packet_handler = packet_handler; 5233 service->required_security_level = security_level; 5234 5235 // add to services list 5236 btstack_linked_list_add(&l2cap_enhanced_services, (btstack_linked_item_t *) service); 5237 5238 // done 5239 return ERROR_CODE_SUCCESS; 5240 } 5241 5242 uint8_t l2cap_ecbm_unregister_service(uint16_t psm) { 5243 l2cap_service_t *service = l2cap_cbm_get_service(psm); 5244 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 5245 5246 btstack_linked_list_remove(&l2cap_enhanced_services, (btstack_linked_item_t *) service); 5247 btstack_memory_l2cap_service_free(service); 5248 return ERROR_CODE_SUCCESS; 5249 } 5250 5251 void l2cap_ecbm_mps_set_min(uint16_t mps_min){ 5252 l2cap_enhanced_mps_min = mps_min; 5253 } 5254 5255 void l2cap_ecbm_mps_set_max(uint16_t mps_max){ 5256 l2cap_enhanced_mps_max = mps_max; 5257 } 5258 5259 uint8_t l2cap_ecbm_create_channels(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 5260 gap_security_level_t security_level, 5261 uint16_t psm, uint8_t num_channels, uint16_t initial_credits, uint16_t mtu, 5262 uint8_t ** receive_sdu_buffers, uint16_t * out_local_cid){ 5263 5264 log_info("create enhanced, handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 5265 5266 hci_connection_t * connection = hci_connection_for_handle(con_handle); 5267 if (!connection) { 5268 log_error("no hci_connection for handle 0x%04x", con_handle); 5269 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 5270 } 5271 5272 // setup all channels 5273 btstack_linked_list_t channels = NULL; 5274 uint8_t status = l2cap_ecbm_setup_channels(&channels, packet_handler, num_channels, connection, psm, mtu, 5275 security_level); 5276 5277 // add to connections list and set state + local_sig_id 5278 l2cap_channel_t * channel; 5279 uint8_t i = 0; 5280 uint8_t local_sig_id = l2cap_next_sig_id(); 5281 while (true) { 5282 channel = (l2cap_channel_t *) btstack_linked_list_pop(&channels); 5283 if (channel == NULL) break; 5284 channel->state = L2CAP_STATE_WILL_SEND_ENHANCED_CONNECTION_REQUEST; 5285 channel->local_sig_id = local_sig_id; 5286 channel->cid_index = i; 5287 channel->num_cids = num_channels; 5288 channel->credits_incoming = initial_credits; 5289 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 5290 channel->receive_sdu_buffer = receive_sdu_buffers[i]; 5291 // store local_cid 5292 if (out_local_cid){ 5293 out_local_cid[i] = channel->local_cid; 5294 } 5295 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 5296 i++; 5297 } 5298 5299 #if 0 5300 // check security level 5301 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 5302 if (!sm_callback_registered){ 5303 sm_callback_registered = true; 5304 // lazy registration for SM events 5305 sm_event_callback_registration.callback = &l2cap_sm_packet_handler; 5306 sm_add_event_handler(&sm_event_callback_registration); 5307 } 5308 5309 // start pairing 5310 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 5311 sm_request_pairing(con_handle); 5312 } else { 5313 // send conn request right away 5314 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 5315 l2cap_run(); 5316 } 5317 #endif 5318 5319 l2cap_run(); 5320 5321 return status; 5322 } 5323 5324 uint8_t l2cap_ecbm_accept_channels(uint16_t local_cid, uint8_t num_channels, uint16_t initial_credits, 5325 uint16_t receive_buffer_size, uint8_t ** receive_buffers, uint16_t * out_local_cids){ 5326 5327 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 5328 if (!channel) { 5329 5330 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 5331 } 5332 // 5333 hci_con_handle_t con_handle = channel->con_handle; 5334 uint8_t local_sig_id = channel->local_sig_id; 5335 uint8_t channel_index = 0; 5336 btstack_linked_list_iterator_t it; 5337 btstack_linked_list_iterator_init(&it, &l2cap_channels); 5338 while (btstack_linked_list_iterator_has_next(&it)) { 5339 channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 5340 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 5341 if (channel->con_handle != con_handle) continue; 5342 if (channel->local_sig_id != local_sig_id) continue; 5343 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT) continue; 5344 5345 if (channel_index < num_channels){ 5346 // assign buffer and cid 5347 out_local_cids[channel_index] = channel->local_cid; 5348 channel->receive_sdu_buffer = receive_buffers[channel_index]; 5349 channel->local_mtu = receive_buffer_size; 5350 channel->credits_incoming = initial_credits; 5351 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 5352 channel_index++; 5353 } else { 5354 // clear local cid for response packet 5355 channel->local_cid = 0; 5356 channel->reason = L2CAP_ECBM_CONNECTION_RESULT_SOME_REFUSED_INSUFFICIENT_RESOURCES_AVAILABLE; 5357 } 5358 // update state 5359 channel->state = L2CAP_STATE_WILL_SEND_ENHANCED_CONNECTION_RESPONSE; 5360 } 5361 l2cap_run(); 5362 return ERROR_CODE_SUCCESS; 5363 } 5364 5365 uint8_t l2cap_ecbm_decline_channels(uint16_t local_cid, uint16_t result){ 5366 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 5367 if (!channel) { 5368 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 5369 } 5370 // 5371 hci_con_handle_t con_handle = channel->con_handle; 5372 uint8_t local_sig_id = channel->local_sig_id; 5373 btstack_linked_list_iterator_t it; 5374 btstack_linked_list_iterator_init(&it, &l2cap_channels); 5375 while (btstack_linked_list_iterator_has_next(&it)) { 5376 channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 5377 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 5378 if (channel->con_handle != con_handle) continue; 5379 if (channel->local_sig_id != local_sig_id) continue; 5380 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT) continue; 5381 5382 // prepare response 5383 channel->local_cid = 0; 5384 channel->reason = result; 5385 channel->state = L2CAP_STATE_WILL_SEND_ENHANCED_CONNECTION_RESPONSE; 5386 } 5387 l2cap_run(); 5388 return ERROR_CODE_SUCCESS; 5389 } 5390 5391 uint8_t l2cap_ecbm_reconfigure_channels(uint8_t num_cids, uint16_t * local_cids, int16_t receive_buffer_size, uint8_t ** receive_buffers){ 5392 btstack_assert(receive_buffers != NULL); 5393 btstack_assert(local_cids != NULL); 5394 5395 if (num_cids > L2CAP_ECBM_MAX_CID_ARRAY_SIZE){ 5396 return ERROR_CODE_UNACCEPTABLE_CONNECTION_PARAMETERS; 5397 } 5398 5399 // check if all cids exist and have the same con handle 5400 uint8_t i; 5401 hci_con_handle_t con_handle = HCI_CON_HANDLE_INVALID; 5402 for (i = 0 ; i < num_cids ; i++){ 5403 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cids[i]); 5404 if (!channel) { 5405 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 5406 } 5407 if (channel->state != L2CAP_STATE_OPEN){ 5408 return ERROR_CODE_COMMAND_DISALLOWED; 5409 } 5410 if (con_handle == HCI_CON_HANDLE_INVALID){ 5411 con_handle = channel->con_handle; 5412 } else if (con_handle != channel->con_handle){ 5413 return ERROR_CODE_UNACCEPTABLE_CONNECTION_PARAMETERS; 5414 } 5415 } 5416 // set renegotiation data and state 5417 uint8_t sig_id = l2cap_next_sig_id(); 5418 for (i = 0 ; i < num_cids ; i++){ 5419 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cids[i]); 5420 channel->cid_index = i; 5421 channel->num_cids = num_cids; 5422 channel->local_sig_id = sig_id; 5423 channel->renegotiate_mtu = receive_buffer_size; 5424 channel->renegotiate_sdu_buffer = receive_buffers[i]; 5425 channel->state = L2CAP_STATE_WILL_SEND_EHNANCED_RENEGOTIATION_REQUEST; 5426 } 5427 5428 5429 l2cap_run(); 5430 return ERROR_CODE_SUCCESS; 5431 } 5432 5433 uint8_t l2cap_ecbm_provide_credits(uint16_t local_cid, uint16_t credits){ 5434 return l2cap_credit_based_provide_credits(local_cid, credits); 5435 } 5436 #endif 5437 5438 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 5439 // @deprecated - please use l2cap_ertm_create_channel 5440 uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, 5441 l2cap_ertm_config_t * ertm_contig, uint8_t * buffer, uint32_t size, uint16_t * out_local_cid){ 5442 log_error("deprecated - please use l2cap_ertm_create_channel"); 5443 return l2cap_ertm_create_channel(packet_handler, address, psm, ertm_contig, buffer, size, out_local_cid); 5444 }; 5445 5446 // @deprecated - please use l2cap_ertm_accept_connection 5447 uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, l2cap_ertm_config_t * ertm_contig, uint8_t * buffer, uint32_t size){ 5448 log_error("deprecated - please use l2cap_ertm_accept_connection"); 5449 return l2cap_ertm_accept_connection(local_cid, ertm_contig, buffer, size); 5450 } 5451 #endif 5452 5453 #ifdef ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE 5454 // @deprecated - please use l2cap_cbm_register_service 5455 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 5456 log_error("deprecated - please use l2cap_cbm_register_service"); 5457 return l2cap_cbm_register_service(packet_handler, psm, security_level); 5458 } 5459 5460 // @deprecated - please use l2cap_cbm_unregister_service 5461 uint8_t l2cap_le_unregister_service(uint16_t psm){ 5462 log_error("deprecated - please use l2cap_cbm_unregister_service"); 5463 return l2cap_cbm_unregister_service(psm); 5464 } 5465 5466 // @deprecated - please use l2cap_cbm_accept_connection 5467 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 5468 log_error("deprecated - please use l2cap_cbm_accept_connection"); 5469 return l2cap_cbm_accept_connection(local_cid, receive_sdu_buffer, mtu, initial_credits); 5470 } 5471 5472 // @deprecated - please use l2cap_cbm_decline_connection 5473 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 5474 log_error("deprecated - please use l2cap_cbm_decline_connection"); 5475 return l2cap_cbm_decline_connection(local_cid, L2CAP_CBM_CONNECTION_RESULT_NO_RESOURCES_AVAILABLE); 5476 } 5477 5478 // @deprecated - please use l2cap_cbm_create_channel 5479 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 5480 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 5481 uint16_t * out_local_cid){ 5482 log_error("deprecated - please use l2cap_cbm_create_channel"); 5483 return l2cap_cbm_create_channel(packet_handler, con_handle, psm, receive_sdu_buffer, mtu, initial_credits, security_level, out_local_cid); 5484 } 5485 5486 // @deprecated - please use l2cap_cbm_provide_credits 5487 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 5488 log_error("deprecated - please use l2cap_cbm_provide_credits"); 5489 return l2cap_cbm_provide_credits(local_cid, credits); 5490 } 5491 5492 // @deprecated - please use l2cap_can_send_packet_now 5493 bool l2cap_le_can_send_now(uint16_t local_cid){ 5494 log_error("deprecated - please use l2cap_can_send_packet_now"); 5495 return l2cap_can_send_packet_now(local_cid); 5496 } 5497 5498 // @deprecated - please use l2cap_request_can_send_now_event 5499 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 5500 log_error("deprecated - please use l2cap_request_can_send_now_event"); 5501 return l2cap_request_can_send_now_event(local_cid); 5502 } 5503 5504 // @deprecated - please use l2cap_cbm_send_data 5505 uint8_t l2cap_le_send_data(uint16_t local_cid, const uint8_t * data, uint16_t size){ 5506 log_error("deprecated - please use l2cap_cbm_send_data"); 5507 return l2cap_send(local_cid, data, size); 5508 } 5509 5510 // @deprecated - please use l2cap_disconnect 5511 uint8_t l2cap_le_disconnect(uint16_t local_cid){ 5512 log_error("deprecated - please use l2cap_disconnect"); 5513 return l2cap_disconnect(local_cid); 5514 } 5515 #endif 5516