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