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