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