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