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_LE_DATA_CHANNELS 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_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK 5 126 #define L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT 5 127 128 // offsets for L2CAP SIGNALING COMMANDS 129 #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET 0 130 #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET 1 131 #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2 132 #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET 4 133 134 #if defined(ENABLE_LE_DATA_CHANNELS) || defined(ENABLE_CLASSIC) 135 #define L2CAP_USES_CHANNELS 136 #endif 137 138 // prototypes 139 static void l2cap_run(void); 140 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 141 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size ); 142 static void l2cap_notify_channel_can_send(void); 143 static void l2cap_emit_can_send_now(btstack_packet_handler_t packet_handler, uint16_t channel); 144 static uint8_t l2cap_next_sig_id(void); 145 static l2cap_fixed_channel_t * l2cap_fixed_channel_for_channel_id(uint16_t local_cid); 146 #ifdef ENABLE_CLASSIC 147 static void l2cap_handle_security_level_incoming_sufficient(l2cap_channel_t * channel); 148 static int l2cap_security_level_0_allowed_for_PSM(uint16_t psm); 149 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel); 150 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel); 151 static void l2cap_finialize_channel_close(l2cap_channel_t *channel); 152 static inline l2cap_service_t * l2cap_get_service(uint16_t psm); 153 static void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status); 154 static void l2cap_emit_channel_closed(l2cap_channel_t *channel); 155 static void l2cap_emit_incoming_connection(l2cap_channel_t *channel); 156 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel); 157 #endif 158 #ifdef ENABLE_LE_DATA_CHANNELS 159 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status); 160 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel); 161 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel); 162 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel); 163 static void l2cap_le_finialize_channel_close(l2cap_channel_t *channel); 164 static void l2cap_le_send_pdu(l2cap_channel_t *channel); 165 static inline l2cap_service_t * l2cap_le_get_service(uint16_t psm); 166 #endif 167 #ifdef L2CAP_USES_CHANNELS 168 static uint16_t l2cap_next_local_cid(void); 169 static l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid); 170 static void l2cap_emit_simple_event_with_cid(l2cap_channel_t * channel, uint8_t event_code); 171 static void l2cap_dispatch_to_channel(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size); 172 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, 173 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level); 174 static void l2cap_free_channel_entry(l2cap_channel_t * channel); 175 #endif 176 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 177 static void l2cap_ertm_notify_channel_can_send(l2cap_channel_t * channel); 178 static void l2cap_ertm_monitor_timeout_callback(btstack_timer_source_t * ts); 179 static void l2cap_ertm_retransmission_timeout_callback(btstack_timer_source_t * ts); 180 #endif 181 182 // l2cap_fixed_channel_t entries 183 #ifdef ENABLE_BLE 184 static l2cap_fixed_channel_t l2cap_fixed_channel_att; 185 static l2cap_fixed_channel_t l2cap_fixed_channel_sm; 186 #endif 187 #ifdef ENABLE_CLASSIC 188 static l2cap_fixed_channel_t l2cap_fixed_channel_connectionless; 189 #endif 190 191 #ifdef ENABLE_CLASSIC 192 static btstack_linked_list_t l2cap_services; 193 static uint8_t l2cap_require_security_level2_for_outgoing_sdp; 194 static bd_addr_t l2cap_outgoing_classic_addr; 195 #endif 196 197 #ifdef ENABLE_LE_DATA_CHANNELS 198 static btstack_linked_list_t l2cap_le_services; 199 #endif 200 201 // single list of channels for Classic Channels, LE Data Channels, Classic Connectionless, ATT, and SM 202 static btstack_linked_list_t l2cap_channels; 203 #ifdef L2CAP_USES_CHANNELS 204 // next channel id for new connections 205 static uint16_t l2cap_local_source_cid; 206 #endif 207 // next signaling sequence number 208 static uint8_t l2cap_sig_seq_nr; 209 210 // used to cache l2cap rejects, echo, and informational requests 211 static l2cap_signaling_response_t l2cap_signaling_responses[NR_PENDING_SIGNALING_RESPONSES]; 212 static int l2cap_signaling_responses_pending; 213 static btstack_packet_callback_registration_t l2cap_hci_event_callback_registration; 214 215 #ifdef ENABLE_BLE 216 // only used for connection parameter update events 217 static uint16_t l2cap_le_custom_max_mtu; 218 #endif 219 220 /* callbacks for events */ 221 static btstack_linked_list_t l2cap_event_handlers; 222 223 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 224 225 // enable for testing 226 // #define L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 16 227 228 /* 229 * CRC lookup table for generator polynom D^16 + D^15 + D^2 + 1 230 */ 231 static const uint16_t crc16_table[256] = { 232 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, 233 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40, 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, 234 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40, 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41, 235 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641, 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040, 236 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, 237 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, 238 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40, 239 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640, 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041, 240 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240, 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441, 241 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, 242 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, 243 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640, 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041, 244 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241, 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440, 245 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, 246 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, 247 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040, 248 }; 249 250 static uint16_t crc16_calc(uint8_t * data, uint16_t len){ 251 uint16_t crc = 0; // initial value = 0 252 while (len--){ 253 crc = (crc >> 8) ^ crc16_table[ (crc ^ ((uint16_t) *data++)) & 0x00FF ]; 254 } 255 return crc; 256 } 257 258 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){ 259 return (((uint16_t) sar) << 14) | (req_seq << 8) | (final << 7) | (tx_seq << 1) | 0; 260 } 261 262 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){ 263 return (req_seq << 8) | (final << 7) | (poll << 4) | (((int) supervisory_function) << 2) | 1; 264 } 265 266 static int l2cap_next_ertm_seq_nr(int seq_nr){ 267 return (seq_nr + 1) & 0x3f; 268 } 269 270 static bool l2cap_ertm_can_store_packet_now(l2cap_channel_t * channel){ 271 // get num free tx buffers 272 int num_free_tx_buffers = channel->num_tx_buffers - channel->num_stored_tx_frames; 273 // calculate num tx buffers for remote MTU 274 int num_tx_buffers_for_max_remote_mtu; 275 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 276 if (channel->remote_mtu <= effective_mps){ 277 // MTU fits into single packet 278 num_tx_buffers_for_max_remote_mtu = 1; 279 } else { 280 // include SDU Length 281 num_tx_buffers_for_max_remote_mtu = (channel->remote_mtu + 2 + (effective_mps - 1)) / effective_mps; 282 } 283 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); 284 return num_tx_buffers_for_max_remote_mtu <= num_free_tx_buffers; 285 } 286 287 static void l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel_t * l2cap_channel){ 288 log_info("Retransmit unacknowleged frames"); 289 l2cap_channel->unacked_frames = 0;; 290 l2cap_channel->tx_send_index = l2cap_channel->tx_read_index; 291 } 292 293 static void l2cap_ertm_next_tx_write_index(l2cap_channel_t * channel){ 294 channel->tx_write_index++; 295 if (channel->tx_write_index < channel->num_tx_buffers) return; 296 channel->tx_write_index = 0; 297 } 298 299 static void l2cap_ertm_start_monitor_timer(l2cap_channel_t * channel){ 300 log_info("Start Monitor timer"); 301 btstack_run_loop_remove_timer(&channel->monitor_timer); 302 btstack_run_loop_set_timer_handler(&channel->monitor_timer, &l2cap_ertm_monitor_timeout_callback); 303 btstack_run_loop_set_timer_context(&channel->monitor_timer, channel); 304 btstack_run_loop_set_timer(&channel->monitor_timer, channel->local_monitor_timeout_ms); 305 btstack_run_loop_add_timer(&channel->monitor_timer); 306 } 307 308 static void l2cap_ertm_stop_monitor_timer(l2cap_channel_t * channel){ 309 log_info("Stop Monitor timer"); 310 btstack_run_loop_remove_timer(&channel->monitor_timer); 311 } 312 313 static void l2cap_ertm_start_retransmission_timer(l2cap_channel_t * channel){ 314 log_info("Start Retransmission timer"); 315 btstack_run_loop_remove_timer(&channel->retransmission_timer); 316 btstack_run_loop_set_timer_handler(&channel->retransmission_timer, &l2cap_ertm_retransmission_timeout_callback); 317 btstack_run_loop_set_timer_context(&channel->retransmission_timer, channel); 318 btstack_run_loop_set_timer(&channel->retransmission_timer, channel->local_retransmission_timeout_ms); 319 btstack_run_loop_add_timer(&channel->retransmission_timer); 320 } 321 322 static void l2cap_ertm_stop_retransmission_timer(l2cap_channel_t * l2cap_channel){ 323 log_info("Stop Retransmission timer"); 324 btstack_run_loop_remove_timer(&l2cap_channel->retransmission_timer); 325 } 326 327 static void l2cap_ertm_monitor_timeout_callback(btstack_timer_source_t * ts){ 328 log_info("Monitor timeout"); 329 l2cap_channel_t * l2cap_channel = (l2cap_channel_t *) btstack_run_loop_get_timer_context(ts); 330 331 // TODO: we assume that it's the oldest packet 332 l2cap_ertm_tx_packet_state_t * tx_state; 333 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 334 335 // check retry count 336 if (tx_state->retry_count < l2cap_channel->remote_max_transmit){ 337 // increment retry count 338 tx_state->retry_count++; 339 340 // start retransmit 341 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 342 343 // start monitor timer 344 l2cap_ertm_start_monitor_timer(l2cap_channel); 345 346 // send RR/P=1 347 l2cap_channel->send_supervisor_frame_receiver_ready_poll = 1; 348 } else { 349 log_info("Monitor timer expired & retry count >= max transmit -> disconnect"); 350 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 351 } 352 l2cap_run(); 353 } 354 355 static void l2cap_ertm_retransmission_timeout_callback(btstack_timer_source_t * ts){ 356 log_info("Retransmission 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 // set retry count = 1 364 tx_state->retry_count = 1; 365 366 // start retransmit 367 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 368 369 // start monitor timer 370 l2cap_ertm_start_monitor_timer(l2cap_channel); 371 372 // send RR/P=1 373 l2cap_channel->send_supervisor_frame_receiver_ready_poll = 1; 374 l2cap_run(); 375 } 376 377 static int l2cap_ertm_send_information_frame(l2cap_channel_t * channel, int index, int final){ 378 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index]; 379 hci_reserve_packet_buffer(); 380 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 381 uint16_t control = l2cap_encanced_control_field_for_information_frame(tx_state->tx_seq, final, channel->req_seq, tx_state->sar); 382 log_info("I-Frame: control 0x%04x", control); 383 little_endian_store_16(acl_buffer, 8, control); 384 (void)memcpy(&acl_buffer[8 + 2], 385 &channel->tx_packets_data[index * channel->local_mps], 386 tx_state->len); 387 // (re-)start retransmission timer on 388 l2cap_ertm_start_retransmission_timer(channel); 389 // send 390 return l2cap_send_prepared(channel->local_cid, 2 + tx_state->len); 391 } 392 393 static void l2cap_ertm_store_fragment(l2cap_channel_t * channel, l2cap_segmentation_and_reassembly_t sar, uint16_t sdu_length, uint8_t * data, uint16_t len){ 394 // get next index for storing packets 395 int index = channel->tx_write_index; 396 397 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index]; 398 tx_state->tx_seq = channel->next_tx_seq; 399 tx_state->sar = sar; 400 tx_state->retry_count = 0; 401 402 uint8_t * tx_packet = &channel->tx_packets_data[index * channel->local_mps]; 403 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); 404 int pos = 0; 405 if (sar == L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU){ 406 little_endian_store_16(tx_packet, 0, sdu_length); 407 pos += 2; 408 } 409 (void)memcpy(&tx_packet[pos], data, len); 410 tx_state->len = pos + len; 411 412 // update 413 channel->num_stored_tx_frames++; 414 channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq); 415 l2cap_ertm_next_tx_write_index(channel); 416 417 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); 418 419 } 420 421 static uint8_t l2cap_ertm_send(l2cap_channel_t * channel, uint8_t * data, uint16_t len){ 422 if (len > channel->remote_mtu){ 423 log_error("l2cap_ertm_send cid 0x%02x, data length exceeds remote MTU.", channel->local_cid); 424 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 425 } 426 427 if (!l2cap_ertm_can_store_packet_now(channel)){ 428 log_error("l2cap_ertm_send cid 0x%02x, fragment store full", channel->local_cid); 429 return BTSTACK_ACL_BUFFERS_FULL; 430 } 431 432 // check if it needs to get fragmented 433 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 434 if (len > effective_mps){ 435 // fragmentation needed. 436 l2cap_segmentation_and_reassembly_t sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU; 437 int chunk_len; 438 while (len){ 439 switch (sar){ 440 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 441 chunk_len = effective_mps - 2; // sdu_length 442 l2cap_ertm_store_fragment(channel, sar, len, data, chunk_len); 443 len -= chunk_len; 444 sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU; 445 break; 446 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 447 chunk_len = effective_mps; 448 if (chunk_len >= len){ 449 sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU; 450 chunk_len = len; 451 } 452 l2cap_ertm_store_fragment(channel, sar, len, data, chunk_len); 453 len -= chunk_len; 454 break; 455 default: 456 break; 457 } 458 } 459 460 } else { 461 l2cap_ertm_store_fragment(channel, L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU, 0, data, len); 462 } 463 464 // try to send 465 l2cap_notify_channel_can_send(); 466 return ERROR_CODE_SUCCESS; 467 } 468 469 static uint16_t l2cap_setup_options_ertm_request(l2cap_channel_t * channel, uint8_t * config_options){ 470 int pos = 0; 471 config_options[pos++] = L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL; 472 config_options[pos++] = 9; // length 473 config_options[pos++] = (uint8_t) channel->mode; 474 config_options[pos++] = channel->num_rx_buffers; // == TxWindows size 475 config_options[pos++] = channel->local_max_transmit; 476 little_endian_store_16( config_options, pos, channel->local_retransmission_timeout_ms); 477 pos += 2; 478 little_endian_store_16( config_options, pos, channel->local_monitor_timeout_ms); 479 pos += 2; 480 little_endian_store_16( config_options, pos, channel->local_mps); 481 pos += 2; 482 // 483 config_options[pos++] = L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT; 484 config_options[pos++] = 2; // length 485 little_endian_store_16(config_options, pos, channel->local_mtu); 486 pos += 2; 487 488 // Issue: iOS (e.g. 10.2) uses "No FCS" as default while Core 5.0 specifies "FCS" as default 489 // Workaround: try to actively negotiate FCS option 490 config_options[pos++] = L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE; 491 config_options[pos++] = 1; // length 492 config_options[pos++] = channel->fcs_option; 493 return pos; // 11+4+3=18 494 } 495 496 static uint16_t l2cap_setup_options_ertm_response(l2cap_channel_t * channel, uint8_t * config_options){ 497 int pos = 0; 498 config_options[pos++] = L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL; 499 config_options[pos++] = 9; // length 500 config_options[pos++] = (uint8_t) channel->mode; 501 // less or equal to remote tx window size 502 config_options[pos++] = btstack_min(channel->num_tx_buffers, channel->remote_tx_window_size); 503 // max transmit in response shall be ignored -> use sender values 504 config_options[pos++] = channel->remote_max_transmit; 505 // A value for the Retransmission time-out shall be sent in a positive Configuration Response 506 // and indicates the value that will be used by the sender of the Configuration Response -> use our value 507 little_endian_store_16( config_options, pos, channel->local_retransmission_timeout_ms); 508 pos += 2; 509 // A value for the Monitor time-out shall be sent in a positive Configuration Response 510 // and indicates the value that will be used by the sender of the Configuration Response -> use our value 511 little_endian_store_16( config_options, pos, channel->local_monitor_timeout_ms); 512 pos += 2; 513 // less or equal to remote mps 514 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 515 little_endian_store_16( config_options, pos, effective_mps); 516 pos += 2; 517 // 518 config_options[pos++] = L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT; // MTU 519 config_options[pos++] = 2; // length 520 little_endian_store_16(config_options, pos, channel->remote_mtu); 521 pos += 2; 522 #if 0 523 // 524 config_options[pos++] = L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE; 525 config_options[pos++] = 1; // length 526 config_options[pos++] = channel->fcs_option; 527 #endif 528 return pos; // 11+4=15 529 } 530 531 static int l2cap_ertm_send_supervisor_frame(l2cap_channel_t * channel, uint16_t control){ 532 hci_reserve_packet_buffer(); 533 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 534 log_info("S-Frame: control 0x%04x", control); 535 little_endian_store_16(acl_buffer, 8, control); 536 return l2cap_send_prepared(channel->local_cid, 2); 537 } 538 539 static uint8_t l2cap_ertm_validate_local_config(l2cap_ertm_config_t * ertm_config){ 540 541 uint8_t result = ERROR_CODE_SUCCESS; 542 if (ertm_config->max_transmit < 1){ 543 log_error("max_transmit must be >= 1"); 544 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 545 } 546 if (ertm_config->retransmission_timeout_ms < 2000){ 547 log_error("retransmission_timeout_ms must be >= 2000 ms"); 548 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 549 } 550 if (ertm_config->monitor_timeout_ms < 12000){ 551 log_error("monitor_timeout_ms must be >= 12000 ms"); 552 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 553 } 554 if (ertm_config->local_mtu < 48){ 555 log_error("local_mtu must be >= 48"); 556 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 557 } 558 if (ertm_config->num_rx_buffers < 1){ 559 log_error("num_rx_buffers must be >= 1"); 560 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 561 } 562 if (ertm_config->num_tx_buffers < 1){ 563 log_error("num_rx_buffers must be >= 1"); 564 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 565 } 566 return result; 567 } 568 569 static void l2cap_ertm_configure_channel(l2cap_channel_t * channel, l2cap_ertm_config_t * ertm_config, uint8_t * buffer, uint32_t size){ 570 571 channel->mode = L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION; 572 channel->ertm_mandatory = ertm_config->ertm_mandatory; 573 channel->local_max_transmit = ertm_config->max_transmit; 574 channel->local_retransmission_timeout_ms = ertm_config->retransmission_timeout_ms; 575 channel->local_monitor_timeout_ms = ertm_config->monitor_timeout_ms; 576 channel->local_mtu = ertm_config->local_mtu; 577 channel->num_rx_buffers = ertm_config->num_rx_buffers; 578 channel->num_tx_buffers = ertm_config->num_tx_buffers; 579 580 // align buffer to 16-byte boundary to assert l2cap_ertm_rx_packet_state_t is aligned 581 int bytes_till_alignment = 16 - (((uintptr_t) buffer) & 0x0f); 582 buffer += bytes_till_alignment; 583 size -= bytes_till_alignment; 584 585 // setup state buffers - use void cast to avoid -Wcast-align warning 586 uint32_t pos = 0; 587 channel->rx_packets_state = (l2cap_ertm_rx_packet_state_t *) (void *) &buffer[pos]; 588 pos += ertm_config->num_rx_buffers * sizeof(l2cap_ertm_rx_packet_state_t); 589 channel->tx_packets_state = (l2cap_ertm_tx_packet_state_t *) (void *) &buffer[pos]; 590 pos += ertm_config->num_tx_buffers * sizeof(l2cap_ertm_tx_packet_state_t); 591 592 // setup reassembly buffer 593 channel->reassembly_buffer = &buffer[pos]; 594 pos += ertm_config->local_mtu; 595 596 // divide rest of data equally 597 channel->local_mps = (size - pos) / (ertm_config->num_rx_buffers + ertm_config->num_tx_buffers); 598 log_info("Local MPS: %u", channel->local_mps); 599 channel->rx_packets_data = &buffer[pos]; 600 pos += ertm_config->num_rx_buffers * channel->local_mps; 601 channel->tx_packets_data = &buffer[pos]; 602 603 channel->fcs_option = ertm_config->fcs_option; 604 } 605 606 uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, 607 l2cap_ertm_config_t * ertm_config, uint8_t * buffer, uint32_t size, uint16_t * out_local_cid){ 608 609 log_info("L2CAP_CREATE_ERTM_CHANNEL addr %s, psm 0x%x, local mtu %u", bd_addr_to_str(address), psm, ertm_config->local_mtu); 610 611 // validate local config 612 uint8_t result = l2cap_ertm_validate_local_config(ertm_config); 613 if (result) return result; 614 615 // determine security level based on psm 616 const gap_security_level_t security_level = l2cap_security_level_0_allowed_for_PSM(psm) ? LEVEL_0 : gap_get_security_level(); 617 618 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); 619 if (!channel) { 620 return BTSTACK_MEMORY_ALLOC_FAILED; 621 } 622 623 // configure ERTM 624 l2cap_ertm_configure_channel(channel, ertm_config, buffer, size); 625 626 // add to connections list 627 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 628 629 // store local_cid 630 if (out_local_cid){ 631 *out_local_cid = channel->local_cid; 632 } 633 634 // check if hci connection is already usable 635 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_ACL); 636 if (conn){ 637 log_info("l2cap_create_channel, hci connection already exists"); 638 l2cap_handle_connection_complete(conn->con_handle, channel); 639 // check if remote supported fearures are already received 640 if (hci_remote_features_available(conn->con_handle)) { 641 l2cap_handle_remote_supported_features_received(channel); 642 } else { 643 hci_remote_features_query(conn->con_handle); 644 }; 645 } 646 647 l2cap_run(); 648 649 return 0; 650 } 651 652 static void l2cap_ertm_notify_channel_can_send(l2cap_channel_t * channel){ 653 if (l2cap_ertm_can_store_packet_now(channel)){ 654 channel->waiting_for_can_send_now = 0; 655 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 656 } 657 } 658 659 uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, l2cap_ertm_config_t * ertm_config, uint8_t * buffer, uint32_t size){ 660 661 log_info("L2CAP_ACCEPT_ERTM_CONNECTION local_cid 0x%x", local_cid); 662 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 663 if (!channel) { 664 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 665 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 666 } 667 668 // validate local config 669 uint8_t result = l2cap_ertm_validate_local_config(ertm_config); 670 if (result) return result; 671 672 // configure L2CAP ERTM 673 l2cap_ertm_configure_channel(channel, ertm_config, buffer, size); 674 675 // default: continue 676 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 677 678 // verify remote ERTM support 679 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 680 if (connection == NULL) return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 681 682 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 683 // ERTM not possible, select basic mode and release buffer 684 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 685 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 686 687 // bail if ERTM is mandatory 688 if (channel->ertm_mandatory){ 689 // We chose 'no resources available' for "ERTM mandatory but you don't even know ERTM exists" 690 log_info("ERTM mandatory -> reject connection"); 691 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 692 channel->reason = 0x04; // no resources available 693 } else { 694 log_info("ERTM not supported by remote -> use Basic mode"); 695 } 696 } 697 698 // process 699 l2cap_run(); 700 701 return ERROR_CODE_SUCCESS; 702 } 703 704 uint8_t l2cap_ertm_set_busy(uint16_t local_cid){ 705 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 706 if (!channel) { 707 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 708 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 709 } 710 if (!channel->local_busy){ 711 channel->local_busy = 1; 712 channel->send_supervisor_frame_receiver_not_ready = 1; 713 l2cap_run(); 714 } 715 return ERROR_CODE_SUCCESS; 716 } 717 718 uint8_t l2cap_ertm_set_ready(uint16_t local_cid){ 719 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 720 if (!channel) { 721 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 722 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 723 } 724 if (channel->local_busy){ 725 channel->local_busy = 0; 726 channel->send_supervisor_frame_receiver_ready_poll = 1; 727 l2cap_run(); 728 } 729 return ERROR_CODE_SUCCESS; 730 } 731 732 // Process-ReqSeq 733 static void l2cap_ertm_process_req_seq(l2cap_channel_t * l2cap_channel, uint8_t req_seq){ 734 int num_buffers_acked = 0; 735 l2cap_ertm_tx_packet_state_t * tx_state; 736 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); 737 while (true){ 738 739 // no unack packets left 740 if (l2cap_channel->unacked_frames == 0) { 741 // stop retransmission timer 742 l2cap_ertm_stop_retransmission_timer(l2cap_channel); 743 break; 744 } 745 746 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 747 // calc delta 748 int delta = (req_seq - tx_state->tx_seq) & 0x03f; 749 if (delta == 0) break; // all packets acknowledged 750 if (delta > l2cap_channel->remote_tx_window_size) break; 751 752 num_buffers_acked++; 753 l2cap_channel->num_stored_tx_frames--; 754 l2cap_channel->unacked_frames--; 755 log_info("RR seq %u => packet with tx_seq %u done", req_seq, tx_state->tx_seq); 756 757 l2cap_channel->tx_read_index++; 758 if (l2cap_channel->tx_read_index >= l2cap_channel->num_rx_buffers){ 759 l2cap_channel->tx_read_index = 0; 760 } 761 } 762 if (num_buffers_acked){ 763 log_info("num_buffers_acked %u", num_buffers_acked); 764 l2cap_ertm_notify_channel_can_send(l2cap_channel); 765 } 766 } 767 768 static l2cap_ertm_tx_packet_state_t * l2cap_ertm_get_tx_state(l2cap_channel_t * l2cap_channel, uint8_t tx_seq){ 769 int i; 770 for (i=0;i<l2cap_channel->num_tx_buffers;i++){ 771 l2cap_ertm_tx_packet_state_t * tx_state = &l2cap_channel->tx_packets_state[i]; 772 if (tx_state->tx_seq == tx_seq) return tx_state; 773 } 774 return NULL; 775 } 776 777 // @param delta number of frames in the future, >= 1 778 // @assumption size <= l2cap_channel->local_mps (checked in l2cap_acl_classic_handler) 779 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){ 780 log_info("Store SDU with delta %u", delta); 781 // get rx state for packet to store 782 int index = l2cap_channel->rx_store_index + delta - 1; 783 if (index > l2cap_channel->num_rx_buffers){ 784 index -= l2cap_channel->num_rx_buffers; 785 } 786 log_info("Index of packet to store %u", index); 787 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 788 // check if buffer is free 789 if (rx_state->valid){ 790 log_error("Packet buffer already used"); 791 return; 792 } 793 rx_state->valid = 1; 794 rx_state->sar = sar; 795 rx_state->len = size; 796 uint8_t * rx_buffer = &l2cap_channel->rx_packets_data[index]; 797 (void)memcpy(rx_buffer, payload, size); 798 } 799 800 // @assumption size <= l2cap_channel->local_mps (checked in l2cap_acl_classic_handler) 801 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){ 802 uint16_t reassembly_sdu_length; 803 switch (sar){ 804 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 805 // assert total packet size <= our mtu 806 if (size > l2cap_channel->local_mtu) break; 807 // packet complete -> disapatch 808 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, (uint8_t*) payload, size); 809 break; 810 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 811 // read SDU len 812 reassembly_sdu_length = little_endian_read_16(payload, 0); 813 payload += 2; 814 size -= 2; 815 // assert reassembled size <= our mtu 816 if (reassembly_sdu_length > l2cap_channel->local_mtu) break; 817 // store start segment 818 l2cap_channel->reassembly_sdu_length = reassembly_sdu_length; 819 (void)memcpy(&l2cap_channel->reassembly_buffer[0], payload, size); 820 l2cap_channel->reassembly_pos = size; 821 break; 822 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 823 // assert size of reassembled data <= our mtu 824 if (l2cap_channel->reassembly_pos + size > l2cap_channel->local_mtu) break; 825 // store continuation segment 826 (void)memcpy(&l2cap_channel->reassembly_buffer[l2cap_channel->reassembly_pos], 827 payload, size); 828 l2cap_channel->reassembly_pos += size; 829 break; 830 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 831 // assert size of reassembled data <= our mtu 832 if (l2cap_channel->reassembly_pos + size > l2cap_channel->local_mtu) break; 833 // store continuation segment 834 (void)memcpy(&l2cap_channel->reassembly_buffer[l2cap_channel->reassembly_pos], 835 payload, size); 836 l2cap_channel->reassembly_pos += size; 837 // assert size of reassembled data matches announced sdu length 838 if (l2cap_channel->reassembly_pos != l2cap_channel->reassembly_sdu_length) break; 839 // packet complete -> disapatch 840 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->reassembly_buffer, l2cap_channel->reassembly_pos); 841 l2cap_channel->reassembly_pos = 0; 842 break; 843 default: 844 btstack_assert(false); 845 break; 846 } 847 } 848 849 static void l2cap_ertm_channel_send_information_frame(l2cap_channel_t * channel){ 850 channel->unacked_frames++; 851 int index = channel->tx_send_index; 852 channel->tx_send_index++; 853 if (channel->tx_send_index >= channel->num_tx_buffers){ 854 channel->tx_send_index = 0; 855 } 856 l2cap_ertm_send_information_frame(channel, index, 0); // final = 0 857 } 858 859 #endif 860 861 #ifdef L2CAP_USES_CHANNELS 862 static uint16_t l2cap_next_local_cid(void){ 863 do { 864 if (l2cap_local_source_cid == 0xffffu) { 865 l2cap_local_source_cid = 0x40; 866 } else { 867 l2cap_local_source_cid++; 868 } 869 } while (l2cap_get_channel_for_local_cid(l2cap_local_source_cid) != NULL); 870 return l2cap_local_source_cid; 871 } 872 #endif 873 874 static uint8_t l2cap_next_sig_id(void){ 875 if (l2cap_sig_seq_nr == 0xffu) { 876 l2cap_sig_seq_nr = 1; 877 } else { 878 l2cap_sig_seq_nr++; 879 } 880 return l2cap_sig_seq_nr; 881 } 882 883 void l2cap_init(void){ 884 #ifdef L2CAP_USES_CHANNELS 885 l2cap_local_source_cid = 0x40; 886 #endif 887 l2cap_sig_seq_nr = 0xff; 888 889 #ifdef ENABLE_CLASSIC 890 // Setup Connectionless Channel 891 l2cap_fixed_channel_connectionless.local_cid = L2CAP_CID_CONNECTIONLESS_CHANNEL; 892 l2cap_fixed_channel_connectionless.channel_type = L2CAP_CHANNEL_TYPE_CONNECTIONLESS; 893 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) &l2cap_fixed_channel_connectionless); 894 #endif 895 896 #ifdef ENABLE_BLE 897 // Setup fixed ATT Channel 898 l2cap_fixed_channel_att.local_cid = L2CAP_CID_ATTRIBUTE_PROTOCOL; 899 l2cap_fixed_channel_att.channel_type = L2CAP_CHANNEL_TYPE_FIXED; 900 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) &l2cap_fixed_channel_att); 901 902 // Setup fixed SM Channel 903 l2cap_fixed_channel_sm.local_cid = L2CAP_CID_SECURITY_MANAGER_PROTOCOL; 904 l2cap_fixed_channel_sm.channel_type = L2CAP_CHANNEL_TYPE_FIXED; 905 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) &l2cap_fixed_channel_sm); 906 #endif 907 908 // 909 // register callback with HCI 910 // 911 l2cap_hci_event_callback_registration.callback = &l2cap_hci_event_handler; 912 hci_add_event_handler(&l2cap_hci_event_callback_registration); 913 914 hci_register_acl_packet_handler(&l2cap_acl_handler); 915 916 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 917 #ifdef ENABLE_CLASSIC 918 gap_connectable_control(0); // no services yet 919 #endif 920 #endif 921 } 922 923 /** 924 * @brief De-Init L2CAP 925 */ 926 void l2cap_deinit(void){ 927 l2cap_channels = NULL; 928 l2cap_signaling_responses_pending = 0; 929 #ifdef ENABLE_CLASSIC 930 l2cap_require_security_level2_for_outgoing_sdp = 0; 931 (void)memset(&l2cap_fixed_channel_connectionless, 0, sizeof(l2cap_fixed_channel_connectionless)); 932 l2cap_services = NULL; 933 (void)memset(l2cap_outgoing_classic_addr, 0, 6); 934 #endif 935 #ifdef ENABLE_BLE 936 l2cap_le_custom_max_mtu = 0; 937 (void)memset(&l2cap_fixed_channel_att, 0, sizeof(l2cap_fixed_channel_att)); 938 (void)memset(&l2cap_fixed_channel_sm, 0, sizeof(l2cap_fixed_channel_sm)); 939 #endif 940 #ifdef ENABLE_LE_DATA_CHANNELS 941 l2cap_le_services = NULL; 942 #endif 943 l2cap_event_handlers = NULL; 944 } 945 946 void l2cap_add_event_handler(btstack_packet_callback_registration_t * callback_handler){ 947 btstack_linked_list_add_tail(&l2cap_event_handlers, (btstack_linked_item_t*) callback_handler); 948 } 949 950 void l2cap_remove_event_handler(btstack_packet_callback_registration_t * callback_handler){ 951 btstack_linked_list_remove(&l2cap_event_handlers, (btstack_linked_item_t*) callback_handler); 952 } 953 954 static void l2cap_emit_event(uint8_t *event, uint16_t size) { 955 hci_dump_packet( HCI_EVENT_PACKET, 0, event, size); 956 // dispatch to all event handlers 957 btstack_linked_list_iterator_t it; 958 btstack_linked_list_iterator_init(&it, &l2cap_event_handlers); 959 while (btstack_linked_list_iterator_has_next(&it)){ 960 btstack_packet_callback_registration_t * entry = (btstack_packet_callback_registration_t*) btstack_linked_list_iterator_next(&it); 961 entry->callback(HCI_EVENT_PACKET, 0, event, size); 962 } 963 } 964 965 void l2cap_request_can_send_fix_channel_now_event(hci_con_handle_t con_handle, uint16_t channel_id){ 966 UNUSED(con_handle); // ok: there is no con handle 967 968 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 969 if (!channel) return; 970 channel->waiting_for_can_send_now = 1; 971 l2cap_notify_channel_can_send(); 972 } 973 974 bool l2cap_can_send_fixed_channel_packet_now(hci_con_handle_t con_handle, uint16_t channel_id){ 975 UNUSED(channel_id); // ok: only depends on Controller LE buffers 976 977 return hci_can_send_acl_packet_now(con_handle); 978 } 979 980 uint8_t *l2cap_get_outgoing_buffer(void){ 981 return hci_get_outgoing_packet_buffer() + COMPLETE_L2CAP_HEADER; // 8 bytes 982 } 983 984 // only for L2CAP Basic Channels 985 bool l2cap_reserve_packet_buffer(void){ 986 return hci_reserve_packet_buffer(); 987 } 988 989 // only for L2CAP Basic Channels 990 void l2cap_release_packet_buffer(void){ 991 hci_release_packet_buffer(); 992 } 993 994 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){ 995 // 0 - Connection handle : PB=pb : BC=00 996 little_endian_store_16(acl_buffer, 0u, con_handle | (packet_boundary << 12u) | (0u << 14u)); 997 // 2 - ACL length 998 little_endian_store_16(acl_buffer, 2u, len + 4u); 999 // 4 - L2CAP packet length 1000 little_endian_store_16(acl_buffer, 4u, len + 0u); 1001 // 6 - L2CAP channel DEST 1002 little_endian_store_16(acl_buffer, 6, remote_cid); 1003 } 1004 1005 // assumption - only on LE connections 1006 uint8_t l2cap_send_prepared_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint16_t len){ 1007 1008 if (!hci_is_packet_buffer_reserved()){ 1009 log_error("l2cap_send_prepared_connectionless called without reserving packet first"); 1010 return BTSTACK_ACL_BUFFERS_FULL; 1011 } 1012 1013 if (!hci_can_send_prepared_acl_packet_now(con_handle)){ 1014 log_info("l2cap_send_prepared_connectionless handle 0x%02x, cid 0x%02x, cannot send", con_handle, cid); 1015 return BTSTACK_ACL_BUFFERS_FULL; 1016 } 1017 1018 log_debug("l2cap_send_prepared_connectionless handle %u, cid 0x%02x", con_handle, cid); 1019 1020 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1021 l2cap_setup_header(acl_buffer, con_handle, 0, cid, len); 1022 // send 1023 return hci_send_acl_packet_buffer(len+8u); 1024 } 1025 1026 // assumption - only on LE connections 1027 uint8_t l2cap_send_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint8_t *data, uint16_t len){ 1028 1029 if (!hci_can_send_acl_packet_now(con_handle)){ 1030 log_info("l2cap_send cid 0x%02x, cannot send", cid); 1031 return BTSTACK_ACL_BUFFERS_FULL; 1032 } 1033 1034 hci_reserve_packet_buffer(); 1035 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1036 1037 (void)memcpy(&acl_buffer[8], data, len); 1038 1039 return l2cap_send_prepared_connectionless(con_handle, cid, len); 1040 } 1041 1042 static void l2cap_emit_can_send_now(btstack_packet_handler_t packet_handler, uint16_t channel) { 1043 log_debug("L2CAP_EVENT_CHANNEL_CAN_SEND_NOW local_cid 0x%x", channel); 1044 uint8_t event[4]; 1045 event[0] = L2CAP_EVENT_CAN_SEND_NOW; 1046 event[1] = sizeof(event) - 2u; 1047 little_endian_store_16(event, 2, channel); 1048 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1049 packet_handler(HCI_EVENT_PACKET, channel, event, sizeof(event)); 1050 } 1051 1052 #ifdef L2CAP_USES_CHANNELS 1053 static void l2cap_dispatch_to_channel(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){ 1054 (* (channel->packet_handler))(type, channel->local_cid, data, size); 1055 } 1056 1057 static void l2cap_emit_simple_event_with_cid(l2cap_channel_t * channel, uint8_t event_code){ 1058 uint8_t event[4]; 1059 event[0] = event_code; 1060 event[1] = sizeof(event) - 2u; 1061 little_endian_store_16(event, 2, channel->local_cid); 1062 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1063 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 1064 } 1065 #endif 1066 1067 #ifdef ENABLE_CLASSIC 1068 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 1069 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", 1070 status, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 1071 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu, channel->flush_timeout); 1072 uint8_t event[26]; 1073 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 1074 event[1] = sizeof(event) - 2; 1075 event[2] = status; 1076 reverse_bd_addr(channel->address, &event[3]); 1077 little_endian_store_16(event, 9, channel->con_handle); 1078 little_endian_store_16(event, 11, channel->psm); 1079 little_endian_store_16(event, 13, channel->local_cid); 1080 little_endian_store_16(event, 15, channel->remote_cid); 1081 little_endian_store_16(event, 17, channel->local_mtu); 1082 little_endian_store_16(event, 19, channel->remote_mtu); 1083 little_endian_store_16(event, 21, channel->flush_timeout); 1084 event[23] = (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) ? 1 : 0; 1085 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1086 log_info("ERTM mode %u, fcs enabled %u", channel->mode, channel->fcs_option); 1087 event[24] = channel->mode; 1088 event[25] = channel->fcs_option; 1089 1090 #else 1091 event[24] = L2CAP_CHANNEL_MODE_BASIC; 1092 event[25] = 0; 1093 #endif 1094 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1095 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 1096 } 1097 1098 static void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 1099 log_info("L2CAP_EVENT_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 1100 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 1101 } 1102 1103 static void l2cap_emit_incoming_connection(l2cap_channel_t *channel) { 1104 log_info("L2CAP_EVENT_INCOMING_CONNECTION addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x", 1105 bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid); 1106 uint8_t event[16]; 1107 event[0] = L2CAP_EVENT_INCOMING_CONNECTION; 1108 event[1] = sizeof(event) - 2; 1109 reverse_bd_addr(channel->address, &event[2]); 1110 little_endian_store_16(event, 8, channel->con_handle); 1111 little_endian_store_16(event, 10, channel->psm); 1112 little_endian_store_16(event, 12, channel->local_cid); 1113 little_endian_store_16(event, 14, channel->remote_cid); 1114 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1115 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 1116 } 1117 1118 static void l2cap_handle_channel_open_failed(l2cap_channel_t * channel, uint8_t status){ 1119 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1120 // emit ertm buffer released, as it's not needed. if in basic mode, it was either not allocated or already released 1121 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1122 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 1123 } 1124 #endif 1125 l2cap_emit_channel_opened(channel, status); 1126 } 1127 1128 static void l2cap_handle_channel_closed(l2cap_channel_t * channel){ 1129 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1130 // emit ertm buffer released, as it's not needed anymore. if in basic mode, it was either not allocated or already released 1131 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1132 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 1133 } 1134 #endif 1135 l2cap_emit_channel_closed(channel); 1136 } 1137 #endif 1138 1139 static l2cap_fixed_channel_t * l2cap_channel_item_by_cid(uint16_t cid){ 1140 btstack_linked_list_iterator_t it; 1141 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1142 while (btstack_linked_list_iterator_has_next(&it)){ 1143 l2cap_fixed_channel_t * channel = (l2cap_fixed_channel_t*) btstack_linked_list_iterator_next(&it); 1144 if (channel->local_cid == cid) { 1145 return channel; 1146 } 1147 } 1148 return NULL; 1149 } 1150 1151 // used for fixed channels in LE (ATT/SM) and Classic (Connectionless Channel). CID < 0x04 1152 static l2cap_fixed_channel_t * l2cap_fixed_channel_for_channel_id(uint16_t local_cid){ 1153 if (local_cid >= 0x40u) return NULL; 1154 return (l2cap_fixed_channel_t*) l2cap_channel_item_by_cid(local_cid); 1155 } 1156 1157 // used for Classic Channels + LE Data Channels. local_cid >= 0x40 1158 #ifdef L2CAP_USES_CHANNELS 1159 static l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){ 1160 if (local_cid < 0x40u) return NULL; 1161 return (l2cap_channel_t*) l2cap_channel_item_by_cid(local_cid); 1162 } 1163 1164 static l2cap_channel_t * l2cap_get_channel_for_local_cid_and_handle(uint16_t local_cid, hci_con_handle_t con_handle){ 1165 if (local_cid < 0x40u) return NULL; 1166 l2cap_channel_t * l2cap_channel = (l2cap_channel_t*) l2cap_channel_item_by_cid(local_cid); 1167 if (l2cap_channel == NULL) return NULL; 1168 if (l2cap_channel->con_handle != con_handle) return NULL; 1169 return l2cap_channel; 1170 } 1171 #endif 1172 1173 #ifdef ENABLE_CLASSIC 1174 void l2cap_request_can_send_now_event(uint16_t local_cid){ 1175 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 1176 if (!channel) return; 1177 channel->waiting_for_can_send_now = 1; 1178 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1179 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1180 l2cap_ertm_notify_channel_can_send(channel); 1181 return; 1182 } 1183 #endif 1184 l2cap_notify_channel_can_send(); 1185 } 1186 1187 bool l2cap_can_send_packet_now(uint16_t local_cid){ 1188 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 1189 if (!channel) return false; 1190 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1191 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1192 return l2cap_ertm_can_store_packet_now(channel); 1193 } 1194 #endif 1195 return hci_can_send_acl_packet_now(channel->con_handle); 1196 } 1197 1198 bool l2cap_can_send_prepared_packet_now(uint16_t local_cid){ 1199 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 1200 if (!channel) return false; 1201 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1202 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1203 return false; 1204 } 1205 #endif 1206 return hci_can_send_prepared_acl_packet_now(channel->con_handle); 1207 } 1208 1209 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){ 1210 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1211 if (channel) { 1212 return channel->remote_mtu; 1213 } 1214 return 0; 1215 } 1216 #endif 1217 1218 #ifdef L2CAP_USES_CHANNELS 1219 static int l2cap_is_dynamic_channel_type(l2cap_channel_type_t channel_type){ 1220 switch (channel_type){ 1221 case L2CAP_CHANNEL_TYPE_CLASSIC: 1222 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 1223 return 1; 1224 default: 1225 return 0; 1226 } 1227 } 1228 #endif 1229 1230 #ifdef ENABLE_CLASSIC 1231 // RTX Timer only exist for dynamic channels 1232 static l2cap_channel_t * l2cap_channel_for_rtx_timer(btstack_timer_source_t * ts){ 1233 btstack_linked_list_iterator_t it; 1234 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1235 while (btstack_linked_list_iterator_has_next(&it)){ 1236 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1237 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 1238 if (&channel->rtx == ts) { 1239 return channel; 1240 } 1241 } 1242 return NULL; 1243 } 1244 1245 static void l2cap_rtx_timeout(btstack_timer_source_t * ts){ 1246 l2cap_channel_t * channel = l2cap_channel_for_rtx_timer(ts); 1247 if (!channel) return; 1248 1249 log_info("l2cap_rtx_timeout for local cid 0x%02x", channel->local_cid); 1250 1251 // "When terminating the channel, it is not necessary to send a L2CAP_DisconnectReq 1252 // and enter WAIT_DISCONNECT state. Channels can be transitioned directly to the CLOSED state." 1253 // notify client 1254 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_RTX_TIMEOUT); 1255 1256 // discard channel 1257 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1258 l2cap_free_channel_entry(channel); 1259 } 1260 1261 #endif 1262 1263 #ifdef L2CAP_USES_CHANNELS 1264 static void l2cap_stop_rtx(l2cap_channel_t * channel){ 1265 log_info("l2cap_stop_rtx for local cid 0x%02x", channel->local_cid); 1266 btstack_run_loop_remove_timer(&channel->rtx); 1267 } 1268 #endif 1269 1270 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){ 1271 if (!hci_can_send_acl_packet_now(handle)){ 1272 log_info("l2cap_send_classic_signaling_packet, cannot send"); 1273 return BTSTACK_ACL_BUFFERS_FULL; 1274 } 1275 hci_reserve_packet_buffer(); 1276 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1277 uint16_t len = l2cap_create_signaling_packet(acl_buffer, handle, pb_flags, cid, cmd, identifier, argptr); 1278 va_end(argptr); 1279 return hci_send_acl_packet_buffer(len); 1280 } 1281 1282 #ifdef ENABLE_CLASSIC 1283 1284 static void l2cap_start_rtx(l2cap_channel_t * channel){ 1285 l2cap_stop_rtx(channel); 1286 log_info("l2cap_start_rtx for local cid 0x%02x", channel->local_cid); 1287 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 1288 btstack_run_loop_set_timer(&channel->rtx, L2CAP_RTX_TIMEOUT_MS); 1289 btstack_run_loop_add_timer(&channel->rtx); 1290 } 1291 1292 static void l2cap_start_ertx(l2cap_channel_t * channel){ 1293 log_info("l2cap_start_ertx for local cid 0x%02x", channel->local_cid); 1294 l2cap_stop_rtx(channel); 1295 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 1296 btstack_run_loop_set_timer(&channel->rtx, L2CAP_ERTX_TIMEOUT_MS); 1297 btstack_run_loop_add_timer(&channel->rtx); 1298 } 1299 1300 void l2cap_require_security_level_2_for_outgoing_sdp(void){ 1301 l2cap_require_security_level2_for_outgoing_sdp = 1; 1302 } 1303 1304 static int l2cap_security_level_0_allowed_for_PSM(uint16_t psm){ 1305 return (psm == BLUETOOTH_PSM_SDP) && (!l2cap_require_security_level2_for_outgoing_sdp); 1306 } 1307 1308 static int l2cap_send_classic_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, int identifier, ...){ 1309 va_list argptr; 1310 va_start(argptr, identifier); 1311 uint8_t pb_flags = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 1312 uint8_t result = l2cap_send_signaling_packet(handle, pb_flags, L2CAP_CID_SIGNALING, cmd, identifier, argptr); 1313 va_end(argptr); 1314 return result; 1315 } 1316 1317 // assumption - only on Classic connections 1318 // cannot be used for L2CAP ERTM 1319 uint8_t l2cap_send_prepared(uint16_t local_cid, uint16_t len){ 1320 1321 if (!hci_is_packet_buffer_reserved()){ 1322 log_error("l2cap_send_prepared called without reserving packet first"); 1323 return BTSTACK_ACL_BUFFERS_FULL; 1324 } 1325 1326 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1327 if (!channel) { 1328 log_error("l2cap_send_prepared no channel for cid 0x%02x", local_cid); 1329 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 1330 } 1331 1332 if (!hci_can_send_prepared_acl_packet_now(channel->con_handle)){ 1333 log_info("l2cap_send_prepared cid 0x%02x, cannot send", local_cid); 1334 return BTSTACK_ACL_BUFFERS_FULL; 1335 } 1336 1337 log_debug("l2cap_send_prepared cid 0x%02x, handle %u, 1 credit used", local_cid, channel->con_handle); 1338 1339 int fcs_size = 0; 1340 1341 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1342 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->fcs_option){ 1343 fcs_size = 2; 1344 } 1345 #endif 1346 1347 // set non-flushable packet boundary flag if supported on Controller 1348 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1349 uint8_t packet_boundary_flag = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 1350 l2cap_setup_header(acl_buffer, channel->con_handle, packet_boundary_flag, channel->remote_cid, len + fcs_size); 1351 1352 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1353 if (fcs_size){ 1354 // calculate FCS over l2cap data 1355 uint16_t fcs = crc16_calc(acl_buffer + 4, 4 + len); 1356 log_info("I-Frame: fcs 0x%04x", fcs); 1357 little_endian_store_16(acl_buffer, 8 + len, fcs); 1358 } 1359 #endif 1360 1361 // send 1362 return hci_send_acl_packet_buffer(len+8+fcs_size); 1363 } 1364 1365 // assumption - only on Classic connections 1366 uint8_t l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){ 1367 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1368 if (!channel) { 1369 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 1370 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 1371 } 1372 1373 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1374 // send in ERTM 1375 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1376 return l2cap_ertm_send(channel, data, len); 1377 } 1378 #endif 1379 1380 if (len > channel->remote_mtu){ 1381 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 1382 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 1383 } 1384 1385 if (!hci_can_send_acl_packet_now(channel->con_handle)){ 1386 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 1387 return BTSTACK_ACL_BUFFERS_FULL; 1388 } 1389 1390 hci_reserve_packet_buffer(); 1391 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1392 (void)memcpy(&acl_buffer[8], data, len); 1393 return l2cap_send_prepared(local_cid, len); 1394 } 1395 1396 int l2cap_send_echo_request(hci_con_handle_t con_handle, uint8_t *data, uint16_t len){ 1397 return l2cap_send_classic_signaling_packet(con_handle, ECHO_REQUEST, l2cap_next_sig_id(), len, data); 1398 } 1399 1400 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, uint16_t flag){ 1401 channel->state_var = channel->state_var | flag; 1402 } 1403 1404 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, uint16_t flag){ 1405 channel->state_var = channel->state_var & ~flag; 1406 } 1407 #endif 1408 1409 1410 #ifdef ENABLE_BLE 1411 static int l2cap_send_le_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, int identifier, ...){ 1412 va_list argptr; 1413 va_start(argptr, identifier); 1414 uint8_t pb_flags = 0x00; // First non-automatically-flushable packet of a higher layer message 1415 uint8_t result = l2cap_send_signaling_packet(handle, pb_flags, L2CAP_CID_SIGNALING, cmd, identifier, argptr); 1416 va_end(argptr); 1417 return result; 1418 } 1419 #endif 1420 1421 uint16_t l2cap_max_mtu(void){ 1422 return HCI_ACL_PAYLOAD_SIZE - L2CAP_HEADER_SIZE; 1423 } 1424 1425 #ifdef ENABLE_BLE 1426 uint16_t l2cap_max_le_mtu(void){ 1427 if (l2cap_le_custom_max_mtu != 0u) return l2cap_le_custom_max_mtu; 1428 return l2cap_max_mtu(); 1429 } 1430 1431 void l2cap_set_max_le_mtu(uint16_t max_mtu){ 1432 if (max_mtu < l2cap_max_mtu()){ 1433 l2cap_le_custom_max_mtu = max_mtu; 1434 } 1435 } 1436 #endif 1437 1438 #ifdef ENABLE_CLASSIC 1439 1440 static uint16_t l2cap_setup_options_mtu(uint8_t * config_options, uint16_t mtu){ 1441 config_options[0] = L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT; // MTU 1442 config_options[1] = 2; // len param 1443 little_endian_store_16(config_options, 2, mtu); 1444 return 4; 1445 } 1446 1447 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1448 static int l2cap_ertm_mode(l2cap_channel_t * channel){ 1449 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1450 return ((connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_DONE) 1451 && (connection->l2cap_state.extended_feature_mask & 0x08)); 1452 } 1453 #endif 1454 1455 static uint16_t l2cap_setup_options_request(l2cap_channel_t * channel, uint8_t * config_options){ 1456 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1457 // use ERTM options if supported by remote and channel ready to use it 1458 if (l2cap_ertm_mode(channel) && channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1459 return l2cap_setup_options_ertm_request(channel, config_options); 1460 } 1461 #endif 1462 uint16_t mtu = channel->local_mtu; 1463 return l2cap_setup_options_mtu(config_options, mtu); 1464 } 1465 1466 static uint16_t l2cap_setup_options_mtu_response(l2cap_channel_t * channel, uint8_t * config_options){ 1467 uint16_t mtu = btstack_min(channel->local_mtu, channel->remote_mtu); 1468 return l2cap_setup_options_mtu(config_options, mtu); 1469 } 1470 1471 static uint32_t l2cap_extended_features_mask(void){ 1472 // extended features request supported, features: fixed channels, unicast connectionless data reception 1473 uint32_t features = 0x280; 1474 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1475 features |= 0x0028; 1476 #endif 1477 return features; 1478 } 1479 #endif 1480 1481 // 1482 #ifdef ENABLE_CLASSIC 1483 1484 // returns true if channel was finalized 1485 static bool l2cap_run_for_classic_channel(l2cap_channel_t * channel){ 1486 1487 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1488 uint8_t config_options[18]; 1489 #else 1490 uint8_t config_options[10]; 1491 #endif 1492 1493 switch (channel->state){ 1494 1495 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1496 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 1497 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1498 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 1499 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 1500 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONN_RESP_PEND); 1501 l2cap_send_classic_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, 1502 channel->local_cid, channel->remote_cid, 1, 0); 1503 } 1504 break; 1505 1506 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1507 if (!hci_can_send_command_packet_now()) break; 1508 // send connection request - set state first 1509 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 1510 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 1511 (void)memcpy(l2cap_outgoing_classic_addr, channel->address, 6); 1512 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, hci_get_allow_role_switch()); 1513 break; 1514 1515 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 1516 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1517 channel->state = L2CAP_STATE_INVALID; 1518 l2cap_send_classic_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, 1519 channel->local_cid, channel->remote_cid, channel->reason, 0); 1520 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1521 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1522 l2cap_free_channel_entry(channel); 1523 channel = NULL; 1524 break; 1525 1526 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 1527 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1528 channel->state = L2CAP_STATE_CONFIG; 1529 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1530 l2cap_send_classic_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, 1531 channel->local_cid, channel->remote_cid, 0, 0); 1532 break; 1533 1534 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 1535 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1536 // success, start l2cap handshake 1537 channel->local_sig_id = l2cap_next_sig_id(); 1538 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 1539 l2cap_send_classic_signaling_packet(channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, 1540 channel->psm, channel->local_cid); 1541 l2cap_start_rtx(channel); 1542 break; 1543 1544 case L2CAP_STATE_CONFIG: 1545 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1546 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1547 // fallback to basic mode if ERTM requested but not not supported by remote 1548 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1549 if (!l2cap_ertm_mode(channel)){ 1550 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 1551 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1552 } 1553 } 1554 #endif 1555 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 1556 uint16_t flags = 0; 1557 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1558 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 1559 flags = 1; 1560 } else { 1561 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1562 } 1563 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 1564 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1565 l2cap_send_classic_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, 1566 channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 1567 1, &channel->unknown_option); 1568 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1569 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 1570 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1571 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1572 uint16_t options_size = l2cap_setup_options_ertm_response(channel, config_options); 1573 l2cap_send_classic_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, 1574 channel->remote_cid, flags, 1575 L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS, options_size, 1576 &config_options); 1577 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM){ 1578 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 1579 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1580 uint16_t options_size = l2cap_setup_options_ertm_response(channel, config_options); 1581 l2cap_send_classic_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, 1582 channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 1583 options_size, &config_options); 1584 #endif 1585 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 1586 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1587 uint16_t options_size = l2cap_setup_options_mtu_response(channel, config_options); 1588 l2cap_send_classic_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, 1589 channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 1590 options_size, &config_options); 1591 } else { 1592 l2cap_send_classic_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, 1593 channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 1594 } 1595 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1596 } 1597 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 1598 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1599 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 1600 channel->local_sig_id = l2cap_next_sig_id(); 1601 uint16_t options_size = l2cap_setup_options_request(channel, config_options); 1602 l2cap_send_classic_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, 1603 channel->remote_cid, 0, options_size, &config_options); 1604 l2cap_start_rtx(channel); 1605 } 1606 if (l2cap_channel_ready_for_open(channel)){ 1607 channel->state = L2CAP_STATE_OPEN; 1608 l2cap_emit_channel_opened(channel, 0); // success 1609 } 1610 break; 1611 1612 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1613 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1614 channel->state = L2CAP_STATE_INVALID; 1615 l2cap_send_classic_signaling_packet(channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, 1616 channel->local_cid, channel->remote_cid); 1617 // 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 :) 1618 l2cap_finialize_channel_close(channel); // -- remove from list 1619 channel = NULL; 1620 break; 1621 1622 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1623 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1624 channel->local_sig_id = l2cap_next_sig_id(); 1625 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1626 l2cap_send_classic_signaling_packet(channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, 1627 channel->remote_cid, channel->local_cid); 1628 break; 1629 default: 1630 break; 1631 } 1632 1633 // handle channel finalize on L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE and L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE 1634 return channel == NULL; 1635 } 1636 1637 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1638 static void l2cap_run_for_classic_channel_ertm(l2cap_channel_t * channel){ 1639 1640 // ERTM mode 1641 if (channel->mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) return; 1642 1643 // check if we can still send 1644 if (channel->con_handle == HCI_CON_HANDLE_INVALID) return; 1645 if (!hci_can_send_acl_packet_now(channel->con_handle)) return; 1646 1647 if (channel->send_supervisor_frame_receiver_ready){ 1648 channel->send_supervisor_frame_receiver_ready = 0; 1649 log_info("Send S-Frame: RR %u, final %u", channel->req_seq, channel->set_final_bit_after_packet_with_poll_bit_set); 1650 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); 1651 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1652 l2cap_ertm_send_supervisor_frame(channel, control); 1653 return; 1654 } 1655 if (channel->send_supervisor_frame_receiver_ready_poll){ 1656 channel->send_supervisor_frame_receiver_ready_poll = 0; 1657 log_info("Send S-Frame: RR %u with poll=1 ", channel->req_seq); 1658 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 1, 0, channel->req_seq); 1659 l2cap_ertm_send_supervisor_frame(channel, control); 1660 return; 1661 } 1662 if (channel->send_supervisor_frame_receiver_not_ready){ 1663 channel->send_supervisor_frame_receiver_not_ready = 0; 1664 log_info("Send S-Frame: RNR %u", channel->req_seq); 1665 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 1666 l2cap_ertm_send_supervisor_frame(channel, control); 1667 return; 1668 } 1669 if (channel->send_supervisor_frame_reject){ 1670 channel->send_supervisor_frame_reject = 0; 1671 log_info("Send S-Frame: REJ %u", channel->req_seq); 1672 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT, 0, 0, channel->req_seq); 1673 l2cap_ertm_send_supervisor_frame(channel, control); 1674 return; 1675 } 1676 if (channel->send_supervisor_frame_selective_reject){ 1677 channel->send_supervisor_frame_selective_reject = 0; 1678 log_info("Send S-Frame: SREJ %u", channel->expected_tx_seq); 1679 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); 1680 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1681 l2cap_ertm_send_supervisor_frame(channel, control); 1682 return; 1683 } 1684 1685 if (channel->srej_active){ 1686 int i; 1687 for (i=0;i<channel->num_tx_buffers;i++){ 1688 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[i]; 1689 if (tx_state->retransmission_requested) { 1690 tx_state->retransmission_requested = 0; 1691 uint8_t final = channel->set_final_bit_after_packet_with_poll_bit_set; 1692 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1693 l2cap_ertm_send_information_frame(channel, i, final); 1694 break; 1695 } 1696 } 1697 if (i == channel->num_tx_buffers){ 1698 // no retransmission request found 1699 channel->srej_active = 0; 1700 } else { 1701 // packet was sent 1702 return; 1703 } 1704 } 1705 } 1706 #endif /* ERTM */ 1707 #endif /* Classic */ 1708 1709 static void l2cap_run_signaling_response(void) { 1710 1711 // check pending signaling responses 1712 while (l2cap_signaling_responses_pending){ 1713 1714 hci_con_handle_t handle = l2cap_signaling_responses[0].handle; 1715 1716 if (!hci_can_send_acl_packet_now(handle)) break; 1717 1718 uint8_t sig_id = l2cap_signaling_responses[0].sig_id; 1719 uint8_t response_code = l2cap_signaling_responses[0].code; 1720 uint16_t result = l2cap_signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT, REJECT_SM_PAIRING 1721 uint8_t buffer[2]; // REJECT_SM_PAIRING 1722 #ifdef ENABLE_CLASSIC 1723 uint16_t info_type = l2cap_signaling_responses[0].data; // INFORMATION_REQUEST 1724 #endif 1725 uint16_t source_cid = l2cap_signaling_responses[0].cid; // CONNECTION_REQUEST, REJECT_SM_PAIRING 1726 1727 // remove first item before sending (to avoid sending response mutliple times) 1728 l2cap_signaling_responses_pending--; 1729 int i; 1730 for (i=0; i < l2cap_signaling_responses_pending; i++){ 1731 (void)memcpy(&l2cap_signaling_responses[i], 1732 &l2cap_signaling_responses[i + 1], 1733 sizeof(l2cap_signaling_response_t)); 1734 } 1735 1736 switch (response_code){ 1737 #ifdef ENABLE_CLASSIC 1738 case CONNECTION_REQUEST: 1739 l2cap_send_classic_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 1740 break; 1741 case ECHO_REQUEST: 1742 l2cap_send_classic_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 1743 break; 1744 case INFORMATION_REQUEST: 1745 switch (info_type){ 1746 case L2CAP_INFO_TYPE_CONNECTIONLESS_MTU: { 1747 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 1748 l2cap_send_classic_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, 1749 sizeof(connectionless_mtu), &connectionless_mtu); 1750 } 1751 break; 1752 case L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED: { 1753 uint32_t features = l2cap_extended_features_mask(); 1754 l2cap_send_classic_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, 1755 sizeof(features), &features); 1756 } 1757 break; 1758 case L2CAP_INFO_TYPE_FIXED_CHANNELS_SUPPORTED: { 1759 uint8_t map[8]; 1760 memset(map, 0, 8); 1761 // L2CAP Signaling Channel (bit 1) + Connectionless reception (bit 2) 1762 map[0] = (1 << 1) | (1 << 2); 1763 #if defined(ENABLE_BLE) || defined (ENABLE_EXPLICIT_BR_EDR_SECURITY_MANAGER) 1764 // BR/EDR Security Manager (bit 7) 1765 map[0] |= (1 << 7); 1766 #endif 1767 l2cap_send_classic_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, 1768 sizeof(map), &map); 1769 } 1770 break; 1771 default: 1772 // all other types are not supported 1773 l2cap_send_classic_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 1, 0, NULL); 1774 break; 1775 } 1776 break; 1777 case COMMAND_REJECT: 1778 l2cap_send_classic_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1779 break; 1780 #endif 1781 #ifdef ENABLE_BLE 1782 case LE_CREDIT_BASED_CONNECTION_REQUEST: 1783 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 1784 break; 1785 case COMMAND_REJECT_LE: 1786 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1787 break; 1788 #endif 1789 case SM_PAIRING_FAILED: 1790 buffer[0] = SM_CODE_PAIRING_FAILED; 1791 buffer[1] = result; 1792 l2cap_send_connectionless(handle, source_cid, buffer, 2); 1793 break; 1794 default: 1795 // should not happen 1796 break; 1797 } 1798 } 1799 } 1800 1801 static bool l2ap_run_information_requests(void){ 1802 // send l2cap information request if requested 1803 btstack_linked_list_iterator_t it; 1804 hci_connections_get_iterator(&it); 1805 while(btstack_linked_list_iterator_has_next(&it)){ 1806 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1807 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST){ 1808 if (!hci_can_send_acl_packet_now(connection->con_handle)) break; 1809 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE; 1810 uint8_t sig_id = l2cap_next_sig_id(); 1811 uint8_t info_type = L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED; 1812 l2cap_send_classic_signaling_packet(connection->con_handle, INFORMATION_REQUEST, sig_id, info_type); 1813 return true; 1814 } 1815 } 1816 return false; 1817 } 1818 1819 #ifdef ENABLE_LE_DATA_CHANNELS 1820 static void l2cap_run_le_data_channels(void){ 1821 btstack_linked_list_iterator_t it; 1822 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1823 while (btstack_linked_list_iterator_has_next(&it)){ 1824 uint16_t mps; 1825 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1826 1827 if (channel->channel_type != L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL) continue; 1828 1829 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1830 switch (channel->state){ 1831 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1832 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1833 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1834 // le psm, source cid, mtu, mps, initial credits 1835 channel->local_sig_id = l2cap_next_sig_id(); 1836 channel->credits_incoming = channel->new_credits_incoming; 1837 channel->new_credits_incoming = 0; 1838 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1839 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); 1840 break; 1841 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1842 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1843 // TODO: support larger MPS 1844 channel->state = L2CAP_STATE_OPEN; 1845 channel->credits_incoming = channel->new_credits_incoming; 1846 channel->new_credits_incoming = 0; 1847 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1848 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); 1849 // notify client 1850 l2cap_emit_le_channel_opened(channel, 0); 1851 break; 1852 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1853 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1854 channel->state = L2CAP_STATE_INVALID; 1855 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1856 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1857 btstack_linked_list_iterator_remove(&it); 1858 l2cap_free_channel_entry(channel); 1859 break; 1860 case L2CAP_STATE_OPEN: 1861 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1862 1863 // send credits 1864 if (channel->new_credits_incoming){ 1865 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1866 channel->local_sig_id = l2cap_next_sig_id(); 1867 uint16_t new_credits = channel->new_credits_incoming; 1868 channel->new_credits_incoming = 0; 1869 channel->credits_incoming += new_credits; 1870 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1871 } 1872 break; 1873 1874 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1875 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1876 channel->local_sig_id = l2cap_next_sig_id(); 1877 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1878 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1879 break; 1880 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1881 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1882 channel->state = L2CAP_STATE_INVALID; 1883 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1884 l2cap_le_finialize_channel_close(channel); // -- remove from list 1885 break; 1886 default: 1887 break; 1888 } 1889 } 1890 } 1891 #endif 1892 1893 // MARK: L2CAP_RUN 1894 // process outstanding signaling tasks 1895 static void l2cap_run(void){ 1896 1897 // log_info("l2cap_run: entered"); 1898 l2cap_run_signaling_response(); 1899 1900 bool done = l2ap_run_information_requests(); 1901 if (done) return; 1902 1903 #if defined(ENABLE_CLASSIC) || defined(ENABLE_BLE) 1904 btstack_linked_list_iterator_t it; 1905 #endif 1906 1907 #ifdef ENABLE_CLASSIC 1908 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1909 while (btstack_linked_list_iterator_has_next(&it)){ 1910 1911 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1912 1913 if (channel->channel_type != L2CAP_CHANNEL_TYPE_CLASSIC) continue; 1914 1915 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1916 bool finalized = l2cap_run_for_classic_channel(channel); 1917 1918 if (!finalized) { 1919 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1920 l2cap_run_for_classic_channel_ertm(channel); 1921 #endif 1922 } 1923 } 1924 #endif 1925 1926 #ifdef ENABLE_LE_DATA_CHANNELS 1927 l2cap_run_le_data_channels(); 1928 #endif 1929 1930 #ifdef ENABLE_BLE 1931 // send l2cap con paramter update if necessary 1932 hci_connections_get_iterator(&it); 1933 while(btstack_linked_list_iterator_has_next(&it)){ 1934 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1935 if ((connection->address_type != BD_ADDR_TYPE_LE_PUBLIC) && (connection->address_type != BD_ADDR_TYPE_LE_RANDOM)) continue; 1936 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1937 switch (connection->le_con_parameter_update_state){ 1938 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1939 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1940 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, l2cap_next_sig_id(), 1941 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1942 break; 1943 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1944 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1945 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1946 break; 1947 case CON_PARAMETER_UPDATE_DENY: 1948 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1949 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1950 break; 1951 default: 1952 break; 1953 } 1954 } 1955 #endif 1956 1957 // log_info("l2cap_run: exit"); 1958 } 1959 1960 #ifdef ENABLE_CLASSIC 1961 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1962 1963 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1964 // assumption: outgoing connection: trigger information request if not already started and wait for done 1965 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1966 switch (connection->l2cap_state.information_state){ 1967 case L2CAP_INFORMATION_STATE_DONE: 1968 break; 1969 case L2CAP_INFORMATION_STATE_IDLE: 1970 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1971 /* fall through */ 1972 default: 1973 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1974 return; 1975 } 1976 #endif 1977 1978 // fine, go ahead 1979 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1980 } 1981 1982 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1983 if ((channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE) || (channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION)) { 1984 log_info("connection complete con_handle %04x - for channel %p cid 0x%04x", (int) con_handle, channel, channel->local_cid); 1985 channel->con_handle = con_handle; 1986 // query remote features if pairing is required 1987 if (channel->required_security_level > LEVEL_0){ 1988 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1989 hci_remote_features_query(con_handle); 1990 } else { 1991 l2cap_ready_to_connect(channel); 1992 } 1993 } 1994 } 1995 1996 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1997 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1998 // double check if all feature pages are complete 1999 2000 bool security_required = channel->required_security_level > LEVEL_0; 2001 2002 // abort if Secure Connections Only Mode with legacy connection 2003 if (security_required && gap_get_secure_connections_only_mode() && gap_secure_connection(channel->con_handle) == 0){ 2004 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY); 2005 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2006 l2cap_free_channel_entry(channel); 2007 return; 2008 } 2009 2010 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) != 0){ 2011 2012 // Core V5.2, Vol 3, Part C, 5.2.2.2 - Security Mode 4 2013 // When a remote device attempts to access a service offered by a Bluetooth device that is in security mode 4 2014 // and a sufficient link key exists and authentication has not been performed the local device shall authenticate 2015 // the remote device and enable encryption after the channel establishment request is received but before a channel 2016 // establishment confirmation (L2CAP_ConnectRsp with result code of 0x0000 or a higher-level channel establishment 2017 // confirmation such as that of RFCOMM) is sent. 2018 2019 // If the remote device has indicated support for Secure Simple Pairing, a channel establishment request is 2020 // received for a service other than SDP, and encryption has not yet been enabled, then the local device shall 2021 // disconnect the ACL link with error code 0x05 - Authentication Failure. 2022 2023 // => Disconnect if l2cap request received in mode 4 and ssp supported, non-sdp psm, not encrypted, no link key available 2024 if ((gap_get_security_mode() == GAP_SECURITY_MODE_4) 2025 && gap_ssp_supported_on_both_sides(channel->con_handle) 2026 && (channel->psm != PSM_SDP) 2027 && (gap_encryption_key_size(channel->con_handle) == 0) 2028 && (gap_bonded(channel->con_handle) == false)){ 2029 hci_disconnect_security_block(channel->con_handle); 2030 return; 2031 } 2032 2033 // incoming: assert security requirements 2034 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 2035 if (channel->required_security_level <= gap_security_level(channel->con_handle)){ 2036 l2cap_handle_security_level_incoming_sufficient(channel); 2037 } else { 2038 // send connection pending if not already done 2039 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONN_RESP_PEND) == 0){ 2040 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND; 2041 } 2042 gap_request_security_level(channel->con_handle, channel->required_security_level); 2043 } 2044 } else { 2045 // outgoing: we have been waiting for remote supported features 2046 if (security_required){ 2047 // request security level 2048 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 2049 gap_request_security_level(channel->con_handle, channel->required_security_level); 2050 } else { 2051 l2cap_ready_to_connect(channel); 2052 } 2053 } 2054 } 2055 #endif 2056 2057 #ifdef L2CAP_USES_CHANNELS 2058 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, 2059 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 2060 2061 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 2062 if (!channel) { 2063 return NULL; 2064 } 2065 2066 // fill in 2067 channel->packet_handler = packet_handler; 2068 channel->channel_type = channel_type; 2069 bd_addr_copy(channel->address, address); 2070 channel->address_type = address_type; 2071 channel->psm = psm; 2072 channel->local_mtu = local_mtu; 2073 channel->remote_mtu = L2CAP_DEFAULT_MTU; 2074 channel->required_security_level = security_level; 2075 2076 // 2077 channel->local_cid = l2cap_next_local_cid(); 2078 channel->con_handle = HCI_CON_HANDLE_INVALID; 2079 2080 // set initial state 2081 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 2082 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 2083 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 2084 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 2085 2086 log_info("create channel %p, local_cid 0x%04x", channel, channel->local_cid); 2087 2088 return channel; 2089 } 2090 2091 static void l2cap_free_channel_entry(l2cap_channel_t * channel){ 2092 log_info("free channel %p, local_cid 0x%04x", channel, channel->local_cid); 2093 // assert all timers are stopped 2094 l2cap_stop_rtx(channel); 2095 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2096 l2cap_ertm_stop_retransmission_timer(channel); 2097 l2cap_ertm_stop_monitor_timer(channel); 2098 #endif 2099 // free memory 2100 btstack_memory_l2cap_channel_free(channel); 2101 } 2102 #endif 2103 2104 #ifdef ENABLE_CLASSIC 2105 2106 /** 2107 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 2108 * @param packet_handler 2109 * @param address 2110 * @param psm 2111 * @param mtu 2112 * @param local_cid 2113 */ 2114 2115 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){ 2116 // limit MTU to the size of our outgoing HCI buffer 2117 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 2118 2119 // determine security level based on psm 2120 const gap_security_level_t security_level = l2cap_security_level_0_allowed_for_PSM(psm) ? LEVEL_0 : gap_get_security_level(); 2121 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); 2122 2123 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); 2124 if (!channel) { 2125 return BTSTACK_MEMORY_ALLOC_FAILED; 2126 } 2127 2128 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2129 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2130 #endif 2131 2132 // add to connections list 2133 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2134 2135 // store local_cid 2136 if (out_local_cid){ 2137 *out_local_cid = channel->local_cid; 2138 } 2139 2140 // state: L2CAP_STATE_WILL_SEND_CREATE_CONNECTION 2141 2142 // check if hci connection is already usable, 2143 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_ACL); 2144 if (conn && conn->state == OPEN){ 2145 // simulate connection complete 2146 l2cap_handle_connection_complete(conn->con_handle, channel); 2147 2148 // state: L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES or L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST 2149 2150 // simulate if remote supported features if requested and already received 2151 if ((channel->state == L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) && hci_remote_features_available(conn->con_handle)) { 2152 // simulate remote features received 2153 l2cap_handle_remote_supported_features_received(channel); 2154 } 2155 } 2156 2157 l2cap_run(); 2158 2159 return ERROR_CODE_SUCCESS; 2160 } 2161 2162 void l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 2163 log_info("disconnect local_cid 0x%x reason 0x%x", local_cid, reason); 2164 UNUSED(reason); 2165 // find channel for local_cid 2166 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2167 if (channel) { 2168 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2169 } 2170 // process 2171 l2cap_run(); 2172 } 2173 2174 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 2175 // mark all channels before emitting open events as these could trigger new connetion requests to the same device 2176 btstack_linked_list_iterator_t it; 2177 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2178 while (btstack_linked_list_iterator_has_next(&it)){ 2179 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2180 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2181 if (bd_addr_cmp( channel->address, address) != 0) continue; 2182 // channel for this address found 2183 switch (channel->state){ 2184 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2185 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2186 channel->state = L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD; 2187 break; 2188 default: 2189 break; 2190 } 2191 } 2192 // emit and free marked entries. restart loop to deal with list changes 2193 int done = 0; 2194 while (!done) { 2195 done = 1; 2196 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2197 while (btstack_linked_list_iterator_has_next(&it)){ 2198 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2199 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2200 if (channel->state == L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD){ 2201 done = 0; 2202 // failure, forward error code 2203 l2cap_handle_channel_open_failed(channel, status); 2204 // discard channel 2205 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2206 l2cap_free_channel_entry(channel); 2207 break; 2208 } 2209 } 2210 } 2211 2212 } 2213 2214 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 2215 btstack_linked_list_iterator_t it; 2216 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2217 while (btstack_linked_list_iterator_has_next(&it)){ 2218 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2219 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2220 if ( ! bd_addr_cmp( channel->address, address) ){ 2221 l2cap_handle_connection_complete(handle, channel); 2222 } 2223 } 2224 // process 2225 l2cap_run(); 2226 } 2227 #endif 2228 2229 static bool l2cap_channel_ready_to_send(l2cap_channel_t * channel){ 2230 switch (channel->channel_type){ 2231 #ifdef ENABLE_CLASSIC 2232 case L2CAP_CHANNEL_TYPE_CLASSIC: 2233 if (channel->state != L2CAP_STATE_OPEN) return false; 2234 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2235 // send if we have more data and remote windows isn't full yet 2236 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2237 if (channel->unacked_frames >= btstack_min(channel->num_stored_tx_frames, channel->remote_tx_window_size)) return false; 2238 return hci_can_send_acl_classic_packet_now() != 0; 2239 } 2240 #endif 2241 if (!channel->waiting_for_can_send_now) return false; 2242 return (hci_can_send_acl_classic_packet_now() != 0); 2243 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2244 if (!channel->waiting_for_can_send_now) return false; 2245 return hci_can_send_acl_classic_packet_now() != 0; 2246 #endif 2247 #ifdef ENABLE_BLE 2248 case L2CAP_CHANNEL_TYPE_FIXED: 2249 if (!channel->waiting_for_can_send_now) return false; 2250 return hci_can_send_acl_le_packet_now() != 0; 2251 #ifdef ENABLE_LE_DATA_CHANNELS 2252 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2253 if (channel->state != L2CAP_STATE_OPEN) return false; 2254 if (channel->send_sdu_buffer == NULL) return false; 2255 if (channel->credits_outgoing == 0u) return false; 2256 return hci_can_send_acl_le_packet_now() != 0; 2257 #endif 2258 #endif 2259 default: 2260 return false; 2261 } 2262 } 2263 2264 static void l2cap_channel_trigger_send(l2cap_channel_t * channel){ 2265 switch (channel->channel_type){ 2266 #ifdef ENABLE_CLASSIC 2267 case L2CAP_CHANNEL_TYPE_CLASSIC: 2268 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2269 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2270 l2cap_ertm_channel_send_information_frame(channel); 2271 return; 2272 } 2273 #endif 2274 channel->waiting_for_can_send_now = 0; 2275 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2276 break; 2277 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2278 channel->waiting_for_can_send_now = 0; 2279 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2280 break; 2281 #endif 2282 #ifdef ENABLE_BLE 2283 case L2CAP_CHANNEL_TYPE_FIXED: 2284 channel->waiting_for_can_send_now = 0; 2285 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2286 break; 2287 #ifdef ENABLE_LE_DATA_CHANNELS 2288 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2289 l2cap_le_send_pdu(channel); 2290 break; 2291 #endif 2292 #endif 2293 default: 2294 break; 2295 } 2296 } 2297 2298 static void l2cap_notify_channel_can_send(void){ 2299 bool done = false; 2300 while (!done){ 2301 done = true; 2302 btstack_linked_list_iterator_t it; 2303 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2304 while (btstack_linked_list_iterator_has_next(&it)){ 2305 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2306 bool ready = l2cap_channel_ready_to_send(channel); 2307 if (!ready) continue; 2308 2309 // requeue channel for fairness 2310 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2311 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2312 2313 // trigger sending 2314 l2cap_channel_trigger_send(channel); 2315 2316 // exit inner loop as we just broke the iterator, but try again 2317 done = false; 2318 break; 2319 } 2320 } 2321 } 2322 2323 #ifdef L2CAP_USES_CHANNELS 2324 2325 static int l2cap_send_open_failed_on_hci_disconnect(l2cap_channel_t * channel){ 2326 // open cannot fail for for incoming connections 2327 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) return 0; 2328 2329 // check state 2330 switch (channel->state){ 2331 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2332 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2333 case L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES: 2334 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2335 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 2336 case L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES: 2337 case L2CAP_STATE_WAIT_CONNECT_RSP: 2338 case L2CAP_STATE_CONFIG: 2339 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 2340 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 2341 case L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE: 2342 case L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD: 2343 return 1; 2344 2345 case L2CAP_STATE_OPEN: 2346 case L2CAP_STATE_CLOSED: 2347 case L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES: 2348 case L2CAP_STATE_WAIT_DISCONNECT: 2349 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_INSUFFICIENT_SECURITY: 2350 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 2351 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 2352 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2353 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 2354 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 2355 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 2356 case L2CAP_STATE_INVALID: 2357 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2358 return 0; 2359 2360 default: 2361 // get a "warning" about new states 2362 btstack_assert(false); 2363 return 0; 2364 } 2365 } 2366 #endif 2367 2368 #ifdef ENABLE_CLASSIC 2369 static void l2cap_handle_hci_disconnect_event(l2cap_channel_t * channel){ 2370 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2371 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2372 } else { 2373 l2cap_handle_channel_closed(channel); 2374 } 2375 l2cap_free_channel_entry(channel); 2376 } 2377 #endif 2378 2379 #ifdef ENABLE_LE_DATA_CHANNELS 2380 static void l2cap_handle_hci_le_disconnect_event(l2cap_channel_t * channel){ 2381 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2382 l2cap_emit_le_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2383 } else { 2384 l2cap_emit_le_channel_closed(channel); 2385 } 2386 l2cap_free_channel_entry(channel); 2387 } 2388 #endif 2389 2390 #ifdef ENABLE_CLASSIC 2391 static void l2cap_check_classic_timeout(hci_con_handle_t handle){ 2392 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) { 2393 return; 2394 } 2395 if (hci_authentication_active_for_handle(handle)) { 2396 return; 2397 } 2398 bool hci_con_used = false; 2399 btstack_linked_list_iterator_t it; 2400 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2401 while (btstack_linked_list_iterator_has_next(&it)){ 2402 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2403 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2404 if (channel->con_handle != handle) continue; 2405 hci_con_used = true; 2406 break; 2407 } 2408 if (hci_con_used) { 2409 return; 2410 } 2411 if (!hci_can_send_command_packet_now()) { 2412 return; 2413 } 2414 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 2415 } 2416 2417 static void l2cap_handle_features_complete(hci_con_handle_t handle){ 2418 if (hci_remote_features_available(handle) == false){ 2419 return; 2420 } 2421 btstack_linked_list_iterator_t it; 2422 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2423 while (btstack_linked_list_iterator_has_next(&it)){ 2424 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2425 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2426 if (channel->con_handle != handle) continue; 2427 log_info("remote supported features, channel %p, cid %04x - state %u", channel, channel->local_cid, channel->state); 2428 l2cap_handle_remote_supported_features_received(channel); 2429 } 2430 } 2431 2432 static void l2cap_handle_security_level_incoming_sufficient(l2cap_channel_t * channel){ 2433 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2434 // we need to know if ERTM is supported before sending a config response 2435 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 2436 switch (connection->l2cap_state.information_state){ 2437 case L2CAP_INFORMATION_STATE_DONE: 2438 break; 2439 case L2CAP_INFORMATION_STATE_IDLE: 2440 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2441 /* fall through */ 2442 default: 2443 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 2444 return; 2445 } 2446 #endif 2447 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2448 l2cap_emit_incoming_connection(channel); 2449 } 2450 2451 static void l2cap_handle_security_level(hci_con_handle_t handle, gap_security_level_t actual_level){ 2452 log_info("security level update for handle 0x%04x", handle); 2453 2454 // trigger l2cap information requests 2455 if (actual_level > LEVEL_0){ 2456 hci_connection_t * hci_connection = hci_connection_for_handle(handle); 2457 btstack_assert(hci_connection != NULL); 2458 if (hci_connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 2459 hci_connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2460 } 2461 } 2462 2463 btstack_linked_list_iterator_t it; 2464 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2465 while (btstack_linked_list_iterator_has_next(&it)){ 2466 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2467 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2468 if (channel->con_handle != handle) continue; 2469 2470 gap_security_level_t required_level = channel->required_security_level; 2471 2472 log_info("channel %p, cid %04x - state %u: actual %u >= required %u?", channel, channel->local_cid, channel->state, actual_level, required_level); 2473 2474 switch (channel->state){ 2475 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2476 if (actual_level >= required_level){ 2477 l2cap_handle_security_level_incoming_sufficient(channel); 2478 } else { 2479 channel->reason = 0x0003; // security block 2480 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2481 } 2482 break; 2483 2484 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2485 if (actual_level >= required_level){ 2486 l2cap_ready_to_connect(channel); 2487 } else { 2488 // security level insufficient, report error and free channel 2489 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY); 2490 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2491 l2cap_free_channel_entry(channel); 2492 } 2493 break; 2494 2495 default: 2496 break; 2497 } 2498 } 2499 } 2500 #endif 2501 2502 #ifdef L2CAP_USES_CHANNELS 2503 static void l2cap_handle_disconnection_complete(hci_con_handle_t handle){ 2504 // collect channels to close 2505 btstack_linked_list_t channels_to_close = NULL; 2506 btstack_linked_list_iterator_t it; 2507 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2508 while (btstack_linked_list_iterator_has_next(&it)) { 2509 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2510 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2511 if (channel->con_handle != handle) continue; 2512 btstack_linked_list_iterator_remove(&it); 2513 btstack_linked_list_add(&channels_to_close, (btstack_linked_item_t *) channel); 2514 } 2515 // send l2cap open failed or closed events for all channels on this handle and free them 2516 btstack_linked_list_iterator_init(&it, &channels_to_close); 2517 while (btstack_linked_list_iterator_has_next(&it)) { 2518 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2519 btstack_linked_list_iterator_remove(&it); 2520 switch(channel->channel_type){ 2521 #ifdef ENABLE_CLASSIC 2522 case L2CAP_CHANNEL_TYPE_CLASSIC: 2523 l2cap_handle_hci_disconnect_event(channel); 2524 break; 2525 #endif 2526 #ifdef ENABLE_LE_DATA_CHANNELS 2527 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2528 l2cap_handle_hci_le_disconnect_event(channel); 2529 break; 2530 #endif 2531 default: 2532 break; 2533 } 2534 } 2535 } 2536 #endif 2537 2538 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 2539 2540 UNUSED(packet_type); // ok: registered with hci_event_callback_registration 2541 UNUSED(cid); // ok: there is no channel 2542 UNUSED(size); // ok: fixed format events read from HCI buffer 2543 2544 #ifdef ENABLE_CLASSIC 2545 bd_addr_t address; 2546 gap_security_level_t security_level; 2547 #endif 2548 #ifdef L2CAP_USES_CHANNELS 2549 hci_con_handle_t handle; 2550 #endif 2551 2552 switch(hci_event_packet_get_type(packet)){ 2553 2554 // Notify channel packet handler if they can send now 2555 case HCI_EVENT_TRANSPORT_PACKET_SENT: 2556 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 2557 case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED: 2558 l2cap_run(); // try sending signaling packets first 2559 l2cap_notify_channel_can_send(); 2560 break; 2561 2562 case HCI_EVENT_COMMAND_STATUS: 2563 #ifdef ENABLE_CLASSIC 2564 // check command status for create connection for errors 2565 if (HCI_EVENT_IS_COMMAND_STATUS(packet, hci_create_connection)){ 2566 // cache outgoing address and reset 2567 (void)memcpy(address, l2cap_outgoing_classic_addr, 6); 2568 memset(l2cap_outgoing_classic_addr, 0, 6); 2569 // error => outgoing connection failed 2570 uint8_t status = hci_event_command_status_get_status(packet); 2571 if (status){ 2572 l2cap_handle_connection_failed_for_addr(address, status); 2573 } 2574 } 2575 #endif 2576 l2cap_run(); // try sending signaling packets first 2577 break; 2578 2579 #ifdef ENABLE_CLASSIC 2580 // handle connection complete events 2581 case HCI_EVENT_CONNECTION_COMPLETE: 2582 reverse_bd_addr(&packet[5], address); 2583 if (packet[2] == 0){ 2584 handle = little_endian_read_16(packet, 3); 2585 l2cap_handle_connection_success_for_addr(address, handle); 2586 } else { 2587 l2cap_handle_connection_failed_for_addr(address, packet[2]); 2588 } 2589 break; 2590 2591 // handle successful create connection cancel command 2592 case HCI_EVENT_COMMAND_COMPLETE: 2593 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 2594 if (packet[5] == 0){ 2595 reverse_bd_addr(&packet[6], address); 2596 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 2597 l2cap_handle_connection_failed_for_addr(address, 0x16); 2598 } 2599 } 2600 l2cap_run(); // try sending signaling packets first 2601 break; 2602 #endif 2603 2604 #ifdef L2CAP_USES_CHANNELS 2605 // handle disconnection complete events 2606 case HCI_EVENT_DISCONNECTION_COMPLETE: 2607 handle = little_endian_read_16(packet, 3); 2608 l2cap_handle_disconnection_complete(handle); 2609 break; 2610 #endif 2611 2612 // HCI Connection Timeouts 2613 #ifdef ENABLE_CLASSIC 2614 case L2CAP_EVENT_TIMEOUT_CHECK: 2615 handle = little_endian_read_16(packet, 2); 2616 l2cap_check_classic_timeout(handle); 2617 break; 2618 2619 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 2620 case HCI_EVENT_READ_REMOTE_EXTENDED_FEATURES_COMPLETE: 2621 handle = little_endian_read_16(packet, 3); 2622 l2cap_handle_features_complete(handle); 2623 break; 2624 2625 case GAP_EVENT_SECURITY_LEVEL: 2626 handle = little_endian_read_16(packet, 2); 2627 security_level = (gap_security_level_t) packet[4]; 2628 l2cap_handle_security_level(handle, security_level); 2629 break; 2630 #endif 2631 default: 2632 break; 2633 } 2634 2635 l2cap_run(); 2636 } 2637 2638 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 2639 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 2640 if (l2cap_signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 2641 l2cap_signaling_responses[l2cap_signaling_responses_pending].handle = handle; 2642 l2cap_signaling_responses[l2cap_signaling_responses_pending].code = code; 2643 l2cap_signaling_responses[l2cap_signaling_responses_pending].sig_id = sig_id; 2644 l2cap_signaling_responses[l2cap_signaling_responses_pending].cid = cid; 2645 l2cap_signaling_responses[l2cap_signaling_responses_pending].data = data; 2646 l2cap_signaling_responses_pending++; 2647 l2cap_run(); 2648 } 2649 } 2650 2651 #ifdef ENABLE_CLASSIC 2652 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 2653 switch (channel->state){ 2654 case L2CAP_STATE_CONFIG: 2655 case L2CAP_STATE_OPEN: 2656 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2657 case L2CAP_STATE_WAIT_DISCONNECT: 2658 break; 2659 default: 2660 // ignore in other states 2661 return; 2662 } 2663 2664 channel->remote_sig_id = identifier; 2665 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2666 l2cap_run(); 2667 } 2668 2669 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 2670 2671 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 2672 l2cap_service_t *service = l2cap_get_service(psm); 2673 if (!service) { 2674 // 0x0002 PSM not supported 2675 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2676 return; 2677 } 2678 2679 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 2680 if (!hci_connection) { 2681 // 2682 log_error("no hci_connection for handle %u", handle); 2683 return; 2684 } 2685 2686 // alloc structure 2687 gap_security_level_t required_level = service->required_security_level; 2688 if (gap_get_secure_connections_only_mode() && (required_level != LEVEL_0)){ 2689 required_level = LEVEL_4; 2690 } 2691 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CLASSIC, hci_connection->address, BD_ADDR_TYPE_ACL, 2692 psm, service->mtu, required_level); 2693 if (!channel){ 2694 // 0x0004 No resources available 2695 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2696 return; 2697 } 2698 2699 channel->con_handle = handle; 2700 channel->remote_cid = source_cid; 2701 channel->remote_sig_id = sig_id; 2702 2703 // limit local mtu to max acl packet length - l2cap header 2704 if (channel->local_mtu > l2cap_max_mtu()) { 2705 channel->local_mtu = l2cap_max_mtu(); 2706 } 2707 2708 // set initial state 2709 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 2710 channel->state_var = L2CAP_CHANNEL_STATE_VAR_INCOMING; 2711 2712 // add to connections list 2713 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2714 2715 // 2716 if (required_level > LEVEL_0){ 2717 // send conn resp pending if remote supported features have not been received yet 2718 if (hci_remote_features_available(handle)) { 2719 l2cap_handle_remote_supported_features_received(channel); 2720 } else { 2721 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND; 2722 hci_remote_features_query(handle); 2723 } 2724 } else { 2725 l2cap_handle_security_level_incoming_sufficient(channel); 2726 } 2727 } 2728 2729 void l2cap_accept_connection(uint16_t local_cid){ 2730 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 2731 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2732 if (!channel) { 2733 log_error("accept called but local_cid 0x%x not found", local_cid); 2734 return; 2735 } 2736 2737 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2738 // configure L2CAP Basic mode 2739 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2740 #endif 2741 2742 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 2743 2744 // process 2745 l2cap_run(); 2746 } 2747 2748 void l2cap_decline_connection(uint16_t local_cid){ 2749 log_info("decline local_cid 0x%x", local_cid); 2750 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 2751 if (!channel) { 2752 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 2753 return; 2754 } 2755 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2756 channel->reason = 0x04; // no resources available 2757 l2cap_run(); 2758 } 2759 2760 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2761 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 2762 2763 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2764 uint8_t use_fcs = 1; 2765 #endif 2766 2767 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2768 2769 uint16_t flags = little_endian_read_16(command, 6); 2770 if (flags & 1) { 2771 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 2772 } 2773 2774 // accept the other's configuration options 2775 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2776 uint16_t pos = 8; 2777 while (pos < end_pos){ 2778 uint8_t option_hint = command[pos] >> 7; 2779 uint8_t option_type = command[pos] & 0x7f; 2780 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2781 pos++; 2782 uint8_t length = command[pos++]; 2783 // MTU { type(8): 1, len(8):2, MTU(16) } 2784 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) && (length == 2)){ 2785 channel->remote_mtu = little_endian_read_16(command, pos); 2786 log_info("Remote MTU %u", channel->remote_mtu); 2787 if (channel->remote_mtu > l2cap_max_mtu()){ 2788 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 2789 channel->remote_mtu = l2cap_max_mtu(); 2790 } 2791 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2792 } 2793 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2794 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_FLUSH_TIMEOUT) && (length == 2)){ 2795 channel->flush_timeout = little_endian_read_16(command, pos); 2796 log_info("Flush timeout: %u ms", channel->flush_timeout); 2797 } 2798 2799 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2800 // Retransmission and Flow Control Option 2801 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2802 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2803 switch(channel->mode){ 2804 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2805 // Store remote config 2806 channel->remote_tx_window_size = command[pos+1]; 2807 channel->remote_max_transmit = command[pos+2]; 2808 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2809 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2810 channel->remote_mps = little_endian_read_16(command, pos + 7); 2811 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2812 channel->remote_tx_window_size, 2813 channel->remote_max_transmit, 2814 channel->remote_retransmission_timeout_ms, 2815 channel->remote_monitor_timeout_ms, 2816 channel->remote_mps); 2817 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2818 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2819 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2820 } else { 2821 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2822 } 2823 break; 2824 case L2CAP_CHANNEL_MODE_BASIC: 2825 switch (mode){ 2826 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2827 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2828 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2829 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2830 } 2831 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2832 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2833 break; 2834 default: // case L2CAP_CHANNEL_MODE_BASIC: 2835 // TODO store and evaluate configuration 2836 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2837 break; 2838 } 2839 break; 2840 default: 2841 break; 2842 } 2843 } 2844 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE && length == 1){ 2845 use_fcs = command[pos]; 2846 } 2847 #endif 2848 // check for unknown options 2849 if ((option_hint == 0) && ((option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) || (option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE))){ 2850 log_info("l2cap cid %u, unknown option 0x%02x", channel->local_cid, option_type); 2851 channel->unknown_option = option_type; 2852 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2853 } 2854 pos += length; 2855 } 2856 2857 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2858 // "FCS" has precedence over "No FCS" 2859 uint8_t update = channel->fcs_option || use_fcs; 2860 log_info("local fcs: %u, remote fcs: %u -> %u", channel->fcs_option, use_fcs, update); 2861 channel->fcs_option = update; 2862 // If ERTM mandatory, but remote didn't send Retransmission and Flowcontrol options -> disconnect 2863 if (((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM) == 0) & (channel->ertm_mandatory)){ 2864 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2865 } 2866 #endif 2867 } 2868 2869 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2870 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2871 log_info("l2cap_signaling_handle_configure_response"); 2872 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2873 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2874 uint16_t pos = 10; 2875 while (pos < end_pos){ 2876 uint8_t option_hint = command[pos] >> 7; 2877 uint8_t option_type = command[pos] & 0x7f; 2878 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2879 pos++; 2880 uint8_t length = command[pos++]; 2881 2882 // Retransmission and Flow Control Option 2883 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2884 switch (channel->mode){ 2885 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2886 if (channel->ertm_mandatory){ 2887 // ?? 2888 } else { 2889 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2890 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2891 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2892 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2893 } 2894 } 2895 break; 2896 case L2CAP_CHANNEL_MODE_BASIC: 2897 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2898 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2899 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2900 } 2901 break; 2902 default: 2903 break; 2904 } 2905 } 2906 2907 // check for unknown options 2908 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2909 log_info("l2cap cid %u, unknown option 0x%02x", channel->local_cid, option_type); 2910 channel->unknown_option = option_type; 2911 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2912 } 2913 2914 pos += length; 2915 } 2916 #else 2917 UNUSED(channel); // ok: no code 2918 UNUSED(result); // ok: no code 2919 UNUSED(command); // ok: no code 2920 #endif 2921 } 2922 2923 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2924 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2925 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2926 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2927 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2928 if (channel->state == L2CAP_STATE_OPEN) return 0; 2929 return 1; 2930 } 2931 2932 2933 // @pre command len is valid, see check in l2cap_signaling_handler_dispatch 2934 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2935 2936 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2937 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2938 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2939 uint16_t result = 0; 2940 2941 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2942 2943 // handle DISCONNECT REQUESTS seperately 2944 if (code == DISCONNECTION_REQUEST){ 2945 l2cap_handle_disconnect_request(channel, identifier); 2946 return; 2947 } 2948 2949 // @STATEMACHINE(l2cap) 2950 switch (channel->state) { 2951 2952 case L2CAP_STATE_WAIT_CONNECT_RSP: 2953 switch (code){ 2954 case CONNECTION_RESPONSE: 2955 if (cmd_len < 8){ 2956 // command imcomplete 2957 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2958 break; 2959 } 2960 l2cap_stop_rtx(channel); 2961 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2962 switch (result) { 2963 case 0: 2964 // successful connection 2965 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2966 channel->state = L2CAP_STATE_CONFIG; 2967 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2968 break; 2969 case 1: 2970 // connection pending. get some coffee, but start the ERTX 2971 l2cap_start_ertx(channel); 2972 break; 2973 default: 2974 // channel closed 2975 channel->state = L2CAP_STATE_CLOSED; 2976 // map l2cap connection response result to BTstack status enumeration 2977 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2978 2979 // drop link key if security block 2980 if ((L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result) == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2981 gap_drop_link_key_for_bd_addr(channel->address); 2982 } 2983 2984 // discard channel 2985 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2986 l2cap_free_channel_entry(channel); 2987 break; 2988 } 2989 break; 2990 2991 default: 2992 //@TODO: implement other signaling packets 2993 break; 2994 } 2995 break; 2996 2997 case L2CAP_STATE_CONFIG: 2998 switch (code) { 2999 case CONFIGURE_REQUEST: 3000 if (cmd_len < 4){ 3001 // command incomplete 3002 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 3003 break; 3004 } 3005 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 3006 l2cap_signaling_handle_configure_request(channel, command); 3007 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 3008 // only done if continuation not set 3009 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 3010 } 3011 break; 3012 case CONFIGURE_RESPONSE: 3013 if (cmd_len < 6){ 3014 // command incomplete 3015 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 3016 break; 3017 } 3018 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3019 l2cap_stop_rtx(channel); 3020 l2cap_signaling_handle_configure_response(channel, result, command); 3021 switch (result){ 3022 case 0: // success 3023 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 3024 break; 3025 case 4: // pending 3026 l2cap_start_ertx(channel); 3027 break; 3028 default: 3029 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3030 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 3031 // remote does not offer ertm but it's required 3032 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3033 break; 3034 } 3035 #endif 3036 // retry on negative result 3037 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 3038 break; 3039 } 3040 break; 3041 default: 3042 break; 3043 } 3044 if (l2cap_channel_ready_for_open(channel)){ 3045 3046 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3047 // assert that packet can be stored in fragment buffers in ertm 3048 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3049 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 3050 uint16_t usable_mtu = channel->num_tx_buffers == 1 ? effective_mps : channel->num_tx_buffers * effective_mps - 2; 3051 if (usable_mtu < channel->remote_mtu){ 3052 log_info("Remote MTU %u > max storable ERTM packet, only using MTU = %u", channel->remote_mtu, usable_mtu); 3053 channel->remote_mtu = usable_mtu; 3054 } 3055 } 3056 #endif 3057 // for open: 3058 channel->state = L2CAP_STATE_OPEN; 3059 l2cap_emit_channel_opened(channel, 0); 3060 } 3061 break; 3062 3063 case L2CAP_STATE_WAIT_DISCONNECT: 3064 switch (code) { 3065 case DISCONNECTION_RESPONSE: 3066 l2cap_finialize_channel_close(channel); 3067 break; 3068 default: 3069 //@TODO: implement other signaling packets 3070 break; 3071 } 3072 break; 3073 3074 case L2CAP_STATE_CLOSED: 3075 // @TODO handle incoming requests 3076 break; 3077 3078 case L2CAP_STATE_OPEN: 3079 //@TODO: implement other signaling packets, e.g. re-configure 3080 break; 3081 default: 3082 break; 3083 } 3084 // log_info("new state %u", channel->state); 3085 } 3086 3087 static void l2cap_handle_information_request_complete(hci_connection_t * connection){ 3088 3089 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 3090 3091 // trigger connection request 3092 btstack_linked_list_iterator_t it; 3093 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3094 while (btstack_linked_list_iterator_has_next(&it)){ 3095 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3096 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3097 if (channel->con_handle != connection->con_handle) continue; 3098 3099 // incoming connection: ask user for channel configuration, esp. if ertm will be mandatory 3100 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 3101 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3102 l2cap_emit_incoming_connection(channel); 3103 continue; 3104 } 3105 3106 // outgoing connection 3107 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 3108 3109 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3110 // if ERTM was requested, but is not listed in extended feature mask: 3111 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 3112 3113 if (channel->ertm_mandatory){ 3114 // bail if ERTM is mandatory 3115 channel->state = L2CAP_STATE_CLOSED; 3116 // map l2cap connection response result to BTstack status enumeration 3117 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 3118 // discard channel 3119 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3120 l2cap_free_channel_entry(channel); 3121 continue; 3122 3123 } else { 3124 // fallback to Basic mode 3125 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 3126 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 3127 } 3128 } 3129 #endif 3130 3131 // respond to connection request 3132 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 3133 continue; 3134 } 3135 } 3136 } 3137 3138 3139 // @pre command len is valid, see check in l2cap_acl_classic_handler 3140 static void l2cap_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command){ 3141 3142 hci_connection_t * connection; 3143 3144 // get code, signal identifier and command len 3145 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3146 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 3147 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3148 3149 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 3150 if ((code < 1) || (code == ECHO_RESPONSE) || (code > INFORMATION_RESPONSE)){ 3151 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3152 return; 3153 } 3154 3155 // general commands without an assigned channel 3156 switch(code) { 3157 3158 case CONNECTION_REQUEST: 3159 if (cmd_len == 4){ 3160 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3161 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3162 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 3163 } else { 3164 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3165 } 3166 return; 3167 3168 case ECHO_REQUEST: 3169 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 3170 return; 3171 3172 case INFORMATION_REQUEST: 3173 if (cmd_len == 2) { 3174 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3175 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 3176 } else { 3177 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3178 } 3179 return; 3180 3181 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3182 case INFORMATION_RESPONSE: 3183 connection = hci_connection_for_handle(handle); 3184 if (!connection) return; 3185 if (connection->l2cap_state.information_state != L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE) return; 3186 3187 // get extended features from response if valid 3188 connection->l2cap_state.extended_feature_mask = 0; 3189 if (cmd_len >= 6) { 3190 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3191 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3192 if (result == 0 && info_type == L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) { 3193 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3194 } 3195 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 3196 3197 } 3198 3199 // information request complete 3200 l2cap_handle_information_request_complete(connection); 3201 return; 3202 #endif 3203 3204 default: 3205 break; 3206 } 3207 3208 // Get potential destination CID 3209 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3210 3211 // Find channel for this sig_id and connection handle 3212 btstack_linked_list_iterator_t it; 3213 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3214 while (btstack_linked_list_iterator_has_next(&it)){ 3215 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3216 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3217 if (channel->con_handle != handle) continue; 3218 if (code & 1) { 3219 // match odd commands (responses) by previous signaling identifier 3220 if (channel->local_sig_id == sig_id) { 3221 l2cap_signaling_handler_channel(channel, command); 3222 break; 3223 } 3224 } else { 3225 // match even commands (requests) by local channel id 3226 if (channel->local_cid == dest_cid) { 3227 l2cap_signaling_handler_channel(channel, command); 3228 break; 3229 } 3230 } 3231 } 3232 } 3233 #endif 3234 3235 #ifdef ENABLE_BLE 3236 3237 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 3238 uint8_t event[6]; 3239 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 3240 event[1] = 4; 3241 little_endian_store_16(event, 2, con_handle); 3242 little_endian_store_16(event, 4, result); 3243 l2cap_emit_event(event, sizeof(event)); 3244 } 3245 3246 // @return valid 3247 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 3248 hci_connection_t * connection; 3249 uint16_t result; 3250 uint8_t event[12]; 3251 3252 #ifdef ENABLE_LE_DATA_CHANNELS 3253 btstack_linked_list_iterator_t it; 3254 l2cap_channel_t * channel; 3255 uint16_t local_cid; 3256 uint16_t le_psm; 3257 uint16_t new_credits; 3258 uint16_t credits_before; 3259 l2cap_service_t * service; 3260 uint16_t source_cid; 3261 #endif 3262 3263 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3264 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3265 log_info("le dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 3266 3267 switch (code){ 3268 3269 case CONNECTION_PARAMETER_UPDATE_REQUEST: 3270 // check size 3271 if (len < 8u) return 0u; 3272 connection = hci_connection_for_handle(handle); 3273 if (connection != NULL){ 3274 if (connection->role != HCI_ROLE_MASTER){ 3275 // reject command without notifying upper layer when not in master role 3276 return 0; 3277 } 3278 le_connection_parameter_range_t existing_range; 3279 gap_get_connection_parameter_range(&existing_range); 3280 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3281 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3282 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3283 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 3284 3285 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 3286 if (update_parameter){ 3287 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 3288 connection->le_conn_interval_min = le_conn_interval_min; 3289 connection->le_conn_interval_max = le_conn_interval_max; 3290 connection->le_conn_latency = le_conn_latency; 3291 connection->le_supervision_timeout = le_supervision_timeout; 3292 } else { 3293 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 3294 } 3295 connection->le_con_param_update_identifier = sig_id; 3296 } 3297 3298 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 3299 event[1] = 8; 3300 little_endian_store_16(event, 2, handle); 3301 (void)memcpy(&event[4], &command[4], 8); 3302 l2cap_emit_event(event, sizeof(event)); 3303 break; 3304 3305 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 3306 // check size 3307 if (len < 2u) return 0u; 3308 result = little_endian_read_16(command, 4); 3309 l2cap_emit_connection_parameter_update_response(handle, result); 3310 break; 3311 3312 #ifdef ENABLE_LE_DATA_CHANNELS 3313 3314 case COMMAND_REJECT: 3315 // Find channel for this sig_id and connection handle 3316 channel = NULL; 3317 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3318 while (btstack_linked_list_iterator_has_next(&it)){ 3319 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3320 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3321 if (a_channel->con_handle != handle) continue; 3322 if (a_channel->local_sig_id != sig_id) continue; 3323 channel = a_channel; 3324 break; 3325 } 3326 if (!channel) break; 3327 3328 // if received while waiting for le connection response, assume legacy device 3329 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 3330 channel->state = L2CAP_STATE_CLOSED; 3331 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 3332 l2cap_emit_le_channel_opened(channel, 0x0002); 3333 3334 // discard channel 3335 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3336 l2cap_free_channel_entry(channel); 3337 break; 3338 } 3339 break; 3340 3341 case LE_CREDIT_BASED_CONNECTION_REQUEST: 3342 // check size 3343 if (len < 10u) return 0u; 3344 3345 // get hci connection, bail if not found (must not happen) 3346 connection = hci_connection_for_handle(handle); 3347 if (!connection) return 0; 3348 3349 // check if service registered 3350 le_psm = little_endian_read_16(command, 4); 3351 service = l2cap_le_get_service(le_psm); 3352 source_cid = little_endian_read_16(command, 6); 3353 3354 if (service){ 3355 if (source_cid < 0x40u){ 3356 // 0x0009 Connection refused - Invalid Source CID 3357 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 3358 return 1; 3359 } 3360 3361 // go through list of channels for this ACL connection and check if we get a match 3362 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3363 while (btstack_linked_list_iterator_has_next(&it)){ 3364 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3365 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3366 if (a_channel->con_handle != handle) continue; 3367 if (a_channel->remote_cid != source_cid) continue; 3368 // 0x000a Connection refused - Source CID already allocated 3369 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 3370 return 1; 3371 } 3372 3373 // security: check encryption 3374 if (service->required_security_level >= LEVEL_2){ 3375 if (gap_encryption_key_size(handle) == 0){ 3376 // 0x0008 Connection refused - insufficient encryption 3377 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 3378 return 1; 3379 } 3380 // anything less than 16 byte key size is insufficient 3381 if (gap_encryption_key_size(handle) < 16){ 3382 // 0x0007 Connection refused – insufficient encryption key size 3383 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 3384 return 1; 3385 } 3386 } 3387 3388 // security: check authencation 3389 if (service->required_security_level >= LEVEL_3){ 3390 if (!gap_authenticated(handle)){ 3391 // 0x0005 Connection refused – insufficient authentication 3392 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 3393 return 1; 3394 } 3395 } 3396 3397 // security: check authorization 3398 if (service->required_security_level >= LEVEL_4){ 3399 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 3400 // 0x0006 Connection refused – insufficient authorization 3401 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 3402 return 1; 3403 } 3404 } 3405 3406 // allocate channel 3407 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 3408 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 3409 if (!channel){ 3410 // 0x0004 Connection refused – no resources available 3411 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 3412 return 1; 3413 } 3414 3415 channel->con_handle = handle; 3416 channel->remote_cid = source_cid; 3417 channel->remote_sig_id = sig_id; 3418 channel->remote_mtu = little_endian_read_16(command, 8); 3419 channel->remote_mps = little_endian_read_16(command, 10); 3420 channel->credits_outgoing = little_endian_read_16(command, 12); 3421 3422 // set initial state 3423 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3424 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3425 3426 // add to connections list 3427 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 3428 3429 // post connection request event 3430 l2cap_emit_le_incoming_connection(channel); 3431 3432 } else { 3433 // Connection refused – LE_PSM not supported 3434 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 3435 } 3436 break; 3437 3438 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 3439 // check size 3440 if (len < 10u) return 0u; 3441 3442 // Find channel for this sig_id and connection handle 3443 channel = NULL; 3444 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3445 while (btstack_linked_list_iterator_has_next(&it)){ 3446 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3447 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3448 if (a_channel->con_handle != handle) continue; 3449 if (a_channel->local_sig_id != sig_id) continue; 3450 channel = a_channel; 3451 break; 3452 } 3453 if (!channel) break; 3454 3455 // cid + 0 3456 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 3457 if (result){ 3458 channel->state = L2CAP_STATE_CLOSED; 3459 // map l2cap connection response result to BTstack status enumeration 3460 l2cap_emit_le_channel_opened(channel, result); 3461 3462 // discard channel 3463 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3464 l2cap_free_channel_entry(channel); 3465 break; 3466 } 3467 3468 // success 3469 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3470 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3471 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3472 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3473 channel->state = L2CAP_STATE_OPEN; 3474 l2cap_emit_le_channel_opened(channel, result); 3475 break; 3476 3477 case LE_FLOW_CONTROL_CREDIT: 3478 // check size 3479 if (len < 4u) return 0u; 3480 3481 // find channel 3482 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3483 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3484 if (!channel) { 3485 log_error("credit: no channel for cid 0x%02x", local_cid); 3486 break; 3487 } 3488 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3489 credits_before = channel->credits_outgoing; 3490 channel->credits_outgoing += new_credits; 3491 // check for credit overrun 3492 if (credits_before > channel->credits_outgoing){ 3493 log_error("credit: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 3494 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3495 break; 3496 } 3497 log_info("credit: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 3498 break; 3499 3500 case DISCONNECTION_REQUEST: 3501 3502 // check size 3503 if (len < 4u) return 0u; 3504 3505 // find channel 3506 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3507 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3508 if (!channel) { 3509 log_error("credit: no channel for cid 0x%02x", local_cid); 3510 break; 3511 } 3512 channel->remote_sig_id = sig_id; 3513 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3514 break; 3515 3516 #endif 3517 3518 case DISCONNECTION_RESPONSE: 3519 break; 3520 3521 default: 3522 // command unknown -> reject command 3523 return 0; 3524 } 3525 return 1; 3526 } 3527 #endif 3528 3529 #ifdef ENABLE_CLASSIC 3530 static void l2cap_acl_classic_handler_for_channel(l2cap_channel_t * l2cap_channel, uint8_t * packet, uint16_t size){ 3531 3532 // forward data only in OPEN state 3533 if (l2cap_channel->state != L2CAP_STATE_OPEN) return; 3534 3535 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3536 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3537 3538 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3539 3540 // assert control + FCS fields are inside 3541 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) return; 3542 3543 if (l2cap_channel->fcs_option){ 3544 // verify FCS (required if one side requested it) 3545 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3546 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3547 3548 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 3549 // simulate fcs error 3550 static int counter = 0; 3551 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 3552 log_info("Simulate fcs error"); 3553 fcs_calculated++; 3554 counter = 0; 3555 } 3556 #endif 3557 3558 if (fcs_calculated == fcs_packet){ 3559 log_info("Packet FCS 0x%04x verified", fcs_packet); 3560 } else { 3561 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3562 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 3563 return; 3564 } 3565 } 3566 3567 // switch on packet type 3568 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3569 uint8_t req_seq = (control >> 8) & 0x3f; 3570 int final = (control >> 7) & 0x01; 3571 if (control & 1){ 3572 // S-Frame 3573 int poll = (control >> 4) & 0x01; 3574 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3575 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3576 l2cap_ertm_tx_packet_state_t * tx_state; 3577 switch (s){ 3578 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3579 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3580 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3581 if (poll && final){ 3582 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3583 log_error("P=F=1 in S-Frame"); 3584 break; 3585 } 3586 if (poll){ 3587 // check if we did request selective retransmission before <==> we have stored SDU segments 3588 int i; 3589 int num_stored_out_of_order_packets = 0; 3590 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3591 int index = l2cap_channel->rx_store_index + i; 3592 if (index >= l2cap_channel->num_rx_buffers){ 3593 index -= l2cap_channel->num_rx_buffers; 3594 } 3595 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3596 if (!rx_state->valid) continue; 3597 num_stored_out_of_order_packets++; 3598 } 3599 if (num_stored_out_of_order_packets){ 3600 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3601 } else { 3602 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3603 } 3604 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3605 } 3606 if (final){ 3607 // Stop-MonitorTimer 3608 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3609 // If UnackedFrames > 0 then Start-RetransTimer 3610 if (l2cap_channel->unacked_frames){ 3611 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3612 } 3613 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3614 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3615 } 3616 break; 3617 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3618 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3619 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3620 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3621 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3622 break; 3623 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3624 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3625 break; 3626 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3627 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3628 if (poll){ 3629 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3630 } 3631 // find requested i-frame 3632 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3633 if (tx_state){ 3634 log_info("Retransmission for tx_seq %u requested", req_seq); 3635 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3636 tx_state->retransmission_requested = 1; 3637 l2cap_channel->srej_active = 1; 3638 } 3639 break; 3640 default: 3641 break; 3642 } 3643 } else { 3644 // I-Frame 3645 // get control 3646 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3647 uint8_t tx_seq = (control >> 1) & 0x3f; 3648 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3649 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3650 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3651 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3652 if (final){ 3653 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3654 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3655 } 3656 3657 // get SDU 3658 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3659 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3660 3661 // assert SDU size is smaller or equal to our buffers 3662 uint16_t max_payload_size = 0; 3663 switch (sar){ 3664 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 3665 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 3666 // SDU Length + MPS 3667 max_payload_size = l2cap_channel->local_mps + 2; 3668 break; 3669 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 3670 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 3671 max_payload_size = l2cap_channel->local_mps; 3672 break; 3673 default: 3674 btstack_assert(false); 3675 break; 3676 } 3677 if (payload_len > max_payload_size){ 3678 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 3679 return; 3680 } 3681 3682 // check ordering 3683 if (l2cap_channel->expected_tx_seq == tx_seq){ 3684 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3685 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3686 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3687 3688 // process SDU 3689 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 3690 3691 // process stored segments 3692 while (true){ 3693 int index = l2cap_channel->rx_store_index; 3694 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3695 if (!rx_state->valid) break; 3696 3697 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3698 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3699 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3700 3701 rx_state->valid = 0; 3702 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3703 3704 // update rx store index 3705 index++; 3706 if (index >= l2cap_channel->num_rx_buffers){ 3707 index = 0; 3708 } 3709 l2cap_channel->rx_store_index = index; 3710 } 3711 3712 // 3713 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3714 3715 } else { 3716 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3717 if (delta < 2){ 3718 // store segment 3719 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 3720 3721 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3722 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3723 } else { 3724 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3725 l2cap_channel->send_supervisor_frame_reject = 1; 3726 } 3727 } 3728 } 3729 return; 3730 } 3731 #endif 3732 3733 // check size 3734 uint16_t payload_size = size - COMPLETE_L2CAP_HEADER; 3735 if (l2cap_channel->local_mtu < payload_size) return; 3736 3737 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], payload_size); 3738 } 3739 #endif 3740 3741 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3742 #ifdef ENABLE_CLASSIC 3743 l2cap_channel_t * l2cap_channel; 3744 l2cap_fixed_channel_t * l2cap_fixed_channel; 3745 3746 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3747 uint8_t broadcast_flag = READ_ACL_FLAGS(packet) >> 2; 3748 switch (channel_id) { 3749 3750 case L2CAP_CID_SIGNALING: { 3751 if (broadcast_flag != 0) break; 3752 uint32_t command_offset = 8; 3753 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 3754 // assert signaling command is fully inside packet 3755 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3756 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3757 if (next_command_offset > size){ 3758 log_error("l2cap signaling command len invalid -> drop"); 3759 break; 3760 } 3761 // handle signaling command 3762 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3763 // go to next command 3764 command_offset = next_command_offset; 3765 } 3766 break; 3767 } 3768 3769 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3770 if (broadcast_flag == 0) break; 3771 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3772 if (!l2cap_fixed_channel) break; 3773 if (!l2cap_fixed_channel->packet_handler) break; 3774 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3775 break; 3776 3777 #ifdef ENABLE_BLE 3778 case L2CAP_CID_BR_EDR_SECURITY_MANAGER: 3779 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3780 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 3781 // Pairing Failed 3782 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_BR_EDR_SECURITY_MANAGER, SM_REASON_PAIRING_NOT_SUPPORTED); 3783 } else { 3784 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3785 } 3786 break; 3787 #endif 3788 3789 default: 3790 if (broadcast_flag != 0) break; 3791 // Find channel for this channel_id and connection handle 3792 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3793 if (l2cap_channel != NULL){ 3794 l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size); 3795 } 3796 break; 3797 } 3798 #else 3799 UNUSED(handle); // ok: no code 3800 UNUSED(packet); // ok: no code 3801 UNUSED(size); // ok: no code 3802 #endif 3803 } 3804 3805 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3806 #ifdef ENABLE_BLE 3807 3808 l2cap_fixed_channel_t * l2cap_fixed_channel; 3809 3810 #ifdef ENABLE_LE_DATA_CHANNELS 3811 l2cap_channel_t * l2cap_channel; 3812 #endif 3813 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3814 switch (channel_id) { 3815 3816 case L2CAP_CID_SIGNALING_LE: { 3817 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3818 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3819 if ((COMPLETE_L2CAP_HEADER + 4u + len) > size) break; 3820 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3821 if (!valid){ 3822 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3823 } 3824 break; 3825 } 3826 3827 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3828 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3829 if (!l2cap_fixed_channel) break; 3830 if (!l2cap_fixed_channel->packet_handler) break; 3831 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3832 break; 3833 3834 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3835 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3836 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 3837 // Pairing Failed 3838 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, SM_REASON_PAIRING_NOT_SUPPORTED); 3839 } else { 3840 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3841 } 3842 break; 3843 3844 default: 3845 3846 #ifdef ENABLE_LE_DATA_CHANNELS 3847 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3848 if (l2cap_channel != NULL) { 3849 // credit counting 3850 if (l2cap_channel->credits_incoming == 0u){ 3851 log_info("LE Data Channel packet received but no incoming credits"); 3852 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3853 break; 3854 } 3855 l2cap_channel->credits_incoming--; 3856 3857 // automatic credits 3858 if ((l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK) && l2cap_channel->automatic_credits){ 3859 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3860 } 3861 3862 // first fragment 3863 uint16_t pos = 0; 3864 if (!l2cap_channel->receive_sdu_len){ 3865 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3866 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3867 l2cap_channel->receive_sdu_len = sdu_len; 3868 l2cap_channel->receive_sdu_pos = 0; 3869 pos += 2u; 3870 size -= 2u; 3871 } 3872 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3873 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3874 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3875 (void)memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], 3876 &packet[COMPLETE_L2CAP_HEADER + pos], 3877 fragment_size); 3878 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3879 // done? 3880 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3881 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3882 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3883 l2cap_channel->receive_sdu_len = 0; 3884 } 3885 } 3886 #endif 3887 break; 3888 } 3889 #else 3890 UNUSED(handle); // ok: no code 3891 UNUSED(packet); // ok: no code 3892 UNUSED(size); // ok: no code 3893 #endif 3894 } 3895 3896 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3897 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3898 UNUSED(channel); // ok: there is no channel 3899 3900 // Assert full L2CAP header present 3901 if (size < COMPLETE_L2CAP_HEADER) return; 3902 3903 // Dispatch to Classic or LE handler (SCO packets are not dispatched to L2CAP) 3904 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3905 hci_connection_t *conn = hci_connection_for_handle(handle); 3906 if (!conn) return; 3907 if (conn->address_type == BD_ADDR_TYPE_ACL){ 3908 l2cap_acl_classic_handler(handle, packet, size); 3909 } else { 3910 l2cap_acl_le_handler(handle, packet, size); 3911 } 3912 3913 l2cap_run(); 3914 } 3915 3916 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3917 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 3918 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3919 if (!channel) return; 3920 channel->packet_handler = packet_handler; 3921 } 3922 3923 #ifdef ENABLE_CLASSIC 3924 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3925 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3926 channel->state = L2CAP_STATE_CLOSED; 3927 l2cap_handle_channel_closed(channel); 3928 // discard channel 3929 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3930 l2cap_free_channel_entry(channel); 3931 } 3932 #endif 3933 3934 #ifdef L2CAP_USES_CHANNELS 3935 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3936 btstack_linked_list_iterator_t it; 3937 btstack_linked_list_iterator_init(&it, services); 3938 while (btstack_linked_list_iterator_has_next(&it)){ 3939 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3940 if ( service->psm == psm){ 3941 return service; 3942 }; 3943 } 3944 return NULL; 3945 } 3946 #endif 3947 3948 #ifdef ENABLE_CLASSIC 3949 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3950 return l2cap_get_service_internal(&l2cap_services, psm); 3951 } 3952 3953 static void l2cap_update_minimal_security_level(void){ 3954 // update minimal service security level 3955 gap_security_level_t minimal_level = LEVEL_1; 3956 btstack_linked_list_iterator_t it; 3957 btstack_linked_list_iterator_init(&it, &l2cap_services); 3958 while (btstack_linked_list_iterator_has_next(&it)){ 3959 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3960 if (service->required_security_level > minimal_level){ 3961 minimal_level = service->required_security_level; 3962 }; 3963 } 3964 gap_set_minimal_service_security_level(minimal_level); 3965 } 3966 3967 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3968 3969 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3970 3971 // check for alread registered psm 3972 l2cap_service_t *service = l2cap_get_service(psm); 3973 if (service) { 3974 log_error("register: PSM %u already registered", psm); 3975 return L2CAP_SERVICE_ALREADY_REGISTERED; 3976 } 3977 3978 // alloc structure 3979 service = btstack_memory_l2cap_service_get(); 3980 if (!service) { 3981 log_error("register: no memory for l2cap_service_t"); 3982 return BTSTACK_MEMORY_ALLOC_FAILED; 3983 } 3984 3985 // fill in 3986 service->psm = psm; 3987 service->mtu = mtu; 3988 service->packet_handler = service_packet_handler; 3989 service->required_security_level = security_level; 3990 3991 // add to services list 3992 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3993 3994 l2cap_update_minimal_security_level(); 3995 3996 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 3997 // enable page scan 3998 gap_connectable_control(1); 3999 #endif 4000 4001 4002 return ERROR_CODE_SUCCESS; 4003 } 4004 4005 uint8_t l2cap_unregister_service(uint16_t psm){ 4006 4007 log_info("unregister psm 0x%x", psm); 4008 4009 l2cap_service_t *service = l2cap_get_service(psm); 4010 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 4011 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 4012 btstack_memory_l2cap_service_free(service); 4013 4014 l2cap_update_minimal_security_level(); 4015 4016 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 4017 // disable page scan when no services registered 4018 if (btstack_linked_list_empty(&l2cap_services)) { 4019 gap_connectable_control(0); 4020 } 4021 #endif 4022 4023 return ERROR_CODE_SUCCESS; 4024 } 4025 #endif 4026 4027 4028 #ifdef ENABLE_LE_DATA_CHANNELS 4029 4030 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 4031 if (!channel->waiting_for_can_send_now) return; 4032 if (channel->send_sdu_buffer) return; 4033 channel->waiting_for_can_send_now = 0; 4034 log_debug("le can send now, local_cid 0x%x", channel->local_cid); 4035 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 4036 } 4037 4038 // 1BH2222 4039 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 4040 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", 4041 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 4042 uint8_t event[19]; 4043 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 4044 event[1] = sizeof(event) - 2u; 4045 event[2] = channel->address_type; 4046 reverse_bd_addr(channel->address, &event[3]); 4047 little_endian_store_16(event, 9, channel->con_handle); 4048 little_endian_store_16(event, 11, channel->psm); 4049 little_endian_store_16(event, 13, channel->local_cid); 4050 little_endian_store_16(event, 15, channel->remote_cid); 4051 little_endian_store_16(event, 17, channel->remote_mtu); 4052 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4053 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4054 } 4055 // 11BH22222 4056 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 4057 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", 4058 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 4059 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 4060 uint8_t event[23]; 4061 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 4062 event[1] = sizeof(event) - 2u; 4063 event[2] = status; 4064 event[3] = channel->address_type; 4065 reverse_bd_addr(channel->address, &event[4]); 4066 little_endian_store_16(event, 10, channel->con_handle); 4067 event[12] = (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) ? 1 : 0; 4068 little_endian_store_16(event, 13, channel->psm); 4069 little_endian_store_16(event, 15, channel->local_cid); 4070 little_endian_store_16(event, 17, channel->remote_cid); 4071 little_endian_store_16(event, 19, channel->local_mtu); 4072 little_endian_store_16(event, 21, channel->remote_mtu); 4073 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4074 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4075 } 4076 // 2 4077 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 4078 log_info("closed local_cid 0x%x", channel->local_cid); 4079 uint8_t event[4]; 4080 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 4081 event[1] = sizeof(event) - 2u; 4082 little_endian_store_16(event, 2, channel->local_cid); 4083 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4084 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4085 } 4086 4087 static void l2cap_le_send_pdu(l2cap_channel_t *channel){ 4088 btstack_assert(channel != NULL); 4089 btstack_assert(channel->send_sdu_buffer != NULL); 4090 btstack_assert(channel->credits_outgoing > 0); 4091 4092 // send part of SDU 4093 hci_reserve_packet_buffer(); 4094 uint8_t * acl_buffer = hci_get_outgoing_packet_buffer(); 4095 uint8_t * l2cap_payload = acl_buffer + 8; 4096 uint16_t pos = 0; 4097 if (!channel->send_sdu_pos){ 4098 // store SDU len 4099 channel->send_sdu_pos += 2u; 4100 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 4101 pos += 2u; 4102 } 4103 uint16_t payload_size = btstack_min(channel->send_sdu_len + 2u - channel->send_sdu_pos, channel->remote_mps - pos); 4104 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 4105 (void)memcpy(&l2cap_payload[pos], 4106 &channel->send_sdu_buffer[channel->send_sdu_pos - 2u], 4107 payload_size); // -2 for virtual SDU len 4108 pos += payload_size; 4109 channel->send_sdu_pos += payload_size; 4110 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 4111 4112 channel->credits_outgoing--; 4113 4114 hci_send_acl_packet_buffer(8u + pos); 4115 4116 if (channel->send_sdu_pos >= (channel->send_sdu_len + 2u)){ 4117 channel->send_sdu_buffer = NULL; 4118 // send done event 4119 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 4120 // inform about can send now 4121 l2cap_le_notify_channel_can_send(channel); 4122 } 4123 } 4124 4125 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 4126 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 4127 channel->state = L2CAP_STATE_CLOSED; 4128 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 4129 // discard channel 4130 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4131 l2cap_free_channel_entry(channel); 4132 } 4133 4134 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 4135 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 4136 } 4137 4138 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 4139 4140 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 4141 4142 // check for alread registered psm 4143 l2cap_service_t *service = l2cap_le_get_service(psm); 4144 if (service) { 4145 return L2CAP_SERVICE_ALREADY_REGISTERED; 4146 } 4147 4148 // alloc structure 4149 service = btstack_memory_l2cap_service_get(); 4150 if (!service) { 4151 log_error("register: no memory for l2cap_service_t"); 4152 return BTSTACK_MEMORY_ALLOC_FAILED; 4153 } 4154 4155 // fill in 4156 service->psm = psm; 4157 service->mtu = 0; 4158 service->packet_handler = packet_handler; 4159 service->required_security_level = security_level; 4160 4161 // add to services list 4162 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 4163 4164 // done 4165 return ERROR_CODE_SUCCESS; 4166 } 4167 4168 uint8_t l2cap_le_unregister_service(uint16_t psm) { 4169 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 4170 l2cap_service_t *service = l2cap_le_get_service(psm); 4171 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 4172 4173 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 4174 btstack_memory_l2cap_service_free(service); 4175 return ERROR_CODE_SUCCESS; 4176 } 4177 4178 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 4179 // get channel 4180 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4181 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4182 4183 // validate state 4184 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 4185 return ERROR_CODE_COMMAND_DISALLOWED; 4186 } 4187 4188 // set state accept connection 4189 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 4190 channel->receive_sdu_buffer = receive_sdu_buffer; 4191 channel->local_mtu = mtu; 4192 channel->new_credits_incoming = initial_credits; 4193 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4194 4195 // test 4196 // channel->new_credits_incoming = 1; 4197 4198 // go 4199 l2cap_run(); 4200 return ERROR_CODE_SUCCESS; 4201 } 4202 4203 /** 4204 * @brief Deny incoming LE Data Channel connection due to resource constraints 4205 * @param local_cid L2CAP LE Data Channel Identifier 4206 */ 4207 4208 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 4209 // get channel 4210 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4211 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4212 4213 // validate state 4214 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 4215 return ERROR_CODE_COMMAND_DISALLOWED; 4216 } 4217 4218 // set state decline connection 4219 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 4220 channel->reason = 0x04; // no resources available 4221 l2cap_run(); 4222 return ERROR_CODE_SUCCESS; 4223 } 4224 4225 static gap_security_level_t l2cap_le_security_level_for_connection(hci_con_handle_t con_handle){ 4226 uint8_t encryption_key_size = gap_encryption_key_size(con_handle); 4227 if (encryption_key_size == 0) return LEVEL_0; 4228 4229 uint8_t authenticated = gap_authenticated(con_handle); 4230 if (!authenticated) return LEVEL_2; 4231 4232 return encryption_key_size == 16 ? LEVEL_4 : LEVEL_3; 4233 } 4234 4235 // used to handle pairing complete after triggering to increase 4236 static void l2cap_sm_packet_handler(uint8_t packet_type, uint16_t channel_nr, uint8_t *packet, uint16_t size) { 4237 UNUSED(channel_nr); 4238 UNUSED(size); 4239 UNUSED(packet_type); 4240 btstack_assert(packet_type == HCI_EVENT_PACKET); 4241 if (hci_event_packet_get_type(packet) != SM_EVENT_PAIRING_COMPLETE) return; 4242 hci_con_handle_t con_handle = sm_event_pairing_complete_get_handle(packet); 4243 btstack_linked_list_iterator_t it; 4244 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4245 while (btstack_linked_list_iterator_has_next(&it)) { 4246 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4247 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 4248 if (channel->con_handle != con_handle) continue; 4249 if (channel->state != L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE) continue; 4250 4251 // found channel, check security level 4252 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4253 // pairing failed or wasn't good enough, inform user 4254 l2cap_emit_le_channel_opened(channel, ERROR_CODE_INSUFFICIENT_SECURITY); 4255 // discard channel 4256 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4257 l2cap_free_channel_entry(channel); 4258 } else { 4259 // send conn request now 4260 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4261 l2cap_run(); 4262 } 4263 } 4264 } 4265 4266 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 4267 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 4268 uint16_t * out_local_cid) { 4269 4270 static btstack_packet_callback_registration_t sm_event_callback_registration; 4271 static bool sm_callback_registered = false; 4272 4273 log_info("create, handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 4274 4275 hci_connection_t * connection = hci_connection_for_handle(con_handle); 4276 if (!connection) { 4277 log_error("no hci_connection for handle 0x%04x", con_handle); 4278 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 4279 } 4280 4281 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, connection->address_type, psm, mtu, security_level); 4282 if (!channel) { 4283 return BTSTACK_MEMORY_ALLOC_FAILED; 4284 } 4285 log_info("created %p", channel); 4286 4287 // store local_cid 4288 if (out_local_cid){ 4289 *out_local_cid = channel->local_cid; 4290 } 4291 4292 // setup channel entry 4293 channel->con_handle = con_handle; 4294 channel->receive_sdu_buffer = receive_sdu_buffer; 4295 channel->new_credits_incoming = initial_credits; 4296 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4297 4298 // add to connections list 4299 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 4300 4301 // check security level 4302 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4303 if (!sm_callback_registered){ 4304 sm_callback_registered = true; 4305 // lazy registration for SM events 4306 sm_event_callback_registration.callback = &l2cap_sm_packet_handler; 4307 sm_add_event_handler(&sm_event_callback_registration); 4308 } 4309 4310 // start pairing 4311 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 4312 sm_request_pairing(con_handle); 4313 } else { 4314 // send conn request right away 4315 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4316 l2cap_run(); 4317 } 4318 4319 return ERROR_CODE_SUCCESS; 4320 } 4321 4322 /** 4323 * @brief Provide credtis for LE Data Channel 4324 * @param local_cid L2CAP LE Data Channel Identifier 4325 * @param credits Number additional credits for peer 4326 */ 4327 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 4328 4329 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4330 if (!channel) { 4331 log_error("le credits no channel for cid 0x%02x", local_cid); 4332 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4333 } 4334 4335 // check state 4336 if (channel->state != L2CAP_STATE_OPEN){ 4337 log_error("le credits but channel 0x%02x not open yet", local_cid); 4338 } 4339 4340 // assert incoming credits + credits <= 0xffff 4341 uint32_t total_credits = channel->credits_incoming; 4342 total_credits += channel->new_credits_incoming; 4343 total_credits += credits; 4344 if (total_credits > 0xffffu){ 4345 log_error("le credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 4346 channel->new_credits_incoming, credits); 4347 } 4348 4349 // set credits_granted 4350 channel->new_credits_incoming += credits; 4351 4352 // go 4353 l2cap_run(); 4354 return ERROR_CODE_SUCCESS; 4355 } 4356 4357 /** 4358 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 4359 * @param local_cid L2CAP LE Data Channel Identifier 4360 */ 4361 bool l2cap_le_can_send_now(uint16_t local_cid){ 4362 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4363 if (!channel) { 4364 log_error("le can send now, no channel for cid 0x%02x", local_cid); 4365 return false; 4366 } 4367 4368 // check state 4369 if (channel->state != L2CAP_STATE_OPEN) return false; 4370 4371 // check queue 4372 if (channel->send_sdu_buffer) return false; 4373 4374 // fine, go ahead 4375 return true; 4376 } 4377 4378 /** 4379 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 4380 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 4381 * so packet handler should be ready to handle it 4382 * @param local_cid L2CAP LE Data Channel Identifier 4383 */ 4384 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 4385 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4386 if (!channel) { 4387 log_error("can send now, no channel for cid 0x%02x", local_cid); 4388 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4389 } 4390 channel->waiting_for_can_send_now = 1; 4391 l2cap_le_notify_channel_can_send(channel); 4392 return ERROR_CODE_SUCCESS; 4393 } 4394 4395 /** 4396 * @brief Send data via LE Data Channel 4397 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 4398 * @param local_cid L2CAP LE Data Channel Identifier 4399 * @param data data to send 4400 * @param size data size 4401 */ 4402 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t size){ 4403 4404 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4405 if (!channel) { 4406 log_error("l2cap send, no channel for cid 0x%02x", local_cid); 4407 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4408 } 4409 4410 if (size > channel->remote_mtu){ 4411 log_error("l2cap send, cid 0x%02x, data length exceeds remote MTU.", local_cid); 4412 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 4413 } 4414 4415 if (channel->send_sdu_buffer){ 4416 log_info("l2cap send, cid 0x%02x, cannot send", local_cid); 4417 return BTSTACK_ACL_BUFFERS_FULL; 4418 } 4419 4420 channel->send_sdu_buffer = data; 4421 channel->send_sdu_len = size; 4422 channel->send_sdu_pos = 0; 4423 4424 l2cap_notify_channel_can_send(); 4425 return ERROR_CODE_SUCCESS; 4426 } 4427 4428 /** 4429 * @brief Disconnect from LE Data Channel 4430 * @param local_cid L2CAP LE Data Channel Identifier 4431 */ 4432 uint8_t l2cap_le_disconnect(uint16_t local_cid) 4433 { 4434 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4435 if (!channel) { 4436 log_error("l2cap send, no channel for cid 0x%02x", local_cid); 4437 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4438 } 4439 4440 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 4441 l2cap_run(); 4442 return ERROR_CODE_SUCCESS; 4443 } 4444 4445 #endif 4446