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 1970 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1971 // ERTM requested: trigger information request if not already started then wait for response 1972 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1973 switch (connection->l2cap_state.information_state){ 1974 case L2CAP_INFORMATION_STATE_DONE: 1975 break; 1976 case L2CAP_INFORMATION_STATE_IDLE: 1977 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1978 /* fall through */ 1979 default: 1980 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1981 return; 1982 } 1983 } 1984 #endif 1985 1986 // fine, go ahead 1987 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1988 } 1989 1990 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1991 if ((channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE) || (channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION)) { 1992 log_info("connection complete con_handle %04x - for channel %p cid 0x%04x", (int) con_handle, channel, channel->local_cid); 1993 channel->con_handle = con_handle; 1994 // query remote features if pairing is required 1995 if (channel->required_security_level > LEVEL_0){ 1996 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1997 hci_remote_features_query(con_handle); 1998 } else { 1999 l2cap_ready_to_connect(channel); 2000 } 2001 } 2002 } 2003 2004 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 2005 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 2006 // double check if all feature pages are complete 2007 2008 bool security_required = channel->required_security_level > LEVEL_0; 2009 2010 // abort if Secure Connections Only Mode with legacy connection 2011 if (security_required && gap_get_secure_connections_only_mode() && gap_secure_connection(channel->con_handle) == 0){ 2012 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY); 2013 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2014 l2cap_free_channel_entry(channel); 2015 return; 2016 } 2017 2018 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) != 0){ 2019 2020 // Core V5.2, Vol 3, Part C, 5.2.2.2 - Security Mode 4 2021 // When a remote device attempts to access a service offered by a Bluetooth device that is in security mode 4 2022 // and a sufficient link key exists and authentication has not been performed the local device shall authenticate 2023 // the remote device and enable encryption after the channel establishment request is received but before a channel 2024 // establishment confirmation (L2CAP_ConnectRsp with result code of 0x0000 or a higher-level channel establishment 2025 // confirmation such as that of RFCOMM) is sent. 2026 2027 // If the remote device has indicated support for Secure Simple Pairing, a channel establishment request is 2028 // received for a service other than SDP, and encryption has not yet been enabled, then the local device shall 2029 // disconnect the ACL link with error code 0x05 - Authentication Failure. 2030 2031 // => Disconnect if l2cap request received in mode 4 and ssp supported, non-sdp psm, not encrypted, no link key available 2032 if ((gap_get_security_mode() == GAP_SECURITY_MODE_4) 2033 && gap_ssp_supported_on_both_sides(channel->con_handle) 2034 && (channel->psm != PSM_SDP) 2035 && (gap_encryption_key_size(channel->con_handle) == 0) 2036 && (gap_bonded(channel->con_handle) == false)){ 2037 hci_disconnect_security_block(channel->con_handle); 2038 return; 2039 } 2040 2041 // incoming: assert security requirements 2042 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 2043 if (channel->required_security_level <= gap_security_level(channel->con_handle)){ 2044 l2cap_handle_security_level_incoming_sufficient(channel); 2045 } else { 2046 // send connection pending if not already done 2047 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONN_RESP_PEND) == 0){ 2048 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND; 2049 } 2050 gap_request_security_level(channel->con_handle, channel->required_security_level); 2051 } 2052 } else { 2053 // outgoing: we have been waiting for remote supported features 2054 if (security_required){ 2055 // request security level 2056 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 2057 gap_request_security_level(channel->con_handle, channel->required_security_level); 2058 } else { 2059 l2cap_ready_to_connect(channel); 2060 } 2061 } 2062 } 2063 #endif 2064 2065 #ifdef L2CAP_USES_CHANNELS 2066 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, 2067 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 2068 2069 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 2070 if (!channel) { 2071 return NULL; 2072 } 2073 2074 // fill in 2075 channel->packet_handler = packet_handler; 2076 channel->channel_type = channel_type; 2077 bd_addr_copy(channel->address, address); 2078 channel->address_type = address_type; 2079 channel->psm = psm; 2080 channel->local_mtu = local_mtu; 2081 channel->remote_mtu = L2CAP_DEFAULT_MTU; 2082 channel->required_security_level = security_level; 2083 2084 // 2085 channel->local_cid = l2cap_next_local_cid(); 2086 channel->con_handle = HCI_CON_HANDLE_INVALID; 2087 2088 // set initial state 2089 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 2090 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 2091 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 2092 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 2093 2094 log_info("create channel %p, local_cid 0x%04x", channel, channel->local_cid); 2095 2096 return channel; 2097 } 2098 2099 static void l2cap_free_channel_entry(l2cap_channel_t * channel){ 2100 log_info("free channel %p, local_cid 0x%04x", channel, channel->local_cid); 2101 // assert all timers are stopped 2102 l2cap_stop_rtx(channel); 2103 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2104 l2cap_ertm_stop_retransmission_timer(channel); 2105 l2cap_ertm_stop_monitor_timer(channel); 2106 #endif 2107 // free memory 2108 btstack_memory_l2cap_channel_free(channel); 2109 } 2110 #endif 2111 2112 #ifdef ENABLE_CLASSIC 2113 2114 /** 2115 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 2116 * @param packet_handler 2117 * @param address 2118 * @param psm 2119 * @param mtu 2120 * @param local_cid 2121 */ 2122 2123 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){ 2124 // limit MTU to the size of our outgoing HCI buffer 2125 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 2126 2127 // determine security level based on psm 2128 const gap_security_level_t security_level = l2cap_security_level_0_allowed_for_PSM(psm) ? LEVEL_0 : gap_get_security_level(); 2129 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); 2130 2131 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); 2132 if (!channel) { 2133 return BTSTACK_MEMORY_ALLOC_FAILED; 2134 } 2135 2136 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2137 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2138 #endif 2139 2140 // add to connections list 2141 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2142 2143 // store local_cid 2144 if (out_local_cid){ 2145 *out_local_cid = channel->local_cid; 2146 } 2147 2148 // state: L2CAP_STATE_WILL_SEND_CREATE_CONNECTION 2149 2150 // check if hci connection is already usable, 2151 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_ACL); 2152 if (conn && conn->state == OPEN){ 2153 // simulate connection complete 2154 l2cap_handle_connection_complete(conn->con_handle, channel); 2155 2156 // state: L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES or L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST 2157 2158 // simulate if remote supported features if requested and already received 2159 if ((channel->state == L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) && hci_remote_features_available(conn->con_handle)) { 2160 // simulate remote features received 2161 l2cap_handle_remote_supported_features_received(channel); 2162 } 2163 } 2164 2165 l2cap_run(); 2166 2167 return ERROR_CODE_SUCCESS; 2168 } 2169 2170 void l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 2171 log_info("disconnect local_cid 0x%x reason 0x%x", local_cid, reason); 2172 UNUSED(reason); 2173 // find channel for local_cid 2174 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2175 if (channel) { 2176 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2177 } 2178 // process 2179 l2cap_run(); 2180 } 2181 2182 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 2183 // mark all channels before emitting open events as these could trigger new connetion requests to the same device 2184 btstack_linked_list_iterator_t it; 2185 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2186 while (btstack_linked_list_iterator_has_next(&it)){ 2187 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2188 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2189 if (bd_addr_cmp( channel->address, address) != 0) continue; 2190 // channel for this address found 2191 switch (channel->state){ 2192 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2193 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2194 channel->state = L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD; 2195 break; 2196 default: 2197 break; 2198 } 2199 } 2200 // emit and free marked entries. restart loop to deal with list changes 2201 int done = 0; 2202 while (!done) { 2203 done = 1; 2204 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2205 while (btstack_linked_list_iterator_has_next(&it)){ 2206 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2207 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2208 if (channel->state == L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD){ 2209 done = 0; 2210 // failure, forward error code 2211 l2cap_handle_channel_open_failed(channel, status); 2212 // discard channel 2213 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2214 l2cap_free_channel_entry(channel); 2215 break; 2216 } 2217 } 2218 } 2219 2220 } 2221 2222 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 2223 btstack_linked_list_iterator_t it; 2224 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2225 while (btstack_linked_list_iterator_has_next(&it)){ 2226 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2227 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2228 if ( ! bd_addr_cmp( channel->address, address) ){ 2229 l2cap_handle_connection_complete(handle, channel); 2230 } 2231 } 2232 // process 2233 l2cap_run(); 2234 } 2235 #endif 2236 2237 static bool l2cap_channel_ready_to_send(l2cap_channel_t * channel){ 2238 switch (channel->channel_type){ 2239 #ifdef ENABLE_CLASSIC 2240 case L2CAP_CHANNEL_TYPE_CLASSIC: 2241 if (channel->state != L2CAP_STATE_OPEN) return false; 2242 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2243 // send if we have more data and remote windows isn't full yet 2244 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2245 if (channel->unacked_frames >= btstack_min(channel->num_stored_tx_frames, channel->remote_tx_window_size)) return false; 2246 return hci_can_send_acl_classic_packet_now() != 0; 2247 } 2248 #endif 2249 if (!channel->waiting_for_can_send_now) return false; 2250 return (hci_can_send_acl_classic_packet_now() != 0); 2251 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2252 if (!channel->waiting_for_can_send_now) return false; 2253 return hci_can_send_acl_classic_packet_now() != 0; 2254 #endif 2255 #ifdef ENABLE_BLE 2256 case L2CAP_CHANNEL_TYPE_FIXED: 2257 if (!channel->waiting_for_can_send_now) return false; 2258 return hci_can_send_acl_le_packet_now() != 0; 2259 #ifdef ENABLE_LE_DATA_CHANNELS 2260 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2261 if (channel->state != L2CAP_STATE_OPEN) return false; 2262 if (channel->send_sdu_buffer == NULL) return false; 2263 if (channel->credits_outgoing == 0u) return false; 2264 return hci_can_send_acl_le_packet_now() != 0; 2265 #endif 2266 #endif 2267 default: 2268 return false; 2269 } 2270 } 2271 2272 static void l2cap_channel_trigger_send(l2cap_channel_t * channel){ 2273 switch (channel->channel_type){ 2274 #ifdef ENABLE_CLASSIC 2275 case L2CAP_CHANNEL_TYPE_CLASSIC: 2276 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2277 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2278 l2cap_ertm_channel_send_information_frame(channel); 2279 return; 2280 } 2281 #endif 2282 channel->waiting_for_can_send_now = 0; 2283 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2284 break; 2285 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2286 channel->waiting_for_can_send_now = 0; 2287 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2288 break; 2289 #endif 2290 #ifdef ENABLE_BLE 2291 case L2CAP_CHANNEL_TYPE_FIXED: 2292 channel->waiting_for_can_send_now = 0; 2293 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2294 break; 2295 #ifdef ENABLE_LE_DATA_CHANNELS 2296 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2297 l2cap_le_send_pdu(channel); 2298 break; 2299 #endif 2300 #endif 2301 default: 2302 break; 2303 } 2304 } 2305 2306 static void l2cap_notify_channel_can_send(void){ 2307 bool done = false; 2308 while (!done){ 2309 done = true; 2310 btstack_linked_list_iterator_t it; 2311 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2312 while (btstack_linked_list_iterator_has_next(&it)){ 2313 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2314 bool ready = l2cap_channel_ready_to_send(channel); 2315 if (!ready) continue; 2316 2317 // requeue channel for fairness 2318 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2319 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2320 2321 // trigger sending 2322 l2cap_channel_trigger_send(channel); 2323 2324 // exit inner loop as we just broke the iterator, but try again 2325 done = false; 2326 break; 2327 } 2328 } 2329 } 2330 2331 #ifdef L2CAP_USES_CHANNELS 2332 2333 static int l2cap_send_open_failed_on_hci_disconnect(l2cap_channel_t * channel){ 2334 // open cannot fail for for incoming connections 2335 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) return 0; 2336 2337 // check state 2338 switch (channel->state){ 2339 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2340 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2341 case L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES: 2342 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2343 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 2344 case L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES: 2345 case L2CAP_STATE_WAIT_CONNECT_RSP: 2346 case L2CAP_STATE_CONFIG: 2347 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 2348 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 2349 case L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE: 2350 case L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD: 2351 return 1; 2352 2353 case L2CAP_STATE_OPEN: 2354 case L2CAP_STATE_CLOSED: 2355 case L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES: 2356 case L2CAP_STATE_WAIT_DISCONNECT: 2357 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_INSUFFICIENT_SECURITY: 2358 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 2359 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 2360 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2361 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 2362 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 2363 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 2364 case L2CAP_STATE_INVALID: 2365 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2366 return 0; 2367 2368 default: 2369 // get a "warning" about new states 2370 btstack_assert(false); 2371 return 0; 2372 } 2373 } 2374 #endif 2375 2376 #ifdef ENABLE_CLASSIC 2377 static void l2cap_handle_hci_disconnect_event(l2cap_channel_t * channel){ 2378 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2379 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2380 } else { 2381 l2cap_handle_channel_closed(channel); 2382 } 2383 l2cap_free_channel_entry(channel); 2384 } 2385 #endif 2386 2387 #ifdef ENABLE_LE_DATA_CHANNELS 2388 static void l2cap_handle_hci_le_disconnect_event(l2cap_channel_t * channel){ 2389 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2390 l2cap_emit_le_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2391 } else { 2392 l2cap_emit_le_channel_closed(channel); 2393 } 2394 l2cap_free_channel_entry(channel); 2395 } 2396 #endif 2397 2398 #ifdef ENABLE_CLASSIC 2399 static void l2cap_check_classic_timeout(hci_con_handle_t handle){ 2400 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) { 2401 return; 2402 } 2403 if (hci_authentication_active_for_handle(handle)) { 2404 return; 2405 } 2406 bool hci_con_used = false; 2407 btstack_linked_list_iterator_t it; 2408 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2409 while (btstack_linked_list_iterator_has_next(&it)){ 2410 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2411 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2412 if (channel->con_handle != handle) continue; 2413 hci_con_used = true; 2414 break; 2415 } 2416 if (hci_con_used) { 2417 return; 2418 } 2419 if (!hci_can_send_command_packet_now()) { 2420 return; 2421 } 2422 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 2423 } 2424 2425 static void l2cap_handle_features_complete(hci_con_handle_t handle){ 2426 if (hci_remote_features_available(handle) == false){ 2427 return; 2428 } 2429 btstack_linked_list_iterator_t it; 2430 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2431 while (btstack_linked_list_iterator_has_next(&it)){ 2432 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2433 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2434 if (channel->con_handle != handle) continue; 2435 log_info("remote supported features, channel %p, cid %04x - state %u", channel, channel->local_cid, channel->state); 2436 l2cap_handle_remote_supported_features_received(channel); 2437 } 2438 } 2439 2440 static void l2cap_handle_security_level_incoming_sufficient(l2cap_channel_t * channel){ 2441 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2442 l2cap_emit_incoming_connection(channel); 2443 } 2444 2445 static void l2cap_handle_security_level(hci_con_handle_t handle, gap_security_level_t actual_level){ 2446 log_info("security level update for handle 0x%04x", handle); 2447 2448 // trigger l2cap information requests 2449 if (actual_level > LEVEL_0){ 2450 hci_connection_t * hci_connection = hci_connection_for_handle(handle); 2451 btstack_assert(hci_connection != NULL); 2452 if (hci_connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 2453 hci_connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2454 } 2455 } 2456 2457 btstack_linked_list_iterator_t it; 2458 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2459 while (btstack_linked_list_iterator_has_next(&it)){ 2460 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2461 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2462 if (channel->con_handle != handle) continue; 2463 2464 gap_security_level_t required_level = channel->required_security_level; 2465 2466 log_info("channel %p, cid %04x - state %u: actual %u >= required %u?", channel, channel->local_cid, channel->state, actual_level, required_level); 2467 2468 switch (channel->state){ 2469 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2470 if (actual_level >= required_level){ 2471 l2cap_handle_security_level_incoming_sufficient(channel); 2472 } else { 2473 channel->reason = 0x0003; // security block 2474 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2475 } 2476 break; 2477 2478 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2479 if (actual_level >= required_level){ 2480 l2cap_ready_to_connect(channel); 2481 } else { 2482 // security level insufficient, report error and free channel 2483 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY); 2484 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2485 l2cap_free_channel_entry(channel); 2486 } 2487 break; 2488 2489 default: 2490 break; 2491 } 2492 } 2493 } 2494 #endif 2495 2496 #ifdef L2CAP_USES_CHANNELS 2497 static void l2cap_handle_disconnection_complete(hci_con_handle_t handle){ 2498 // collect channels to close 2499 btstack_linked_list_t channels_to_close = NULL; 2500 btstack_linked_list_iterator_t it; 2501 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2502 while (btstack_linked_list_iterator_has_next(&it)) { 2503 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2504 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2505 if (channel->con_handle != handle) continue; 2506 btstack_linked_list_iterator_remove(&it); 2507 btstack_linked_list_add(&channels_to_close, (btstack_linked_item_t *) channel); 2508 } 2509 // send l2cap open failed or closed events for all channels on this handle and free them 2510 btstack_linked_list_iterator_init(&it, &channels_to_close); 2511 while (btstack_linked_list_iterator_has_next(&it)) { 2512 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2513 btstack_linked_list_iterator_remove(&it); 2514 switch(channel->channel_type){ 2515 #ifdef ENABLE_CLASSIC 2516 case L2CAP_CHANNEL_TYPE_CLASSIC: 2517 l2cap_handle_hci_disconnect_event(channel); 2518 break; 2519 #endif 2520 #ifdef ENABLE_LE_DATA_CHANNELS 2521 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2522 l2cap_handle_hci_le_disconnect_event(channel); 2523 break; 2524 #endif 2525 default: 2526 break; 2527 } 2528 } 2529 } 2530 #endif 2531 2532 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 2533 2534 UNUSED(packet_type); // ok: registered with hci_event_callback_registration 2535 UNUSED(cid); // ok: there is no channel 2536 UNUSED(size); // ok: fixed format events read from HCI buffer 2537 2538 #ifdef ENABLE_CLASSIC 2539 bd_addr_t address; 2540 gap_security_level_t security_level; 2541 #endif 2542 #ifdef L2CAP_USES_CHANNELS 2543 hci_con_handle_t handle; 2544 #endif 2545 2546 switch(hci_event_packet_get_type(packet)){ 2547 2548 // Notify channel packet handler if they can send now 2549 case HCI_EVENT_TRANSPORT_PACKET_SENT: 2550 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 2551 case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED: 2552 l2cap_run(); // try sending signaling packets first 2553 l2cap_notify_channel_can_send(); 2554 break; 2555 2556 case HCI_EVENT_COMMAND_STATUS: 2557 #ifdef ENABLE_CLASSIC 2558 // check command status for create connection for errors 2559 if (HCI_EVENT_IS_COMMAND_STATUS(packet, hci_create_connection)){ 2560 // cache outgoing address and reset 2561 (void)memcpy(address, l2cap_outgoing_classic_addr, 6); 2562 memset(l2cap_outgoing_classic_addr, 0, 6); 2563 // error => outgoing connection failed 2564 uint8_t status = hci_event_command_status_get_status(packet); 2565 if (status){ 2566 l2cap_handle_connection_failed_for_addr(address, status); 2567 } 2568 } 2569 #endif 2570 l2cap_run(); // try sending signaling packets first 2571 break; 2572 2573 #ifdef ENABLE_CLASSIC 2574 // handle connection complete events 2575 case HCI_EVENT_CONNECTION_COMPLETE: 2576 reverse_bd_addr(&packet[5], address); 2577 if (packet[2] == 0){ 2578 handle = little_endian_read_16(packet, 3); 2579 l2cap_handle_connection_success_for_addr(address, handle); 2580 } else { 2581 l2cap_handle_connection_failed_for_addr(address, packet[2]); 2582 } 2583 break; 2584 2585 // handle successful create connection cancel command 2586 case HCI_EVENT_COMMAND_COMPLETE: 2587 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 2588 if (packet[5] == 0){ 2589 reverse_bd_addr(&packet[6], address); 2590 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 2591 l2cap_handle_connection_failed_for_addr(address, 0x16); 2592 } 2593 } 2594 l2cap_run(); // try sending signaling packets first 2595 break; 2596 #endif 2597 2598 #ifdef L2CAP_USES_CHANNELS 2599 // handle disconnection complete events 2600 case HCI_EVENT_DISCONNECTION_COMPLETE: 2601 handle = little_endian_read_16(packet, 3); 2602 l2cap_handle_disconnection_complete(handle); 2603 break; 2604 #endif 2605 2606 // HCI Connection Timeouts 2607 #ifdef ENABLE_CLASSIC 2608 case L2CAP_EVENT_TIMEOUT_CHECK: 2609 handle = little_endian_read_16(packet, 2); 2610 l2cap_check_classic_timeout(handle); 2611 break; 2612 2613 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 2614 case HCI_EVENT_READ_REMOTE_EXTENDED_FEATURES_COMPLETE: 2615 handle = little_endian_read_16(packet, 3); 2616 l2cap_handle_features_complete(handle); 2617 break; 2618 2619 case GAP_EVENT_SECURITY_LEVEL: 2620 handle = little_endian_read_16(packet, 2); 2621 security_level = (gap_security_level_t) packet[4]; 2622 l2cap_handle_security_level(handle, security_level); 2623 break; 2624 #endif 2625 default: 2626 break; 2627 } 2628 2629 l2cap_run(); 2630 } 2631 2632 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 2633 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 2634 if (l2cap_signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 2635 l2cap_signaling_responses[l2cap_signaling_responses_pending].handle = handle; 2636 l2cap_signaling_responses[l2cap_signaling_responses_pending].code = code; 2637 l2cap_signaling_responses[l2cap_signaling_responses_pending].sig_id = sig_id; 2638 l2cap_signaling_responses[l2cap_signaling_responses_pending].cid = cid; 2639 l2cap_signaling_responses[l2cap_signaling_responses_pending].data = data; 2640 l2cap_signaling_responses_pending++; 2641 l2cap_run(); 2642 } 2643 } 2644 2645 #ifdef ENABLE_CLASSIC 2646 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 2647 switch (channel->state){ 2648 case L2CAP_STATE_CONFIG: 2649 case L2CAP_STATE_OPEN: 2650 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2651 case L2CAP_STATE_WAIT_DISCONNECT: 2652 break; 2653 default: 2654 // ignore in other states 2655 return; 2656 } 2657 2658 channel->remote_sig_id = identifier; 2659 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2660 l2cap_run(); 2661 } 2662 2663 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 2664 2665 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 2666 l2cap_service_t *service = l2cap_get_service(psm); 2667 if (!service) { 2668 // 0x0002 PSM not supported 2669 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2670 return; 2671 } 2672 2673 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 2674 if (!hci_connection) { 2675 // 2676 log_error("no hci_connection for handle %u", handle); 2677 return; 2678 } 2679 2680 // alloc structure 2681 gap_security_level_t required_level = service->required_security_level; 2682 if (gap_get_secure_connections_only_mode() && (required_level != LEVEL_0)){ 2683 required_level = LEVEL_4; 2684 } 2685 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CLASSIC, hci_connection->address, BD_ADDR_TYPE_ACL, 2686 psm, service->mtu, required_level); 2687 if (!channel){ 2688 // 0x0004 No resources available 2689 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2690 return; 2691 } 2692 2693 channel->con_handle = handle; 2694 channel->remote_cid = source_cid; 2695 channel->remote_sig_id = sig_id; 2696 2697 // limit local mtu to max acl packet length - l2cap header 2698 if (channel->local_mtu > l2cap_max_mtu()) { 2699 channel->local_mtu = l2cap_max_mtu(); 2700 } 2701 2702 // set initial state 2703 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 2704 channel->state_var = L2CAP_CHANNEL_STATE_VAR_INCOMING; 2705 2706 // add to connections list 2707 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2708 2709 // 2710 if (required_level > LEVEL_0){ 2711 // send conn resp pending if remote supported features have not been received yet 2712 if (hci_remote_features_available(handle)) { 2713 l2cap_handle_remote_supported_features_received(channel); 2714 } else { 2715 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND; 2716 hci_remote_features_query(handle); 2717 } 2718 } else { 2719 l2cap_handle_security_level_incoming_sufficient(channel); 2720 } 2721 } 2722 2723 void l2cap_accept_connection(uint16_t local_cid){ 2724 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 2725 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2726 if (!channel) { 2727 log_error("accept called but local_cid 0x%x not found", local_cid); 2728 return; 2729 } 2730 2731 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2732 // configure L2CAP Basic mode 2733 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2734 #endif 2735 2736 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 2737 2738 // process 2739 l2cap_run(); 2740 } 2741 2742 void l2cap_decline_connection(uint16_t local_cid){ 2743 log_info("decline local_cid 0x%x", local_cid); 2744 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 2745 if (!channel) { 2746 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 2747 return; 2748 } 2749 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2750 channel->reason = 0x04; // no resources available 2751 l2cap_run(); 2752 } 2753 2754 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2755 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 2756 2757 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2758 uint8_t use_fcs = 1; 2759 #endif 2760 2761 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2762 2763 uint16_t flags = little_endian_read_16(command, 6); 2764 if (flags & 1) { 2765 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 2766 } 2767 2768 // accept the other's configuration options 2769 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2770 uint16_t pos = 8; 2771 while (pos < end_pos){ 2772 uint8_t option_hint = command[pos] >> 7; 2773 uint8_t option_type = command[pos] & 0x7f; 2774 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2775 pos++; 2776 uint8_t length = command[pos++]; 2777 // MTU { type(8): 1, len(8):2, MTU(16) } 2778 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) && (length == 2)){ 2779 channel->remote_mtu = little_endian_read_16(command, pos); 2780 log_info("Remote MTU %u", channel->remote_mtu); 2781 if (channel->remote_mtu > l2cap_max_mtu()){ 2782 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 2783 channel->remote_mtu = l2cap_max_mtu(); 2784 } 2785 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2786 } 2787 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2788 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_FLUSH_TIMEOUT) && (length == 2)){ 2789 channel->flush_timeout = little_endian_read_16(command, pos); 2790 log_info("Flush timeout: %u ms", channel->flush_timeout); 2791 } 2792 2793 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2794 // Retransmission and Flow Control Option 2795 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2796 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2797 switch(channel->mode){ 2798 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2799 // Store remote config 2800 channel->remote_tx_window_size = command[pos+1]; 2801 channel->remote_max_transmit = command[pos+2]; 2802 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2803 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2804 channel->remote_mps = little_endian_read_16(command, pos + 7); 2805 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2806 channel->remote_tx_window_size, 2807 channel->remote_max_transmit, 2808 channel->remote_retransmission_timeout_ms, 2809 channel->remote_monitor_timeout_ms, 2810 channel->remote_mps); 2811 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2812 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2813 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2814 } else { 2815 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2816 } 2817 break; 2818 case L2CAP_CHANNEL_MODE_BASIC: 2819 switch (mode){ 2820 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2821 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2822 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2823 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2824 } 2825 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2826 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2827 break; 2828 default: // case L2CAP_CHANNEL_MODE_BASIC: 2829 // TODO store and evaluate configuration 2830 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2831 break; 2832 } 2833 break; 2834 default: 2835 break; 2836 } 2837 } 2838 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE && length == 1){ 2839 use_fcs = command[pos]; 2840 } 2841 #endif 2842 // check for unknown options 2843 if ((option_hint == 0) && ((option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) || (option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE))){ 2844 log_info("l2cap cid %u, unknown option 0x%02x", channel->local_cid, option_type); 2845 channel->unknown_option = option_type; 2846 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2847 } 2848 pos += length; 2849 } 2850 2851 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2852 // "FCS" has precedence over "No FCS" 2853 uint8_t update = channel->fcs_option || use_fcs; 2854 log_info("local fcs: %u, remote fcs: %u -> %u", channel->fcs_option, use_fcs, update); 2855 channel->fcs_option = update; 2856 // If ERTM mandatory, but remote didn't send Retransmission and Flowcontrol options -> disconnect 2857 if (((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM) == 0) & (channel->ertm_mandatory)){ 2858 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2859 } 2860 #endif 2861 } 2862 2863 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2864 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2865 log_info("l2cap_signaling_handle_configure_response"); 2866 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2867 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2868 uint16_t pos = 10; 2869 while (pos < end_pos){ 2870 uint8_t option_hint = command[pos] >> 7; 2871 uint8_t option_type = command[pos] & 0x7f; 2872 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2873 pos++; 2874 uint8_t length = command[pos++]; 2875 2876 // Retransmission and Flow Control Option 2877 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2878 switch (channel->mode){ 2879 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2880 if (channel->ertm_mandatory){ 2881 // ?? 2882 } else { 2883 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2884 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2885 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2886 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2887 } 2888 } 2889 break; 2890 case L2CAP_CHANNEL_MODE_BASIC: 2891 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2892 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2893 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2894 } 2895 break; 2896 default: 2897 break; 2898 } 2899 } 2900 2901 // check for unknown options 2902 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2903 log_info("l2cap cid %u, unknown option 0x%02x", channel->local_cid, option_type); 2904 channel->unknown_option = option_type; 2905 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2906 } 2907 2908 pos += length; 2909 } 2910 #else 2911 UNUSED(channel); // ok: no code 2912 UNUSED(result); // ok: no code 2913 UNUSED(command); // ok: no code 2914 #endif 2915 } 2916 2917 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2918 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2919 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2920 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2921 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2922 if (channel->state == L2CAP_STATE_OPEN) return 0; 2923 return 1; 2924 } 2925 2926 2927 // @pre command len is valid, see check in l2cap_signaling_handler_dispatch 2928 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2929 2930 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2931 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2932 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2933 uint16_t result = 0; 2934 2935 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2936 2937 // handle DISCONNECT REQUESTS seperately 2938 if (code == DISCONNECTION_REQUEST){ 2939 l2cap_handle_disconnect_request(channel, identifier); 2940 return; 2941 } 2942 2943 // @STATEMACHINE(l2cap) 2944 switch (channel->state) { 2945 2946 case L2CAP_STATE_WAIT_CONNECT_RSP: 2947 switch (code){ 2948 case CONNECTION_RESPONSE: 2949 if (cmd_len < 8){ 2950 // command imcomplete 2951 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2952 break; 2953 } 2954 l2cap_stop_rtx(channel); 2955 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2956 switch (result) { 2957 case 0: 2958 // successful connection 2959 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2960 channel->state = L2CAP_STATE_CONFIG; 2961 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2962 break; 2963 case 1: 2964 // connection pending. get some coffee, but start the ERTX 2965 l2cap_start_ertx(channel); 2966 break; 2967 default: 2968 // channel closed 2969 channel->state = L2CAP_STATE_CLOSED; 2970 // map l2cap connection response result to BTstack status enumeration 2971 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2972 2973 // drop link key if security block 2974 if ((L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result) == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2975 gap_drop_link_key_for_bd_addr(channel->address); 2976 } 2977 2978 // discard channel 2979 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2980 l2cap_free_channel_entry(channel); 2981 break; 2982 } 2983 break; 2984 2985 default: 2986 //@TODO: implement other signaling packets 2987 break; 2988 } 2989 break; 2990 2991 case L2CAP_STATE_CONFIG: 2992 switch (code) { 2993 case CONFIGURE_REQUEST: 2994 if (cmd_len < 4){ 2995 // command incomplete 2996 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2997 break; 2998 } 2999 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 3000 l2cap_signaling_handle_configure_request(channel, command); 3001 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 3002 // only done if continuation not set 3003 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 3004 } 3005 break; 3006 case CONFIGURE_RESPONSE: 3007 if (cmd_len < 6){ 3008 // command incomplete 3009 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 3010 break; 3011 } 3012 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3013 l2cap_stop_rtx(channel); 3014 l2cap_signaling_handle_configure_response(channel, result, command); 3015 switch (result){ 3016 case 0: // success 3017 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 3018 break; 3019 case 4: // pending 3020 l2cap_start_ertx(channel); 3021 break; 3022 default: 3023 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3024 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 3025 // remote does not offer ertm but it's required 3026 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3027 break; 3028 } 3029 #endif 3030 // retry on negative result 3031 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 3032 break; 3033 } 3034 break; 3035 default: 3036 break; 3037 } 3038 if (l2cap_channel_ready_for_open(channel)){ 3039 3040 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3041 // assert that packet can be stored in fragment buffers in ertm 3042 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3043 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 3044 uint16_t usable_mtu = channel->num_tx_buffers == 1 ? effective_mps : channel->num_tx_buffers * effective_mps - 2; 3045 if (usable_mtu < channel->remote_mtu){ 3046 log_info("Remote MTU %u > max storable ERTM packet, only using MTU = %u", channel->remote_mtu, usable_mtu); 3047 channel->remote_mtu = usable_mtu; 3048 } 3049 } 3050 #endif 3051 // for open: 3052 channel->state = L2CAP_STATE_OPEN; 3053 l2cap_emit_channel_opened(channel, 0); 3054 } 3055 break; 3056 3057 case L2CAP_STATE_WAIT_DISCONNECT: 3058 switch (code) { 3059 case DISCONNECTION_RESPONSE: 3060 l2cap_finialize_channel_close(channel); 3061 break; 3062 default: 3063 //@TODO: implement other signaling packets 3064 break; 3065 } 3066 break; 3067 3068 case L2CAP_STATE_CLOSED: 3069 // @TODO handle incoming requests 3070 break; 3071 3072 case L2CAP_STATE_OPEN: 3073 //@TODO: implement other signaling packets, e.g. re-configure 3074 break; 3075 default: 3076 break; 3077 } 3078 // log_info("new state %u", channel->state); 3079 } 3080 3081 static void l2cap_handle_information_request_complete(hci_connection_t * connection){ 3082 3083 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 3084 3085 // trigger connection request 3086 btstack_linked_list_iterator_t it; 3087 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3088 while (btstack_linked_list_iterator_has_next(&it)){ 3089 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3090 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3091 if (channel->con_handle != connection->con_handle) continue; 3092 3093 // incoming connection: information request was triggered after user has accepted connection, 3094 // now: verify channel configuration, esp. if ertm will be mandatory 3095 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 3096 // default: continue 3097 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 3098 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 3099 // ERTM not possible, select basic mode and release buffer 3100 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 3101 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 3102 3103 // bail if ERTM is mandatory 3104 if (channel->ertm_mandatory){ 3105 // We chose 'no resources available' for "ERTM mandatory but you don't even know ERTM exists" 3106 log_info("ERTM mandatory -> reject connection"); 3107 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 3108 channel->reason = 0x04; // no resources available 3109 } else { 3110 log_info("ERTM not supported by remote -> use Basic mode"); 3111 } 3112 } 3113 continue; 3114 } 3115 3116 // outgoing connection: information request is triggered before connection request 3117 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 3118 3119 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3120 // if ERTM was requested, but is not listed in extended feature mask: 3121 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 3122 3123 if (channel->ertm_mandatory){ 3124 // bail if ERTM is mandatory 3125 channel->state = L2CAP_STATE_CLOSED; 3126 // map l2cap connection response result to BTstack status enumeration 3127 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 3128 // discard channel 3129 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3130 l2cap_free_channel_entry(channel); 3131 continue; 3132 3133 } else { 3134 // fallback to Basic mode 3135 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 3136 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 3137 } 3138 } 3139 #endif 3140 3141 // respond to connection request 3142 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 3143 continue; 3144 } 3145 } 3146 } 3147 3148 3149 // @pre command len is valid, see check in l2cap_acl_classic_handler 3150 static void l2cap_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command){ 3151 3152 hci_connection_t * connection; 3153 3154 // get code, signal identifier and command len 3155 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3156 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 3157 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3158 3159 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 3160 if ((code < 1) || (code == ECHO_RESPONSE) || (code > INFORMATION_RESPONSE)){ 3161 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3162 return; 3163 } 3164 3165 // general commands without an assigned channel 3166 switch(code) { 3167 3168 case CONNECTION_REQUEST: 3169 if (cmd_len == 4){ 3170 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3171 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3172 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 3173 } else { 3174 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3175 } 3176 return; 3177 3178 case ECHO_REQUEST: 3179 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 3180 return; 3181 3182 case INFORMATION_REQUEST: 3183 if (cmd_len == 2) { 3184 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3185 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 3186 } else { 3187 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3188 } 3189 return; 3190 3191 case INFORMATION_RESPONSE: 3192 connection = hci_connection_for_handle(handle); 3193 if (!connection) return; 3194 switch (connection->l2cap_state.information_state){ 3195 case L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE: 3196 // get extended features from response if valid 3197 if (cmd_len >= 6) { 3198 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3199 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3200 if (result == 0 && info_type == L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) { 3201 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3202 } 3203 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 3204 if ((connection->l2cap_state.extended_feature_mask & 0x80) != 0){ 3205 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_FIXED_CHANNELS_REQUEST; 3206 } else { 3207 // information request complete 3208 l2cap_handle_information_request_complete(connection); 3209 } 3210 } 3211 break; 3212 case L2CAP_INFORMATION_STATE_W4_FIXED_CHANNELS_RESPONSE: 3213 if (cmd_len >= 12) { 3214 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3215 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3216 if (result == 0 && info_type == L2CAP_INFO_TYPE_FIXED_CHANNELS_SUPPORTED) { 3217 connection->l2cap_state.fixed_channels_supported = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3218 } 3219 log_info("fixed channels mask 0x%02x", connection->l2cap_state.fixed_channels_supported); 3220 // information request complete 3221 l2cap_handle_information_request_complete(connection); 3222 } 3223 break; 3224 default: 3225 break; 3226 } 3227 return; 3228 3229 default: 3230 break; 3231 } 3232 3233 // Get potential destination CID 3234 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3235 3236 // Find channel for this sig_id and connection handle 3237 btstack_linked_list_iterator_t it; 3238 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3239 while (btstack_linked_list_iterator_has_next(&it)){ 3240 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3241 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3242 if (channel->con_handle != handle) continue; 3243 if (code & 1) { 3244 // match odd commands (responses) by previous signaling identifier 3245 if (channel->local_sig_id == sig_id) { 3246 l2cap_signaling_handler_channel(channel, command); 3247 break; 3248 } 3249 } else { 3250 // match even commands (requests) by local channel id 3251 if (channel->local_cid == dest_cid) { 3252 l2cap_signaling_handler_channel(channel, command); 3253 break; 3254 } 3255 } 3256 } 3257 } 3258 #endif 3259 3260 #ifdef ENABLE_BLE 3261 3262 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 3263 uint8_t event[6]; 3264 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 3265 event[1] = 4; 3266 little_endian_store_16(event, 2, con_handle); 3267 little_endian_store_16(event, 4, result); 3268 l2cap_emit_event(event, sizeof(event)); 3269 } 3270 3271 // @return valid 3272 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 3273 hci_connection_t * connection; 3274 uint16_t result; 3275 uint8_t event[12]; 3276 3277 #ifdef ENABLE_LE_DATA_CHANNELS 3278 btstack_linked_list_iterator_t it; 3279 l2cap_channel_t * channel; 3280 uint16_t local_cid; 3281 uint16_t le_psm; 3282 uint16_t new_credits; 3283 uint16_t credits_before; 3284 l2cap_service_t * service; 3285 uint16_t source_cid; 3286 #endif 3287 3288 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3289 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3290 log_info("le dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 3291 3292 switch (code){ 3293 3294 case CONNECTION_PARAMETER_UPDATE_REQUEST: 3295 // check size 3296 if (len < 8u) return 0u; 3297 connection = hci_connection_for_handle(handle); 3298 if (connection != NULL){ 3299 if (connection->role != HCI_ROLE_MASTER){ 3300 // reject command without notifying upper layer when not in master role 3301 return 0; 3302 } 3303 le_connection_parameter_range_t existing_range; 3304 gap_get_connection_parameter_range(&existing_range); 3305 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3306 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3307 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3308 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 3309 3310 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 3311 if (update_parameter){ 3312 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 3313 connection->le_conn_interval_min = le_conn_interval_min; 3314 connection->le_conn_interval_max = le_conn_interval_max; 3315 connection->le_conn_latency = le_conn_latency; 3316 connection->le_supervision_timeout = le_supervision_timeout; 3317 } else { 3318 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 3319 } 3320 connection->le_con_param_update_identifier = sig_id; 3321 } 3322 3323 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 3324 event[1] = 8; 3325 little_endian_store_16(event, 2, handle); 3326 (void)memcpy(&event[4], &command[4], 8); 3327 l2cap_emit_event(event, sizeof(event)); 3328 break; 3329 3330 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 3331 // check size 3332 if (len < 2u) return 0u; 3333 result = little_endian_read_16(command, 4); 3334 l2cap_emit_connection_parameter_update_response(handle, result); 3335 break; 3336 3337 #ifdef ENABLE_LE_DATA_CHANNELS 3338 3339 case COMMAND_REJECT: 3340 // Find channel for this sig_id and connection handle 3341 channel = NULL; 3342 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3343 while (btstack_linked_list_iterator_has_next(&it)){ 3344 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3345 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3346 if (a_channel->con_handle != handle) continue; 3347 if (a_channel->local_sig_id != sig_id) continue; 3348 channel = a_channel; 3349 break; 3350 } 3351 if (!channel) break; 3352 3353 // if received while waiting for le connection response, assume legacy device 3354 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 3355 channel->state = L2CAP_STATE_CLOSED; 3356 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 3357 l2cap_emit_le_channel_opened(channel, 0x0002); 3358 3359 // discard channel 3360 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3361 l2cap_free_channel_entry(channel); 3362 break; 3363 } 3364 break; 3365 3366 case LE_CREDIT_BASED_CONNECTION_REQUEST: 3367 // check size 3368 if (len < 10u) return 0u; 3369 3370 // get hci connection, bail if not found (must not happen) 3371 connection = hci_connection_for_handle(handle); 3372 if (!connection) return 0; 3373 3374 // check if service registered 3375 le_psm = little_endian_read_16(command, 4); 3376 service = l2cap_le_get_service(le_psm); 3377 source_cid = little_endian_read_16(command, 6); 3378 3379 if (service){ 3380 if (source_cid < 0x40u){ 3381 // 0x0009 Connection refused - Invalid Source CID 3382 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 3383 return 1; 3384 } 3385 3386 // go through list of channels for this ACL connection and check if we get a match 3387 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3388 while (btstack_linked_list_iterator_has_next(&it)){ 3389 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3390 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3391 if (a_channel->con_handle != handle) continue; 3392 if (a_channel->remote_cid != source_cid) continue; 3393 // 0x000a Connection refused - Source CID already allocated 3394 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 3395 return 1; 3396 } 3397 3398 // security: check encryption 3399 if (service->required_security_level >= LEVEL_2){ 3400 if (gap_encryption_key_size(handle) == 0){ 3401 // 0x0008 Connection refused - insufficient encryption 3402 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 3403 return 1; 3404 } 3405 // anything less than 16 byte key size is insufficient 3406 if (gap_encryption_key_size(handle) < 16){ 3407 // 0x0007 Connection refused – insufficient encryption key size 3408 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 3409 return 1; 3410 } 3411 } 3412 3413 // security: check authencation 3414 if (service->required_security_level >= LEVEL_3){ 3415 if (!gap_authenticated(handle)){ 3416 // 0x0005 Connection refused – insufficient authentication 3417 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 3418 return 1; 3419 } 3420 } 3421 3422 // security: check authorization 3423 if (service->required_security_level >= LEVEL_4){ 3424 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 3425 // 0x0006 Connection refused – insufficient authorization 3426 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 3427 return 1; 3428 } 3429 } 3430 3431 // allocate channel 3432 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 3433 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 3434 if (!channel){ 3435 // 0x0004 Connection refused – no resources available 3436 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 3437 return 1; 3438 } 3439 3440 channel->con_handle = handle; 3441 channel->remote_cid = source_cid; 3442 channel->remote_sig_id = sig_id; 3443 channel->remote_mtu = little_endian_read_16(command, 8); 3444 channel->remote_mps = little_endian_read_16(command, 10); 3445 channel->credits_outgoing = little_endian_read_16(command, 12); 3446 3447 // set initial state 3448 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3449 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3450 3451 // add to connections list 3452 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 3453 3454 // post connection request event 3455 l2cap_emit_le_incoming_connection(channel); 3456 3457 } else { 3458 // Connection refused – LE_PSM not supported 3459 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 3460 } 3461 break; 3462 3463 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 3464 // check size 3465 if (len < 10u) return 0u; 3466 3467 // Find channel for this sig_id and connection handle 3468 channel = NULL; 3469 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3470 while (btstack_linked_list_iterator_has_next(&it)){ 3471 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3472 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3473 if (a_channel->con_handle != handle) continue; 3474 if (a_channel->local_sig_id != sig_id) continue; 3475 channel = a_channel; 3476 break; 3477 } 3478 if (!channel) break; 3479 3480 // cid + 0 3481 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 3482 if (result){ 3483 channel->state = L2CAP_STATE_CLOSED; 3484 // map l2cap connection response result to BTstack status enumeration 3485 l2cap_emit_le_channel_opened(channel, result); 3486 3487 // discard channel 3488 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3489 l2cap_free_channel_entry(channel); 3490 break; 3491 } 3492 3493 // success 3494 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3495 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3496 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3497 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3498 channel->state = L2CAP_STATE_OPEN; 3499 l2cap_emit_le_channel_opened(channel, result); 3500 break; 3501 3502 case LE_FLOW_CONTROL_CREDIT: 3503 // check size 3504 if (len < 4u) return 0u; 3505 3506 // find channel 3507 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3508 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3509 if (!channel) { 3510 log_error("credit: no channel for cid 0x%02x", local_cid); 3511 break; 3512 } 3513 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3514 credits_before = channel->credits_outgoing; 3515 channel->credits_outgoing += new_credits; 3516 // check for credit overrun 3517 if (credits_before > channel->credits_outgoing){ 3518 log_error("credit: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 3519 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3520 break; 3521 } 3522 log_info("credit: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 3523 break; 3524 3525 case DISCONNECTION_REQUEST: 3526 3527 // check size 3528 if (len < 4u) return 0u; 3529 3530 // find channel 3531 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3532 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3533 if (!channel) { 3534 log_error("credit: no channel for cid 0x%02x", local_cid); 3535 break; 3536 } 3537 channel->remote_sig_id = sig_id; 3538 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3539 break; 3540 3541 #endif 3542 3543 case DISCONNECTION_RESPONSE: 3544 break; 3545 3546 default: 3547 // command unknown -> reject command 3548 return 0; 3549 } 3550 return 1; 3551 } 3552 #endif 3553 3554 #ifdef ENABLE_CLASSIC 3555 static void l2cap_acl_classic_handler_for_channel(l2cap_channel_t * l2cap_channel, uint8_t * packet, uint16_t size){ 3556 3557 // forward data only in OPEN state 3558 if (l2cap_channel->state != L2CAP_STATE_OPEN) return; 3559 3560 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3561 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3562 3563 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3564 3565 // assert control + FCS fields are inside 3566 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) return; 3567 3568 if (l2cap_channel->fcs_option){ 3569 // verify FCS (required if one side requested it) 3570 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3571 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3572 3573 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 3574 // simulate fcs error 3575 static int counter = 0; 3576 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 3577 log_info("Simulate fcs error"); 3578 fcs_calculated++; 3579 counter = 0; 3580 } 3581 #endif 3582 3583 if (fcs_calculated == fcs_packet){ 3584 log_info("Packet FCS 0x%04x verified", fcs_packet); 3585 } else { 3586 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3587 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 3588 return; 3589 } 3590 } 3591 3592 // switch on packet type 3593 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3594 uint8_t req_seq = (control >> 8) & 0x3f; 3595 int final = (control >> 7) & 0x01; 3596 if (control & 1){ 3597 // S-Frame 3598 int poll = (control >> 4) & 0x01; 3599 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3600 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3601 l2cap_ertm_tx_packet_state_t * tx_state; 3602 switch (s){ 3603 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3604 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3605 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3606 if (poll && final){ 3607 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3608 log_error("P=F=1 in S-Frame"); 3609 break; 3610 } 3611 if (poll){ 3612 // check if we did request selective retransmission before <==> we have stored SDU segments 3613 int i; 3614 int num_stored_out_of_order_packets = 0; 3615 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3616 int index = l2cap_channel->rx_store_index + i; 3617 if (index >= l2cap_channel->num_rx_buffers){ 3618 index -= l2cap_channel->num_rx_buffers; 3619 } 3620 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3621 if (!rx_state->valid) continue; 3622 num_stored_out_of_order_packets++; 3623 } 3624 if (num_stored_out_of_order_packets){ 3625 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3626 } else { 3627 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3628 } 3629 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3630 } 3631 if (final){ 3632 // Stop-MonitorTimer 3633 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3634 // If UnackedFrames > 0 then Start-RetransTimer 3635 if (l2cap_channel->unacked_frames){ 3636 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3637 } 3638 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3639 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3640 } 3641 break; 3642 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3643 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3644 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3645 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3646 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3647 break; 3648 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3649 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3650 break; 3651 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3652 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3653 if (poll){ 3654 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3655 } 3656 // find requested i-frame 3657 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3658 if (tx_state){ 3659 log_info("Retransmission for tx_seq %u requested", req_seq); 3660 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3661 tx_state->retransmission_requested = 1; 3662 l2cap_channel->srej_active = 1; 3663 } 3664 break; 3665 default: 3666 break; 3667 } 3668 } else { 3669 // I-Frame 3670 // get control 3671 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3672 uint8_t tx_seq = (control >> 1) & 0x3f; 3673 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3674 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3675 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3676 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3677 if (final){ 3678 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3679 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3680 } 3681 3682 // get SDU 3683 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3684 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3685 3686 // assert SDU size is smaller or equal to our buffers 3687 uint16_t max_payload_size = 0; 3688 switch (sar){ 3689 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 3690 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 3691 // SDU Length + MPS 3692 max_payload_size = l2cap_channel->local_mps + 2; 3693 break; 3694 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 3695 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 3696 max_payload_size = l2cap_channel->local_mps; 3697 break; 3698 default: 3699 btstack_assert(false); 3700 break; 3701 } 3702 if (payload_len > max_payload_size){ 3703 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 3704 return; 3705 } 3706 3707 // check ordering 3708 if (l2cap_channel->expected_tx_seq == tx_seq){ 3709 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3710 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3711 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3712 3713 // process SDU 3714 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 3715 3716 // process stored segments 3717 while (true){ 3718 int index = l2cap_channel->rx_store_index; 3719 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3720 if (!rx_state->valid) break; 3721 3722 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3723 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3724 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3725 3726 rx_state->valid = 0; 3727 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3728 3729 // update rx store index 3730 index++; 3731 if (index >= l2cap_channel->num_rx_buffers){ 3732 index = 0; 3733 } 3734 l2cap_channel->rx_store_index = index; 3735 } 3736 3737 // 3738 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3739 3740 } else { 3741 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3742 if (delta < 2){ 3743 // store segment 3744 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 3745 3746 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3747 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3748 } else { 3749 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3750 l2cap_channel->send_supervisor_frame_reject = 1; 3751 } 3752 } 3753 } 3754 return; 3755 } 3756 #endif 3757 3758 // check size 3759 uint16_t payload_size = size - COMPLETE_L2CAP_HEADER; 3760 if (l2cap_channel->local_mtu < payload_size) return; 3761 3762 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], payload_size); 3763 } 3764 #endif 3765 3766 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3767 #ifdef ENABLE_CLASSIC 3768 l2cap_channel_t * l2cap_channel; 3769 l2cap_fixed_channel_t * l2cap_fixed_channel; 3770 3771 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3772 uint8_t broadcast_flag = READ_ACL_FLAGS(packet) >> 2; 3773 switch (channel_id) { 3774 3775 case L2CAP_CID_SIGNALING: { 3776 if (broadcast_flag != 0) break; 3777 uint32_t command_offset = 8; 3778 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 3779 // assert signaling command is fully inside packet 3780 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3781 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3782 if (next_command_offset > size){ 3783 log_error("l2cap signaling command len invalid -> drop"); 3784 break; 3785 } 3786 // handle signaling command 3787 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3788 // go to next command 3789 command_offset = next_command_offset; 3790 } 3791 break; 3792 } 3793 3794 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3795 if (broadcast_flag == 0) break; 3796 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3797 if (!l2cap_fixed_channel) break; 3798 if (!l2cap_fixed_channel->packet_handler) break; 3799 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3800 break; 3801 3802 #ifdef ENABLE_BLE 3803 case L2CAP_CID_BR_EDR_SECURITY_MANAGER: 3804 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3805 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 3806 // Pairing Failed 3807 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_BR_EDR_SECURITY_MANAGER, SM_REASON_PAIRING_NOT_SUPPORTED); 3808 } else { 3809 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3810 } 3811 break; 3812 #endif 3813 3814 default: 3815 if (broadcast_flag != 0) break; 3816 // Find channel for this channel_id and connection handle 3817 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3818 if (l2cap_channel != NULL){ 3819 l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size); 3820 } 3821 break; 3822 } 3823 #else 3824 UNUSED(handle); // ok: no code 3825 UNUSED(packet); // ok: no code 3826 UNUSED(size); // ok: no code 3827 #endif 3828 } 3829 3830 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3831 #ifdef ENABLE_BLE 3832 3833 l2cap_fixed_channel_t * l2cap_fixed_channel; 3834 3835 #ifdef ENABLE_LE_DATA_CHANNELS 3836 l2cap_channel_t * l2cap_channel; 3837 #endif 3838 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3839 switch (channel_id) { 3840 3841 case L2CAP_CID_SIGNALING_LE: { 3842 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3843 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3844 if ((COMPLETE_L2CAP_HEADER + 4u + len) > size) break; 3845 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3846 if (!valid){ 3847 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3848 } 3849 break; 3850 } 3851 3852 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3853 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3854 if (!l2cap_fixed_channel) break; 3855 if (!l2cap_fixed_channel->packet_handler) break; 3856 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3857 break; 3858 3859 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3860 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3861 if ((l2cap_fixed_channel == NULL) || (l2cap_fixed_channel->packet_handler == NULL)){ 3862 // Pairing Failed 3863 l2cap_register_signaling_response(handle, (uint8_t) SM_PAIRING_FAILED, 0, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, SM_REASON_PAIRING_NOT_SUPPORTED); 3864 } else { 3865 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3866 } 3867 break; 3868 3869 default: 3870 3871 #ifdef ENABLE_LE_DATA_CHANNELS 3872 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3873 if (l2cap_channel != NULL) { 3874 // credit counting 3875 if (l2cap_channel->credits_incoming == 0u){ 3876 log_info("LE Data Channel packet received but no incoming credits"); 3877 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3878 break; 3879 } 3880 l2cap_channel->credits_incoming--; 3881 3882 // automatic credits 3883 if ((l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK) && l2cap_channel->automatic_credits){ 3884 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3885 } 3886 3887 // first fragment 3888 uint16_t pos = 0; 3889 if (!l2cap_channel->receive_sdu_len){ 3890 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3891 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3892 l2cap_channel->receive_sdu_len = sdu_len; 3893 l2cap_channel->receive_sdu_pos = 0; 3894 pos += 2u; 3895 size -= 2u; 3896 } 3897 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3898 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3899 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3900 (void)memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], 3901 &packet[COMPLETE_L2CAP_HEADER + pos], 3902 fragment_size); 3903 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3904 // done? 3905 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3906 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3907 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3908 l2cap_channel->receive_sdu_len = 0; 3909 } 3910 } 3911 #endif 3912 break; 3913 } 3914 #else 3915 UNUSED(handle); // ok: no code 3916 UNUSED(packet); // ok: no code 3917 UNUSED(size); // ok: no code 3918 #endif 3919 } 3920 3921 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3922 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3923 UNUSED(channel); // ok: there is no channel 3924 3925 // Assert full L2CAP header present 3926 if (size < COMPLETE_L2CAP_HEADER) return; 3927 3928 // Dispatch to Classic or LE handler (SCO packets are not dispatched to L2CAP) 3929 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3930 hci_connection_t *conn = hci_connection_for_handle(handle); 3931 if (!conn) return; 3932 if (conn->address_type == BD_ADDR_TYPE_ACL){ 3933 l2cap_acl_classic_handler(handle, packet, size); 3934 } else { 3935 l2cap_acl_le_handler(handle, packet, size); 3936 } 3937 3938 l2cap_run(); 3939 } 3940 3941 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3942 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 3943 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3944 if (!channel) return; 3945 channel->packet_handler = packet_handler; 3946 } 3947 3948 #ifdef ENABLE_CLASSIC 3949 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3950 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3951 channel->state = L2CAP_STATE_CLOSED; 3952 l2cap_handle_channel_closed(channel); 3953 // discard channel 3954 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3955 l2cap_free_channel_entry(channel); 3956 } 3957 #endif 3958 3959 #ifdef L2CAP_USES_CHANNELS 3960 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3961 btstack_linked_list_iterator_t it; 3962 btstack_linked_list_iterator_init(&it, services); 3963 while (btstack_linked_list_iterator_has_next(&it)){ 3964 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3965 if ( service->psm == psm){ 3966 return service; 3967 }; 3968 } 3969 return NULL; 3970 } 3971 #endif 3972 3973 #ifdef ENABLE_CLASSIC 3974 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3975 return l2cap_get_service_internal(&l2cap_services, psm); 3976 } 3977 3978 static void l2cap_update_minimal_security_level(void){ 3979 // update minimal service security level 3980 gap_security_level_t minimal_level = LEVEL_1; 3981 btstack_linked_list_iterator_t it; 3982 btstack_linked_list_iterator_init(&it, &l2cap_services); 3983 while (btstack_linked_list_iterator_has_next(&it)){ 3984 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3985 if (service->required_security_level > minimal_level){ 3986 minimal_level = service->required_security_level; 3987 }; 3988 } 3989 gap_set_minimal_service_security_level(minimal_level); 3990 } 3991 3992 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3993 3994 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3995 3996 // check for alread registered psm 3997 l2cap_service_t *service = l2cap_get_service(psm); 3998 if (service) { 3999 log_error("register: PSM %u already registered", psm); 4000 return L2CAP_SERVICE_ALREADY_REGISTERED; 4001 } 4002 4003 // alloc structure 4004 service = btstack_memory_l2cap_service_get(); 4005 if (!service) { 4006 log_error("register: no memory for l2cap_service_t"); 4007 return BTSTACK_MEMORY_ALLOC_FAILED; 4008 } 4009 4010 // fill in 4011 service->psm = psm; 4012 service->mtu = mtu; 4013 service->packet_handler = service_packet_handler; 4014 service->required_security_level = security_level; 4015 4016 // add to services list 4017 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 4018 4019 l2cap_update_minimal_security_level(); 4020 4021 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 4022 // enable page scan 4023 gap_connectable_control(1); 4024 #endif 4025 4026 4027 return ERROR_CODE_SUCCESS; 4028 } 4029 4030 uint8_t l2cap_unregister_service(uint16_t psm){ 4031 4032 log_info("unregister psm 0x%x", psm); 4033 4034 l2cap_service_t *service = l2cap_get_service(psm); 4035 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 4036 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 4037 btstack_memory_l2cap_service_free(service); 4038 4039 l2cap_update_minimal_security_level(); 4040 4041 #ifndef ENABLE_EXPLICIT_CONNECTABLE_MODE_CONTROL 4042 // disable page scan when no services registered 4043 if (btstack_linked_list_empty(&l2cap_services)) { 4044 gap_connectable_control(0); 4045 } 4046 #endif 4047 4048 return ERROR_CODE_SUCCESS; 4049 } 4050 #endif 4051 4052 4053 #ifdef ENABLE_LE_DATA_CHANNELS 4054 4055 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 4056 if (!channel->waiting_for_can_send_now) return; 4057 if (channel->send_sdu_buffer) return; 4058 channel->waiting_for_can_send_now = 0; 4059 log_debug("le can send now, local_cid 0x%x", channel->local_cid); 4060 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 4061 } 4062 4063 // 1BH2222 4064 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 4065 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", 4066 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 4067 uint8_t event[19]; 4068 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 4069 event[1] = sizeof(event) - 2u; 4070 event[2] = channel->address_type; 4071 reverse_bd_addr(channel->address, &event[3]); 4072 little_endian_store_16(event, 9, channel->con_handle); 4073 little_endian_store_16(event, 11, channel->psm); 4074 little_endian_store_16(event, 13, channel->local_cid); 4075 little_endian_store_16(event, 15, channel->remote_cid); 4076 little_endian_store_16(event, 17, channel->remote_mtu); 4077 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4078 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4079 } 4080 // 11BH22222 4081 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 4082 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", 4083 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 4084 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 4085 uint8_t event[23]; 4086 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 4087 event[1] = sizeof(event) - 2u; 4088 event[2] = status; 4089 event[3] = channel->address_type; 4090 reverse_bd_addr(channel->address, &event[4]); 4091 little_endian_store_16(event, 10, channel->con_handle); 4092 event[12] = (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) ? 1 : 0; 4093 little_endian_store_16(event, 13, channel->psm); 4094 little_endian_store_16(event, 15, channel->local_cid); 4095 little_endian_store_16(event, 17, channel->remote_cid); 4096 little_endian_store_16(event, 19, channel->local_mtu); 4097 little_endian_store_16(event, 21, channel->remote_mtu); 4098 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4099 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4100 } 4101 // 2 4102 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 4103 log_info("closed local_cid 0x%x", channel->local_cid); 4104 uint8_t event[4]; 4105 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 4106 event[1] = sizeof(event) - 2u; 4107 little_endian_store_16(event, 2, channel->local_cid); 4108 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 4109 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 4110 } 4111 4112 static void l2cap_le_send_pdu(l2cap_channel_t *channel){ 4113 btstack_assert(channel != NULL); 4114 btstack_assert(channel->send_sdu_buffer != NULL); 4115 btstack_assert(channel->credits_outgoing > 0); 4116 4117 // send part of SDU 4118 hci_reserve_packet_buffer(); 4119 uint8_t * acl_buffer = hci_get_outgoing_packet_buffer(); 4120 uint8_t * l2cap_payload = acl_buffer + 8; 4121 uint16_t pos = 0; 4122 if (!channel->send_sdu_pos){ 4123 // store SDU len 4124 channel->send_sdu_pos += 2u; 4125 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 4126 pos += 2u; 4127 } 4128 uint16_t payload_size = btstack_min(channel->send_sdu_len + 2u - channel->send_sdu_pos, channel->remote_mps - pos); 4129 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 4130 (void)memcpy(&l2cap_payload[pos], 4131 &channel->send_sdu_buffer[channel->send_sdu_pos - 2u], 4132 payload_size); // -2 for virtual SDU len 4133 pos += payload_size; 4134 channel->send_sdu_pos += payload_size; 4135 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 4136 4137 channel->credits_outgoing--; 4138 4139 hci_send_acl_packet_buffer(8u + pos); 4140 4141 if (channel->send_sdu_pos >= (channel->send_sdu_len + 2u)){ 4142 channel->send_sdu_buffer = NULL; 4143 // send done event 4144 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 4145 // inform about can send now 4146 l2cap_le_notify_channel_can_send(channel); 4147 } 4148 } 4149 4150 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 4151 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 4152 channel->state = L2CAP_STATE_CLOSED; 4153 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 4154 // discard channel 4155 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4156 l2cap_free_channel_entry(channel); 4157 } 4158 4159 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 4160 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 4161 } 4162 4163 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 4164 4165 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 4166 4167 // check for alread registered psm 4168 l2cap_service_t *service = l2cap_le_get_service(psm); 4169 if (service) { 4170 return L2CAP_SERVICE_ALREADY_REGISTERED; 4171 } 4172 4173 // alloc structure 4174 service = btstack_memory_l2cap_service_get(); 4175 if (!service) { 4176 log_error("register: no memory for l2cap_service_t"); 4177 return BTSTACK_MEMORY_ALLOC_FAILED; 4178 } 4179 4180 // fill in 4181 service->psm = psm; 4182 service->mtu = 0; 4183 service->packet_handler = packet_handler; 4184 service->required_security_level = security_level; 4185 4186 // add to services list 4187 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 4188 4189 // done 4190 return ERROR_CODE_SUCCESS; 4191 } 4192 4193 uint8_t l2cap_le_unregister_service(uint16_t psm) { 4194 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 4195 l2cap_service_t *service = l2cap_le_get_service(psm); 4196 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 4197 4198 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 4199 btstack_memory_l2cap_service_free(service); 4200 return ERROR_CODE_SUCCESS; 4201 } 4202 4203 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 4204 // get channel 4205 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4206 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4207 4208 // validate state 4209 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 4210 return ERROR_CODE_COMMAND_DISALLOWED; 4211 } 4212 4213 // set state accept connection 4214 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 4215 channel->receive_sdu_buffer = receive_sdu_buffer; 4216 channel->local_mtu = mtu; 4217 channel->new_credits_incoming = initial_credits; 4218 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4219 4220 // test 4221 // channel->new_credits_incoming = 1; 4222 4223 // go 4224 l2cap_run(); 4225 return ERROR_CODE_SUCCESS; 4226 } 4227 4228 /** 4229 * @brief Deny incoming LE Data Channel connection due to resource constraints 4230 * @param local_cid L2CAP LE Data Channel Identifier 4231 */ 4232 4233 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 4234 // get channel 4235 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4236 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4237 4238 // validate state 4239 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 4240 return ERROR_CODE_COMMAND_DISALLOWED; 4241 } 4242 4243 // set state decline connection 4244 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 4245 channel->reason = 0x04; // no resources available 4246 l2cap_run(); 4247 return ERROR_CODE_SUCCESS; 4248 } 4249 4250 static gap_security_level_t l2cap_le_security_level_for_connection(hci_con_handle_t con_handle){ 4251 uint8_t encryption_key_size = gap_encryption_key_size(con_handle); 4252 if (encryption_key_size == 0) return LEVEL_0; 4253 4254 uint8_t authenticated = gap_authenticated(con_handle); 4255 if (!authenticated) return LEVEL_2; 4256 4257 return encryption_key_size == 16 ? LEVEL_4 : LEVEL_3; 4258 } 4259 4260 // used to handle pairing complete after triggering to increase 4261 static void l2cap_sm_packet_handler(uint8_t packet_type, uint16_t channel_nr, uint8_t *packet, uint16_t size) { 4262 UNUSED(channel_nr); 4263 UNUSED(size); 4264 UNUSED(packet_type); 4265 btstack_assert(packet_type == HCI_EVENT_PACKET); 4266 if (hci_event_packet_get_type(packet) != SM_EVENT_PAIRING_COMPLETE) return; 4267 hci_con_handle_t con_handle = sm_event_pairing_complete_get_handle(packet); 4268 btstack_linked_list_iterator_t it; 4269 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4270 while (btstack_linked_list_iterator_has_next(&it)) { 4271 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4272 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 4273 if (channel->con_handle != con_handle) continue; 4274 if (channel->state != L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE) continue; 4275 4276 // found channel, check security level 4277 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4278 // pairing failed or wasn't good enough, inform user 4279 l2cap_emit_le_channel_opened(channel, ERROR_CODE_INSUFFICIENT_SECURITY); 4280 // discard channel 4281 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4282 l2cap_free_channel_entry(channel); 4283 } else { 4284 // send conn request now 4285 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4286 l2cap_run(); 4287 } 4288 } 4289 } 4290 4291 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 4292 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 4293 uint16_t * out_local_cid) { 4294 4295 static btstack_packet_callback_registration_t sm_event_callback_registration; 4296 static bool sm_callback_registered = false; 4297 4298 log_info("create, handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 4299 4300 hci_connection_t * connection = hci_connection_for_handle(con_handle); 4301 if (!connection) { 4302 log_error("no hci_connection for handle 0x%04x", con_handle); 4303 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 4304 } 4305 4306 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); 4307 if (!channel) { 4308 return BTSTACK_MEMORY_ALLOC_FAILED; 4309 } 4310 log_info("created %p", channel); 4311 4312 // store local_cid 4313 if (out_local_cid){ 4314 *out_local_cid = channel->local_cid; 4315 } 4316 4317 // setup channel entry 4318 channel->con_handle = con_handle; 4319 channel->receive_sdu_buffer = receive_sdu_buffer; 4320 channel->new_credits_incoming = initial_credits; 4321 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4322 4323 // add to connections list 4324 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 4325 4326 // check security level 4327 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4328 if (!sm_callback_registered){ 4329 sm_callback_registered = true; 4330 // lazy registration for SM events 4331 sm_event_callback_registration.callback = &l2cap_sm_packet_handler; 4332 sm_add_event_handler(&sm_event_callback_registration); 4333 } 4334 4335 // start pairing 4336 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 4337 sm_request_pairing(con_handle); 4338 } else { 4339 // send conn request right away 4340 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4341 l2cap_run(); 4342 } 4343 4344 return ERROR_CODE_SUCCESS; 4345 } 4346 4347 /** 4348 * @brief Provide credtis for LE Data Channel 4349 * @param local_cid L2CAP LE Data Channel Identifier 4350 * @param credits Number additional credits for peer 4351 */ 4352 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 4353 4354 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4355 if (!channel) { 4356 log_error("le credits no channel for cid 0x%02x", local_cid); 4357 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4358 } 4359 4360 // check state 4361 if (channel->state != L2CAP_STATE_OPEN){ 4362 log_error("le credits but channel 0x%02x not open yet", local_cid); 4363 } 4364 4365 // assert incoming credits + credits <= 0xffff 4366 uint32_t total_credits = channel->credits_incoming; 4367 total_credits += channel->new_credits_incoming; 4368 total_credits += credits; 4369 if (total_credits > 0xffffu){ 4370 log_error("le credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 4371 channel->new_credits_incoming, credits); 4372 } 4373 4374 // set credits_granted 4375 channel->new_credits_incoming += credits; 4376 4377 // go 4378 l2cap_run(); 4379 return ERROR_CODE_SUCCESS; 4380 } 4381 4382 /** 4383 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 4384 * @param local_cid L2CAP LE Data Channel Identifier 4385 */ 4386 bool l2cap_le_can_send_now(uint16_t local_cid){ 4387 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4388 if (!channel) { 4389 log_error("le can send now, no channel for cid 0x%02x", local_cid); 4390 return false; 4391 } 4392 4393 // check state 4394 if (channel->state != L2CAP_STATE_OPEN) return false; 4395 4396 // check queue 4397 if (channel->send_sdu_buffer) return false; 4398 4399 // fine, go ahead 4400 return true; 4401 } 4402 4403 /** 4404 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 4405 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 4406 * so packet handler should be ready to handle it 4407 * @param local_cid L2CAP LE Data Channel Identifier 4408 */ 4409 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 4410 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4411 if (!channel) { 4412 log_error("can send now, no channel for cid 0x%02x", local_cid); 4413 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4414 } 4415 channel->waiting_for_can_send_now = 1; 4416 l2cap_le_notify_channel_can_send(channel); 4417 return ERROR_CODE_SUCCESS; 4418 } 4419 4420 /** 4421 * @brief Send data via LE Data Channel 4422 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 4423 * @param local_cid L2CAP LE Data Channel Identifier 4424 * @param data data to send 4425 * @param size data size 4426 */ 4427 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t size){ 4428 4429 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4430 if (!channel) { 4431 log_error("l2cap send, no channel for cid 0x%02x", local_cid); 4432 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4433 } 4434 4435 if (size > channel->remote_mtu){ 4436 log_error("l2cap send, cid 0x%02x, data length exceeds remote MTU.", local_cid); 4437 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 4438 } 4439 4440 if (channel->send_sdu_buffer){ 4441 log_info("l2cap send, cid 0x%02x, cannot send", local_cid); 4442 return BTSTACK_ACL_BUFFERS_FULL; 4443 } 4444 4445 channel->send_sdu_buffer = data; 4446 channel->send_sdu_len = size; 4447 channel->send_sdu_pos = 0; 4448 4449 l2cap_notify_channel_can_send(); 4450 return ERROR_CODE_SUCCESS; 4451 } 4452 4453 /** 4454 * @brief Disconnect from LE Data Channel 4455 * @param local_cid L2CAP LE Data Channel Identifier 4456 */ 4457 uint8_t l2cap_le_disconnect(uint16_t local_cid) 4458 { 4459 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4460 if (!channel) { 4461 log_error("l2cap send, no channel for cid 0x%02x", local_cid); 4462 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4463 } 4464 4465 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 4466 l2cap_run(); 4467 return ERROR_CODE_SUCCESS; 4468 } 4469 4470 #endif 4471