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