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