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