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