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 // get code, signalind identifier and command len 3143 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3144 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 3145 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3146 3147 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 3148 if ((code < 1) || (code == ECHO_RESPONSE) || (code > INFORMATION_RESPONSE)){ 3149 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3150 return; 3151 } 3152 3153 // general commands without an assigned channel 3154 switch(code) { 3155 3156 case CONNECTION_REQUEST: 3157 if (cmd_len == 4){ 3158 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3159 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3160 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 3161 } else { 3162 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3163 } 3164 return; 3165 3166 case ECHO_REQUEST: 3167 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 3168 return; 3169 3170 case INFORMATION_REQUEST: 3171 if (cmd_len == 2) { 3172 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3173 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 3174 } else { 3175 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3176 } 3177 return; 3178 3179 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3180 case INFORMATION_RESPONSE: { 3181 hci_connection_t * connection = hci_connection_for_handle(handle); 3182 if (!connection) return; 3183 if (connection->l2cap_state.information_state != L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE) return; 3184 3185 // get extended features from response if valid 3186 connection->l2cap_state.extended_feature_mask = 0; 3187 if (cmd_len >= 6) { 3188 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3189 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3190 if (result == 0 && info_type == L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) { 3191 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3192 } 3193 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 3194 } 3195 3196 // information request complete 3197 l2cap_handle_information_request_complete(connection); 3198 return; 3199 } 3200 #endif 3201 3202 default: 3203 break; 3204 } 3205 3206 // Get potential destination CID 3207 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3208 3209 // Find channel for this sig_id and connection handle 3210 btstack_linked_list_iterator_t it; 3211 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3212 while (btstack_linked_list_iterator_has_next(&it)){ 3213 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3214 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3215 if (channel->con_handle != handle) continue; 3216 if (code & 1) { 3217 // match odd commands (responses) by previous signaling identifier 3218 if (channel->local_sig_id == sig_id) { 3219 l2cap_signaling_handler_channel(channel, command); 3220 break; 3221 } 3222 } else { 3223 // match even commands (requests) by local channel id 3224 if (channel->local_cid == dest_cid) { 3225 l2cap_signaling_handler_channel(channel, command); 3226 break; 3227 } 3228 } 3229 } 3230 } 3231 #endif 3232 3233 #ifdef ENABLE_BLE 3234 3235 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 3236 uint8_t event[6]; 3237 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 3238 event[1] = 4; 3239 little_endian_store_16(event, 2, con_handle); 3240 little_endian_store_16(event, 4, result); 3241 l2cap_emit_event(event, sizeof(event)); 3242 } 3243 3244 // @return valid 3245 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 3246 hci_connection_t * connection; 3247 uint16_t result; 3248 uint8_t event[12]; 3249 3250 #ifdef ENABLE_LE_DATA_CHANNELS 3251 btstack_linked_list_iterator_t it; 3252 l2cap_channel_t * channel; 3253 uint16_t local_cid; 3254 uint16_t le_psm; 3255 uint16_t new_credits; 3256 uint16_t credits_before; 3257 l2cap_service_t * service; 3258 uint16_t source_cid; 3259 #endif 3260 3261 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3262 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3263 log_info("le dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 3264 3265 switch (code){ 3266 3267 case CONNECTION_PARAMETER_UPDATE_REQUEST: 3268 // check size 3269 if (len < 8u) return 0u; 3270 connection = hci_connection_for_handle(handle); 3271 if (connection != NULL){ 3272 if (connection->role != HCI_ROLE_MASTER){ 3273 // reject command without notifying upper layer when not in master role 3274 return 0; 3275 } 3276 le_connection_parameter_range_t existing_range; 3277 gap_get_connection_parameter_range(&existing_range); 3278 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3279 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3280 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3281 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 3282 3283 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 3284 if (update_parameter){ 3285 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 3286 connection->le_conn_interval_min = le_conn_interval_min; 3287 connection->le_conn_interval_max = le_conn_interval_max; 3288 connection->le_conn_latency = le_conn_latency; 3289 connection->le_supervision_timeout = le_supervision_timeout; 3290 } else { 3291 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 3292 } 3293 connection->le_con_param_update_identifier = sig_id; 3294 } 3295 3296 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 3297 event[1] = 8; 3298 little_endian_store_16(event, 2, handle); 3299 (void)memcpy(&event[4], &command[4], 8); 3300 l2cap_emit_event(event, sizeof(event)); 3301 break; 3302 3303 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 3304 // check size 3305 if (len < 2u) return 0u; 3306 result = little_endian_read_16(command, 4); 3307 l2cap_emit_connection_parameter_update_response(handle, result); 3308 break; 3309 3310 #ifdef ENABLE_LE_DATA_CHANNELS 3311 3312 case COMMAND_REJECT: 3313 // Find channel for this sig_id and connection handle 3314 channel = NULL; 3315 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3316 while (btstack_linked_list_iterator_has_next(&it)){ 3317 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3318 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3319 if (a_channel->con_handle != handle) continue; 3320 if (a_channel->local_sig_id != sig_id) continue; 3321 channel = a_channel; 3322 break; 3323 } 3324 if (!channel) break; 3325 3326 // if received while waiting for le connection response, assume legacy device 3327 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 3328 channel->state = L2CAP_STATE_CLOSED; 3329 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 3330 l2cap_emit_le_channel_opened(channel, 0x0002); 3331 3332 // discard channel 3333 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3334 l2cap_free_channel_entry(channel); 3335 break; 3336 } 3337 break; 3338 3339 case LE_CREDIT_BASED_CONNECTION_REQUEST: 3340 // check size 3341 if (len < 10u) return 0u; 3342 3343 // get hci connection, bail if not found (must not happen) 3344 connection = hci_connection_for_handle(handle); 3345 if (!connection) return 0; 3346 3347 // check if service registered 3348 le_psm = little_endian_read_16(command, 4); 3349 service = l2cap_le_get_service(le_psm); 3350 source_cid = little_endian_read_16(command, 6); 3351 3352 if (service){ 3353 if (source_cid < 0x40u){ 3354 // 0x0009 Connection refused - Invalid Source CID 3355 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 3356 return 1; 3357 } 3358 3359 // go through list of channels for this ACL connection and check if we get a match 3360 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3361 while (btstack_linked_list_iterator_has_next(&it)){ 3362 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3363 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3364 if (a_channel->con_handle != handle) continue; 3365 if (a_channel->remote_cid != source_cid) continue; 3366 // 0x000a Connection refused - Source CID already allocated 3367 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 3368 return 1; 3369 } 3370 3371 // security: check encryption 3372 if (service->required_security_level >= LEVEL_2){ 3373 if (gap_encryption_key_size(handle) == 0){ 3374 // 0x0008 Connection refused - insufficient encryption 3375 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 3376 return 1; 3377 } 3378 // anything less than 16 byte key size is insufficient 3379 if (gap_encryption_key_size(handle) < 16){ 3380 // 0x0007 Connection refused – insufficient encryption key size 3381 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 3382 return 1; 3383 } 3384 } 3385 3386 // security: check authencation 3387 if (service->required_security_level >= LEVEL_3){ 3388 if (!gap_authenticated(handle)){ 3389 // 0x0005 Connection refused – insufficient authentication 3390 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 3391 return 1; 3392 } 3393 } 3394 3395 // security: check authorization 3396 if (service->required_security_level >= LEVEL_4){ 3397 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 3398 // 0x0006 Connection refused – insufficient authorization 3399 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 3400 return 1; 3401 } 3402 } 3403 3404 // allocate channel 3405 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 3406 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 3407 if (!channel){ 3408 // 0x0004 Connection refused – no resources available 3409 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 3410 return 1; 3411 } 3412 3413 channel->con_handle = handle; 3414 channel->remote_cid = source_cid; 3415 channel->remote_sig_id = sig_id; 3416 channel->remote_mtu = little_endian_read_16(command, 8); 3417 channel->remote_mps = little_endian_read_16(command, 10); 3418 channel->credits_outgoing = little_endian_read_16(command, 12); 3419 3420 // set initial state 3421 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3422 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3423 3424 // add to connections list 3425 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 3426 3427 // post connection request event 3428 l2cap_emit_le_incoming_connection(channel); 3429 3430 } else { 3431 // Connection refused – LE_PSM not supported 3432 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 3433 } 3434 break; 3435 3436 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 3437 // check size 3438 if (len < 10u) return 0u; 3439 3440 // Find channel for this sig_id and connection handle 3441 channel = NULL; 3442 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3443 while (btstack_linked_list_iterator_has_next(&it)){ 3444 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3445 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3446 if (a_channel->con_handle != handle) continue; 3447 if (a_channel->local_sig_id != sig_id) continue; 3448 channel = a_channel; 3449 break; 3450 } 3451 if (!channel) break; 3452 3453 // cid + 0 3454 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 3455 if (result){ 3456 channel->state = L2CAP_STATE_CLOSED; 3457 // map l2cap connection response result to BTstack status enumeration 3458 l2cap_emit_le_channel_opened(channel, result); 3459 3460 // discard channel 3461 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3462 l2cap_free_channel_entry(channel); 3463 break; 3464 } 3465 3466 // success 3467 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3468 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3469 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3470 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3471 channel->state = L2CAP_STATE_OPEN; 3472 l2cap_emit_le_channel_opened(channel, result); 3473 break; 3474 3475 case LE_FLOW_CONTROL_CREDIT: 3476 // check size 3477 if (len < 4u) return 0u; 3478 3479 // find channel 3480 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3481 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3482 if (!channel) { 3483 log_error("credit: no channel for cid 0x%02x", local_cid); 3484 break; 3485 } 3486 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3487 credits_before = channel->credits_outgoing; 3488 channel->credits_outgoing += new_credits; 3489 // check for credit overrun 3490 if (credits_before > channel->credits_outgoing){ 3491 log_error("credit: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 3492 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3493 break; 3494 } 3495 log_info("credit: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 3496 break; 3497 3498 case DISCONNECTION_REQUEST: 3499 3500 // check size 3501 if (len < 4u) return 0u; 3502 3503 // find channel 3504 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3505 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3506 if (!channel) { 3507 log_error("credit: no channel for cid 0x%02x", local_cid); 3508 break; 3509 } 3510 channel->remote_sig_id = sig_id; 3511 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3512 break; 3513 3514 #endif 3515 3516 case DISCONNECTION_RESPONSE: 3517 break; 3518 3519 default: 3520 // command unknown -> reject command 3521 return 0; 3522 } 3523 return 1; 3524 } 3525 #endif 3526 3527 #ifdef ENABLE_CLASSIC 3528 static void l2cap_acl_classic_handler_for_channel(l2cap_channel_t * l2cap_channel, uint8_t * packet, uint16_t size){ 3529 3530 // forward data only in OPEN state 3531 if (l2cap_channel->state != L2CAP_STATE_OPEN) return; 3532 3533 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3534 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3535 3536 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3537 3538 // assert control + FCS fields are inside 3539 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) return; 3540 3541 if (l2cap_channel->fcs_option){ 3542 // verify FCS (required if one side requested it) 3543 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3544 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3545 3546 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 3547 // simulate fcs error 3548 static int counter = 0; 3549 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 3550 log_info("Simulate fcs error"); 3551 fcs_calculated++; 3552 counter = 0; 3553 } 3554 #endif 3555 3556 if (fcs_calculated == fcs_packet){ 3557 log_info("Packet FCS 0x%04x verified", fcs_packet); 3558 } else { 3559 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3560 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 3561 return; 3562 } 3563 } 3564 3565 // switch on packet type 3566 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3567 uint8_t req_seq = (control >> 8) & 0x3f; 3568 int final = (control >> 7) & 0x01; 3569 if (control & 1){ 3570 // S-Frame 3571 int poll = (control >> 4) & 0x01; 3572 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3573 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3574 l2cap_ertm_tx_packet_state_t * tx_state; 3575 switch (s){ 3576 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3577 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3578 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3579 if (poll && final){ 3580 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3581 log_error("P=F=1 in S-Frame"); 3582 break; 3583 } 3584 if (poll){ 3585 // check if we did request selective retransmission before <==> we have stored SDU segments 3586 int i; 3587 int num_stored_out_of_order_packets = 0; 3588 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3589 int index = l2cap_channel->rx_store_index + i; 3590 if (index >= l2cap_channel->num_rx_buffers){ 3591 index -= l2cap_channel->num_rx_buffers; 3592 } 3593 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3594 if (!rx_state->valid) continue; 3595 num_stored_out_of_order_packets++; 3596 } 3597 if (num_stored_out_of_order_packets){ 3598 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3599 } else { 3600 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3601 } 3602 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3603 } 3604 if (final){ 3605 // Stop-MonitorTimer 3606 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3607 // If UnackedFrames > 0 then Start-RetransTimer 3608 if (l2cap_channel->unacked_frames){ 3609 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3610 } 3611 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3612 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3613 } 3614 break; 3615 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3616 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3617 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3618 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3619 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3620 break; 3621 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3622 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3623 break; 3624 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3625 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3626 if (poll){ 3627 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3628 } 3629 // find requested i-frame 3630 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3631 if (tx_state){ 3632 log_info("Retransmission for tx_seq %u requested", req_seq); 3633 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3634 tx_state->retransmission_requested = 1; 3635 l2cap_channel->srej_active = 1; 3636 } 3637 break; 3638 default: 3639 break; 3640 } 3641 } else { 3642 // I-Frame 3643 // get control 3644 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3645 uint8_t tx_seq = (control >> 1) & 0x3f; 3646 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3647 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3648 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3649 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3650 if (final){ 3651 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3652 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3653 } 3654 3655 // get SDU 3656 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3657 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3658 3659 // assert SDU size is smaller or equal to our buffers 3660 uint16_t max_payload_size = 0; 3661 switch (sar){ 3662 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 3663 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 3664 // SDU Length + MPS 3665 max_payload_size = l2cap_channel->local_mps + 2; 3666 break; 3667 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 3668 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 3669 max_payload_size = l2cap_channel->local_mps; 3670 break; 3671 default: 3672 btstack_assert(false); 3673 break; 3674 } 3675 if (payload_len > max_payload_size){ 3676 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 3677 return; 3678 } 3679 3680 // check ordering 3681 if (l2cap_channel->expected_tx_seq == tx_seq){ 3682 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3683 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3684 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3685 3686 // process SDU 3687 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 3688 3689 // process stored segments 3690 while (true){ 3691 int index = l2cap_channel->rx_store_index; 3692 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3693 if (!rx_state->valid) break; 3694 3695 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3696 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3697 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3698 3699 rx_state->valid = 0; 3700 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3701 3702 // update rx store index 3703 index++; 3704 if (index >= l2cap_channel->num_rx_buffers){ 3705 index = 0; 3706 } 3707 l2cap_channel->rx_store_index = index; 3708 } 3709 3710 // 3711 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3712 3713 } else { 3714 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3715 if (delta < 2){ 3716 // store segment 3717 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 3718 3719 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3720 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3721 } else { 3722 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3723 l2cap_channel->send_supervisor_frame_reject = 1; 3724 } 3725 } 3726 } 3727 return; 3728 } 3729 #endif 3730 3731 // check size 3732 uint16_t payload_size = size - COMPLETE_L2CAP_HEADER; 3733 if (l2cap_channel->local_mtu < payload_size) return; 3734 3735 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], payload_size); 3736 } 3737 #endif 3738 3739 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3740 #ifdef ENABLE_CLASSIC 3741 l2cap_channel_t * l2cap_channel; 3742 l2cap_fixed_channel_t * l2cap_fixed_channel; 3743 3744 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3745 uint8_t broadcast_flag = READ_ACL_FLAGS(packet) >> 2; 3746 switch (channel_id) { 3747 3748 case L2CAP_CID_SIGNALING: { 3749 if (broadcast_flag != 0) break; 3750 uint32_t command_offset = 8; 3751 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 3752 // assert signaling command is fully inside packet 3753 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3754 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3755 if (next_command_offset > size){ 3756 log_error("l2cap signaling command len invalid -> drop"); 3757 break; 3758 } 3759 // handle signaling command 3760 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3761 // go to next command 3762 command_offset = next_command_offset; 3763 } 3764 break; 3765 } 3766 3767 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3768 if (broadcast_flag == 0) break; 3769 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3770 if (!l2cap_fixed_channel) break; 3771 if (!l2cap_fixed_channel->packet_handler) break; 3772 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3773 break; 3774 3775 #ifdef ENABLE_BLE 3776 case L2CAP_CID_BR_EDR_SECURITY_MANAGER: 3777 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3778 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 3779 // Pairing Failed 3780 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_BR_EDR_SECURITY_MANAGER, SM_REASON_PAIRING_NOT_SUPPORTED); 3781 } else { 3782 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3783 } 3784 break; 3785 #endif 3786 3787 default: 3788 if (broadcast_flag != 0) break; 3789 // Find channel for this channel_id and connection handle 3790 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3791 if (l2cap_channel != NULL){ 3792 l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size); 3793 } 3794 break; 3795 } 3796 #else 3797 UNUSED(handle); // ok: no code 3798 UNUSED(packet); // ok: no code 3799 UNUSED(size); // ok: no code 3800 #endif 3801 } 3802 3803 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3804 #ifdef ENABLE_BLE 3805 3806 l2cap_fixed_channel_t * l2cap_fixed_channel; 3807 3808 #ifdef ENABLE_LE_DATA_CHANNELS 3809 l2cap_channel_t * l2cap_channel; 3810 #endif 3811 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3812 switch (channel_id) { 3813 3814 case L2CAP_CID_SIGNALING_LE: { 3815 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3816 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3817 if ((COMPLETE_L2CAP_HEADER + 4u + len) > size) break; 3818 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3819 if (!valid){ 3820 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3821 } 3822 break; 3823 } 3824 3825 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3826 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3827 if (!l2cap_fixed_channel) break; 3828 if (!l2cap_fixed_channel->packet_handler) break; 3829 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3830 break; 3831 3832 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3833 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3834 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 3835 // Pairing Failed 3836 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, SM_REASON_PAIRING_NOT_SUPPORTED); 3837 } else { 3838 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3839 } 3840 break; 3841 3842 default: 3843 3844 #ifdef ENABLE_LE_DATA_CHANNELS 3845 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3846 if (l2cap_channel != NULL) { 3847 // credit counting 3848 if (l2cap_channel->credits_incoming == 0u){ 3849 log_info("LE Data Channel packet received but no incoming credits"); 3850 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3851 break; 3852 } 3853 l2cap_channel->credits_incoming--; 3854 3855 // automatic credits 3856 if ((l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK) && l2cap_channel->automatic_credits){ 3857 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3858 } 3859 3860 // first fragment 3861 uint16_t pos = 0; 3862 if (!l2cap_channel->receive_sdu_len){ 3863 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3864 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3865 l2cap_channel->receive_sdu_len = sdu_len; 3866 l2cap_channel->receive_sdu_pos = 0; 3867 pos += 2u; 3868 size -= 2u; 3869 } 3870 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3871 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3872 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3873 (void)memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], 3874 &packet[COMPLETE_L2CAP_HEADER + pos], 3875 fragment_size); 3876 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3877 // done? 3878 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3879 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3880 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3881 l2cap_channel->receive_sdu_len = 0; 3882 } 3883 } 3884 #endif 3885 break; 3886 } 3887 #else 3888 UNUSED(handle); // ok: no code 3889 UNUSED(packet); // ok: no code 3890 UNUSED(size); // ok: no code 3891 #endif 3892 } 3893 3894 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3895 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3896 UNUSED(channel); // ok: there is no channel 3897 3898 // Assert full L2CAP header present 3899 if (size < COMPLETE_L2CAP_HEADER) return; 3900 3901 // Dispatch to Classic or LE handler (SCO packets are not dispatched to L2CAP) 3902 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3903 hci_connection_t *conn = hci_connection_for_handle(handle); 3904 if (!conn) return; 3905 if (conn->address_type == BD_ADDR_TYPE_ACL){ 3906 l2cap_acl_classic_handler(handle, packet, size); 3907 } else { 3908 l2cap_acl_le_handler(handle, packet, size); 3909 } 3910 3911 l2cap_run(); 3912 } 3913 3914 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3915 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 3916 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3917 if (!channel) return; 3918 channel->packet_handler = packet_handler; 3919 } 3920 3921 #ifdef ENABLE_CLASSIC 3922 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3923 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3924 channel->state = L2CAP_STATE_CLOSED; 3925 l2cap_handle_channel_closed(channel); 3926 // discard channel 3927 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3928 l2cap_free_channel_entry(channel); 3929 } 3930 #endif 3931 3932 #ifdef L2CAP_USES_CHANNELS 3933 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3934 btstack_linked_list_iterator_t it; 3935 btstack_linked_list_iterator_init(&it, services); 3936 while (btstack_linked_list_iterator_has_next(&it)){ 3937 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3938 if ( service->psm == psm){ 3939 return service; 3940 }; 3941 } 3942 return NULL; 3943 } 3944 #endif 3945 3946 #ifdef ENABLE_CLASSIC 3947 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3948 return l2cap_get_service_internal(&l2cap_services, psm); 3949 } 3950 3951 static void l2cap_update_minimal_security_level(void){ 3952 // update minimal service security level 3953 gap_security_level_t minimal_level = LEVEL_1; 3954 btstack_linked_list_iterator_t it; 3955 btstack_linked_list_iterator_init(&it, &l2cap_services); 3956 while (btstack_linked_list_iterator_has_next(&it)){ 3957 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3958 if (service->required_security_level > minimal_level){ 3959 minimal_level = service->required_security_level; 3960 }; 3961 } 3962 gap_set_minimal_service_security_level(minimal_level); 3963 } 3964 3965 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3966 3967 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3968 3969 // check for alread registered psm 3970 l2cap_service_t *service = l2cap_get_service(psm); 3971 if (service) { 3972 log_error("register: PSM %u already registered", psm); 3973 return L2CAP_SERVICE_ALREADY_REGISTERED; 3974 } 3975 3976 // alloc structure 3977 service = btstack_memory_l2cap_service_get(); 3978 if (!service) { 3979 log_error("register: no memory for l2cap_service_t"); 3980 return BTSTACK_MEMORY_ALLOC_FAILED; 3981 } 3982 3983 // fill in 3984 service->psm = psm; 3985 service->mtu = mtu; 3986 service->packet_handler = service_packet_handler; 3987 service->required_security_level = security_level; 3988 3989 // add to services list 3990 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3991 3992 l2cap_update_minimal_security_level(); 3993 3994 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 3995 // enable page scan 3996 gap_connectable_control(1); 3997 #endif 3998 3999 4000 return ERROR_CODE_SUCCESS; 4001 } 4002 4003 uint8_t l2cap_unregister_service(uint16_t psm){ 4004 4005 log_info("unregister psm 0x%x", psm); 4006 4007 l2cap_service_t *service = l2cap_get_service(psm); 4008 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 4009 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 4010 btstack_memory_l2cap_service_free(service); 4011 4012 l2cap_update_minimal_security_level(); 4013 4014 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 4015 // disable page scan when no services registered 4016 if (btstack_linked_list_empty(&l2cap_services)) { 4017 gap_connectable_control(0); 4018 } 4019 #endif 4020 4021 return ERROR_CODE_SUCCESS; 4022 } 4023 #endif 4024 4025 4026 #ifdef ENABLE_LE_DATA_CHANNELS 4027 4028 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 4029 if (!channel->waiting_for_can_send_now) return; 4030 if (channel->send_sdu_buffer) return; 4031 channel->waiting_for_can_send_now = 0; 4032 log_debug("le can send now, local_cid 0x%x", channel->local_cid); 4033 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 4034 } 4035 4036 // 1BH2222 4037 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 4038 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", 4039 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 4040 uint8_t event[19]; 4041 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 4042 event[1] = sizeof(event) - 2u; 4043 event[2] = channel->address_type; 4044 reverse_bd_addr(channel->address, &event[3]); 4045 little_endian_store_16(event, 9, channel->con_handle); 4046 little_endian_store_16(event, 11, channel->psm); 4047 little_endian_store_16(event, 13, channel->local_cid); 4048 little_endian_store_16(event, 15, channel->remote_cid); 4049 little_endian_store_16(event, 17, channel->remote_mtu); 4050 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4051 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4052 } 4053 // 11BH22222 4054 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 4055 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", 4056 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 4057 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 4058 uint8_t event[23]; 4059 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 4060 event[1] = sizeof(event) - 2u; 4061 event[2] = status; 4062 event[3] = channel->address_type; 4063 reverse_bd_addr(channel->address, &event[4]); 4064 little_endian_store_16(event, 10, channel->con_handle); 4065 event[12] = (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) ? 1 : 0; 4066 little_endian_store_16(event, 13, channel->psm); 4067 little_endian_store_16(event, 15, channel->local_cid); 4068 little_endian_store_16(event, 17, channel->remote_cid); 4069 little_endian_store_16(event, 19, channel->local_mtu); 4070 little_endian_store_16(event, 21, channel->remote_mtu); 4071 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4072 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4073 } 4074 // 2 4075 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 4076 log_info("closed local_cid 0x%x", channel->local_cid); 4077 uint8_t event[4]; 4078 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 4079 event[1] = sizeof(event) - 2u; 4080 little_endian_store_16(event, 2, channel->local_cid); 4081 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4082 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4083 } 4084 4085 static void l2cap_le_send_pdu(l2cap_channel_t *channel){ 4086 btstack_assert(channel != NULL); 4087 btstack_assert(channel->send_sdu_buffer != NULL); 4088 btstack_assert(channel->credits_outgoing > 0); 4089 4090 // send part of SDU 4091 hci_reserve_packet_buffer(); 4092 uint8_t * acl_buffer = hci_get_outgoing_packet_buffer(); 4093 uint8_t * l2cap_payload = acl_buffer + 8; 4094 uint16_t pos = 0; 4095 if (!channel->send_sdu_pos){ 4096 // store SDU len 4097 channel->send_sdu_pos += 2u; 4098 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 4099 pos += 2u; 4100 } 4101 uint16_t payload_size = btstack_min(channel->send_sdu_len + 2u - channel->send_sdu_pos, channel->remote_mps - pos); 4102 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 4103 (void)memcpy(&l2cap_payload[pos], 4104 &channel->send_sdu_buffer[channel->send_sdu_pos - 2u], 4105 payload_size); // -2 for virtual SDU len 4106 pos += payload_size; 4107 channel->send_sdu_pos += payload_size; 4108 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 4109 4110 channel->credits_outgoing--; 4111 4112 hci_send_acl_packet_buffer(8u + pos); 4113 4114 if (channel->send_sdu_pos >= (channel->send_sdu_len + 2u)){ 4115 channel->send_sdu_buffer = NULL; 4116 // send done event 4117 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 4118 // inform about can send now 4119 l2cap_le_notify_channel_can_send(channel); 4120 } 4121 } 4122 4123 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 4124 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 4125 channel->state = L2CAP_STATE_CLOSED; 4126 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 4127 // discard channel 4128 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4129 l2cap_free_channel_entry(channel); 4130 } 4131 4132 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 4133 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 4134 } 4135 4136 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 4137 4138 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 4139 4140 // check for alread registered psm 4141 l2cap_service_t *service = l2cap_le_get_service(psm); 4142 if (service) { 4143 return L2CAP_SERVICE_ALREADY_REGISTERED; 4144 } 4145 4146 // alloc structure 4147 service = btstack_memory_l2cap_service_get(); 4148 if (!service) { 4149 log_error("register: no memory for l2cap_service_t"); 4150 return BTSTACK_MEMORY_ALLOC_FAILED; 4151 } 4152 4153 // fill in 4154 service->psm = psm; 4155 service->mtu = 0; 4156 service->packet_handler = packet_handler; 4157 service->required_security_level = security_level; 4158 4159 // add to services list 4160 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 4161 4162 // done 4163 return ERROR_CODE_SUCCESS; 4164 } 4165 4166 uint8_t l2cap_le_unregister_service(uint16_t psm) { 4167 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 4168 l2cap_service_t *service = l2cap_le_get_service(psm); 4169 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 4170 4171 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 4172 btstack_memory_l2cap_service_free(service); 4173 return ERROR_CODE_SUCCESS; 4174 } 4175 4176 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 4177 // get channel 4178 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4179 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4180 4181 // validate state 4182 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 4183 return ERROR_CODE_COMMAND_DISALLOWED; 4184 } 4185 4186 // set state accept connection 4187 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 4188 channel->receive_sdu_buffer = receive_sdu_buffer; 4189 channel->local_mtu = mtu; 4190 channel->new_credits_incoming = initial_credits; 4191 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4192 4193 // test 4194 // channel->new_credits_incoming = 1; 4195 4196 // go 4197 l2cap_run(); 4198 return ERROR_CODE_SUCCESS; 4199 } 4200 4201 /** 4202 * @brief Deny incoming LE Data Channel connection due to resource constraints 4203 * @param local_cid L2CAP LE Data Channel Identifier 4204 */ 4205 4206 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 4207 // get channel 4208 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4209 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4210 4211 // validate state 4212 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 4213 return ERROR_CODE_COMMAND_DISALLOWED; 4214 } 4215 4216 // set state decline connection 4217 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 4218 channel->reason = 0x04; // no resources available 4219 l2cap_run(); 4220 return ERROR_CODE_SUCCESS; 4221 } 4222 4223 static gap_security_level_t l2cap_le_security_level_for_connection(hci_con_handle_t con_handle){ 4224 uint8_t encryption_key_size = gap_encryption_key_size(con_handle); 4225 if (encryption_key_size == 0) return LEVEL_0; 4226 4227 uint8_t authenticated = gap_authenticated(con_handle); 4228 if (!authenticated) return LEVEL_2; 4229 4230 return encryption_key_size == 16 ? LEVEL_4 : LEVEL_3; 4231 } 4232 4233 // used to handle pairing complete after triggering to increase 4234 static void l2cap_sm_packet_handler(uint8_t packet_type, uint16_t channel_nr, uint8_t *packet, uint16_t size) { 4235 UNUSED(channel_nr); 4236 UNUSED(size); 4237 UNUSED(packet_type); 4238 btstack_assert(packet_type == HCI_EVENT_PACKET); 4239 if (hci_event_packet_get_type(packet) != SM_EVENT_PAIRING_COMPLETE) return; 4240 hci_con_handle_t con_handle = sm_event_pairing_complete_get_handle(packet); 4241 btstack_linked_list_iterator_t it; 4242 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4243 while (btstack_linked_list_iterator_has_next(&it)) { 4244 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4245 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 4246 if (channel->con_handle != con_handle) continue; 4247 if (channel->state != L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE) continue; 4248 4249 // found channel, check security level 4250 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4251 // pairing failed or wasn't good enough, inform user 4252 l2cap_emit_le_channel_opened(channel, ERROR_CODE_INSUFFICIENT_SECURITY); 4253 // discard channel 4254 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4255 l2cap_free_channel_entry(channel); 4256 } else { 4257 // send conn request now 4258 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4259 l2cap_run(); 4260 } 4261 } 4262 } 4263 4264 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 4265 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 4266 uint16_t * out_local_cid) { 4267 4268 static btstack_packet_callback_registration_t sm_event_callback_registration; 4269 static bool sm_callback_registered = false; 4270 4271 log_info("create, handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 4272 4273 hci_connection_t * connection = hci_connection_for_handle(con_handle); 4274 if (!connection) { 4275 log_error("no hci_connection for handle 0x%04x", con_handle); 4276 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 4277 } 4278 4279 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); 4280 if (!channel) { 4281 return BTSTACK_MEMORY_ALLOC_FAILED; 4282 } 4283 log_info("created %p", channel); 4284 4285 // store local_cid 4286 if (out_local_cid){ 4287 *out_local_cid = channel->local_cid; 4288 } 4289 4290 // setup channel entry 4291 channel->con_handle = con_handle; 4292 channel->receive_sdu_buffer = receive_sdu_buffer; 4293 channel->new_credits_incoming = initial_credits; 4294 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4295 4296 // add to connections list 4297 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 4298 4299 // check security level 4300 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4301 if (!sm_callback_registered){ 4302 sm_callback_registered = true; 4303 // lazy registration for SM events 4304 sm_event_callback_registration.callback = &l2cap_sm_packet_handler; 4305 sm_add_event_handler(&sm_event_callback_registration); 4306 } 4307 4308 // start pairing 4309 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 4310 sm_request_pairing(con_handle); 4311 } else { 4312 // send conn request right away 4313 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4314 l2cap_run(); 4315 } 4316 4317 return ERROR_CODE_SUCCESS; 4318 } 4319 4320 /** 4321 * @brief Provide credtis for LE Data Channel 4322 * @param local_cid L2CAP LE Data Channel Identifier 4323 * @param credits Number additional credits for peer 4324 */ 4325 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 4326 4327 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4328 if (!channel) { 4329 log_error("le credits no channel for cid 0x%02x", local_cid); 4330 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4331 } 4332 4333 // check state 4334 if (channel->state != L2CAP_STATE_OPEN){ 4335 log_error("le credits but channel 0x%02x not open yet", local_cid); 4336 } 4337 4338 // assert incoming credits + credits <= 0xffff 4339 uint32_t total_credits = channel->credits_incoming; 4340 total_credits += channel->new_credits_incoming; 4341 total_credits += credits; 4342 if (total_credits > 0xffffu){ 4343 log_error("le credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 4344 channel->new_credits_incoming, credits); 4345 } 4346 4347 // set credits_granted 4348 channel->new_credits_incoming += credits; 4349 4350 // go 4351 l2cap_run(); 4352 return ERROR_CODE_SUCCESS; 4353 } 4354 4355 /** 4356 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 4357 * @param local_cid L2CAP LE Data Channel Identifier 4358 */ 4359 bool l2cap_le_can_send_now(uint16_t local_cid){ 4360 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4361 if (!channel) { 4362 log_error("le can send now, no channel for cid 0x%02x", local_cid); 4363 return false; 4364 } 4365 4366 // check state 4367 if (channel->state != L2CAP_STATE_OPEN) return false; 4368 4369 // check queue 4370 if (channel->send_sdu_buffer) return false; 4371 4372 // fine, go ahead 4373 return true; 4374 } 4375 4376 /** 4377 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 4378 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 4379 * so packet handler should be ready to handle it 4380 * @param local_cid L2CAP LE Data Channel Identifier 4381 */ 4382 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 4383 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4384 if (!channel) { 4385 log_error("can send now, no channel for cid 0x%02x", local_cid); 4386 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4387 } 4388 channel->waiting_for_can_send_now = 1; 4389 l2cap_le_notify_channel_can_send(channel); 4390 return ERROR_CODE_SUCCESS; 4391 } 4392 4393 /** 4394 * @brief Send data via LE Data Channel 4395 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 4396 * @param local_cid L2CAP LE Data Channel Identifier 4397 * @param data data to send 4398 * @param size data size 4399 */ 4400 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t size){ 4401 4402 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4403 if (!channel) { 4404 log_error("l2cap send, no channel for cid 0x%02x", local_cid); 4405 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4406 } 4407 4408 if (size > channel->remote_mtu){ 4409 log_error("l2cap send, cid 0x%02x, data length exceeds remote MTU.", local_cid); 4410 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 4411 } 4412 4413 if (channel->send_sdu_buffer){ 4414 log_info("l2cap send, cid 0x%02x, cannot send", local_cid); 4415 return BTSTACK_ACL_BUFFERS_FULL; 4416 } 4417 4418 channel->send_sdu_buffer = data; 4419 channel->send_sdu_len = size; 4420 channel->send_sdu_pos = 0; 4421 4422 l2cap_notify_channel_can_send(); 4423 return ERROR_CODE_SUCCESS; 4424 } 4425 4426 /** 4427 * @brief Disconnect from LE Data Channel 4428 * @param local_cid L2CAP LE Data Channel Identifier 4429 */ 4430 uint8_t l2cap_le_disconnect(uint16_t local_cid) 4431 { 4432 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4433 if (!channel) { 4434 log_error("l2cap send, no channel for cid 0x%02x", local_cid); 4435 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4436 } 4437 4438 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 4439 l2cap_run(); 4440 return ERROR_CODE_SUCCESS; 4441 } 4442 4443 #endif 4444