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