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 #ifdef ENABLE_CLASSIC 917 gap_connectable_control(0); // no services yet 918 #endif 919 } 920 921 void l2cap_register_packet_handler(void (*handler)(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 922 #ifdef ENABLE_BLE 923 l2cap_event_packet_handler = handler; 924 #else 925 UNUSED(handler); // ok: no code 926 #endif 927 } 928 929 void l2cap_request_can_send_fix_channel_now_event(hci_con_handle_t con_handle, uint16_t channel_id){ 930 UNUSED(con_handle); // ok: there is no con handle 931 932 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 933 if (!channel) return; 934 channel->waiting_for_can_send_now = 1; 935 l2cap_notify_channel_can_send(); 936 } 937 938 int l2cap_can_send_fixed_channel_packet_now(hci_con_handle_t con_handle, uint16_t channel_id){ 939 UNUSED(channel_id); // ok: only depends on Controller LE buffers 940 941 return hci_can_send_acl_packet_now(con_handle); 942 } 943 944 uint8_t *l2cap_get_outgoing_buffer(void){ 945 return hci_get_outgoing_packet_buffer() + COMPLETE_L2CAP_HEADER; // 8 bytes 946 } 947 948 // only for L2CAP Basic Channels 949 int l2cap_reserve_packet_buffer(void){ 950 return hci_reserve_packet_buffer(); 951 } 952 953 // only for L2CAP Basic Channels 954 void l2cap_release_packet_buffer(void){ 955 hci_release_packet_buffer(); 956 } 957 958 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){ 959 // 0 - Connection handle : PB=pb : BC=00 960 little_endian_store_16(acl_buffer, 0u, con_handle | (packet_boundary << 12u) | (0u << 14u)); 961 // 2 - ACL length 962 little_endian_store_16(acl_buffer, 2u, len + 4u); 963 // 4 - L2CAP packet length 964 little_endian_store_16(acl_buffer, 4u, len + 0u); 965 // 6 - L2CAP channel DEST 966 little_endian_store_16(acl_buffer, 6, remote_cid); 967 } 968 969 // assumption - only on LE connections 970 int l2cap_send_prepared_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint16_t len){ 971 972 if (!hci_is_packet_buffer_reserved()){ 973 log_error("l2cap_send_prepared_connectionless called without reserving packet first"); 974 return BTSTACK_ACL_BUFFERS_FULL; 975 } 976 977 if (!hci_can_send_prepared_acl_packet_now(con_handle)){ 978 log_info("l2cap_send_prepared_connectionless handle 0x%02x, cid 0x%02x, cannot send", con_handle, cid); 979 return BTSTACK_ACL_BUFFERS_FULL; 980 } 981 982 log_debug("l2cap_send_prepared_connectionless handle %u, cid 0x%02x", con_handle, cid); 983 984 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 985 l2cap_setup_header(acl_buffer, con_handle, 0, cid, len); 986 // send 987 return hci_send_acl_packet_buffer(len+8u); 988 } 989 990 // assumption - only on LE connections 991 int l2cap_send_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint8_t *data, uint16_t len){ 992 993 if (!hci_can_send_acl_packet_now(con_handle)){ 994 log_info("l2cap_send cid 0x%02x, cannot send", cid); 995 return BTSTACK_ACL_BUFFERS_FULL; 996 } 997 998 hci_reserve_packet_buffer(); 999 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1000 1001 (void)memcpy(&acl_buffer[8], data, len); 1002 1003 return l2cap_send_prepared_connectionless(con_handle, cid, len); 1004 } 1005 1006 static void l2cap_emit_can_send_now(btstack_packet_handler_t packet_handler, uint16_t channel) { 1007 log_debug("L2CAP_EVENT_CHANNEL_CAN_SEND_NOW local_cid 0x%x", channel); 1008 uint8_t event[4]; 1009 event[0] = L2CAP_EVENT_CAN_SEND_NOW; 1010 event[1] = sizeof(event) - 2u; 1011 little_endian_store_16(event, 2, channel); 1012 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1013 packet_handler(HCI_EVENT_PACKET, channel, event, sizeof(event)); 1014 } 1015 1016 #ifdef L2CAP_USES_CHANNELS 1017 static void l2cap_dispatch_to_channel(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){ 1018 (* (channel->packet_handler))(type, channel->local_cid, data, size); 1019 } 1020 1021 static void l2cap_emit_simple_event_with_cid(l2cap_channel_t * channel, uint8_t event_code){ 1022 uint8_t event[4]; 1023 event[0] = event_code; 1024 event[1] = sizeof(event) - 2u; 1025 little_endian_store_16(event, 2, channel->local_cid); 1026 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1027 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 1028 } 1029 #endif 1030 1031 #ifdef ENABLE_CLASSIC 1032 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 1033 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", 1034 status, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 1035 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu, channel->flush_timeout); 1036 uint8_t event[26]; 1037 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 1038 event[1] = sizeof(event) - 2; 1039 event[2] = status; 1040 reverse_bd_addr(channel->address, &event[3]); 1041 little_endian_store_16(event, 9, channel->con_handle); 1042 little_endian_store_16(event, 11, channel->psm); 1043 little_endian_store_16(event, 13, channel->local_cid); 1044 little_endian_store_16(event, 15, channel->remote_cid); 1045 little_endian_store_16(event, 17, channel->local_mtu); 1046 little_endian_store_16(event, 19, channel->remote_mtu); 1047 little_endian_store_16(event, 21, channel->flush_timeout); 1048 event[23] = (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) ? 1 : 0; 1049 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1050 log_info("ERTM mode %u, fcs enabled %u", channel->mode, channel->fcs_option); 1051 event[24] = channel->mode; 1052 event[25] = channel->fcs_option; 1053 1054 #else 1055 event[24] = L2CAP_CHANNEL_MODE_BASIC; 1056 event[25] = 0; 1057 #endif 1058 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1059 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 1060 } 1061 1062 static void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 1063 log_info("L2CAP_EVENT_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 1064 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 1065 } 1066 1067 static void l2cap_emit_incoming_connection(l2cap_channel_t *channel) { 1068 log_info("L2CAP_EVENT_INCOMING_CONNECTION addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x", 1069 bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid); 1070 uint8_t event[16]; 1071 event[0] = L2CAP_EVENT_INCOMING_CONNECTION; 1072 event[1] = sizeof(event) - 2; 1073 reverse_bd_addr(channel->address, &event[2]); 1074 little_endian_store_16(event, 8, channel->con_handle); 1075 little_endian_store_16(event, 10, channel->psm); 1076 little_endian_store_16(event, 12, channel->local_cid); 1077 little_endian_store_16(event, 14, channel->remote_cid); 1078 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1079 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 1080 } 1081 1082 static void l2cap_handle_channel_open_failed(l2cap_channel_t * channel, uint8_t status){ 1083 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1084 // emit ertm buffer released, as it's not needed. if in basic mode, it was either not allocated or already released 1085 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1086 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 1087 } 1088 #endif 1089 l2cap_emit_channel_opened(channel, status); 1090 } 1091 1092 static void l2cap_handle_channel_closed(l2cap_channel_t * channel){ 1093 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1094 // emit ertm buffer released, as it's not needed anymore. if in basic mode, it was either not allocated or already released 1095 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1096 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 1097 } 1098 #endif 1099 l2cap_emit_channel_closed(channel); 1100 } 1101 #endif 1102 1103 static l2cap_fixed_channel_t * l2cap_channel_item_by_cid(uint16_t cid){ 1104 btstack_linked_list_iterator_t it; 1105 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1106 while (btstack_linked_list_iterator_has_next(&it)){ 1107 l2cap_fixed_channel_t * channel = (l2cap_fixed_channel_t*) btstack_linked_list_iterator_next(&it); 1108 if (channel->local_cid == cid) { 1109 return channel; 1110 } 1111 } 1112 return NULL; 1113 } 1114 1115 // used for fixed channels in LE (ATT/SM) and Classic (Connectionless Channel). CID < 0x04 1116 static l2cap_fixed_channel_t * l2cap_fixed_channel_for_channel_id(uint16_t local_cid){ 1117 if (local_cid >= 0x40u) return NULL; 1118 return (l2cap_fixed_channel_t*) l2cap_channel_item_by_cid(local_cid); 1119 } 1120 1121 // used for Classic Channels + LE Data Channels. local_cid >= 0x40 1122 #ifdef L2CAP_USES_CHANNELS 1123 static l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){ 1124 if (local_cid < 0x40u) return NULL; 1125 return (l2cap_channel_t*) l2cap_channel_item_by_cid(local_cid); 1126 } 1127 1128 static l2cap_channel_t * l2cap_get_channel_for_local_cid_and_handle(uint16_t local_cid, hci_con_handle_t con_handle){ 1129 if (local_cid < 0x40u) return NULL; 1130 l2cap_channel_t * l2cap_channel = (l2cap_channel_t*) l2cap_channel_item_by_cid(local_cid); 1131 if (l2cap_channel == NULL) return NULL; 1132 if (l2cap_channel->con_handle != con_handle) return NULL; 1133 return l2cap_channel; 1134 } 1135 1136 void l2cap_request_can_send_now_event(uint16_t local_cid){ 1137 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 1138 if (!channel) return; 1139 channel->waiting_for_can_send_now = 1; 1140 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1141 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1142 l2cap_ertm_notify_channel_can_send(channel); 1143 return; 1144 } 1145 #endif 1146 l2cap_notify_channel_can_send(); 1147 } 1148 1149 int l2cap_can_send_packet_now(uint16_t local_cid){ 1150 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 1151 if (!channel) return 0; 1152 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1153 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1154 return l2cap_ertm_can_store_packet_now(channel); 1155 } 1156 #endif 1157 return hci_can_send_acl_packet_now(channel->con_handle); 1158 } 1159 1160 int l2cap_can_send_prepared_packet_now(uint16_t local_cid){ 1161 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 1162 if (!channel) return 0; 1163 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1164 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1165 return 0; 1166 } 1167 #endif 1168 return hci_can_send_prepared_acl_packet_now(channel->con_handle); 1169 } 1170 1171 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){ 1172 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1173 if (channel) { 1174 return channel->remote_mtu; 1175 } 1176 return 0; 1177 } 1178 #endif 1179 1180 #ifdef L2CAP_USES_CHANNELS 1181 static int l2cap_is_dynamic_channel_type(l2cap_channel_type_t channel_type){ 1182 switch (channel_type){ 1183 case L2CAP_CHANNEL_TYPE_CLASSIC: 1184 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 1185 return 1; 1186 default: 1187 return 0; 1188 } 1189 } 1190 #endif 1191 1192 #ifdef ENABLE_CLASSIC 1193 // RTX Timer only exist for dynamic channels 1194 static l2cap_channel_t * l2cap_channel_for_rtx_timer(btstack_timer_source_t * ts){ 1195 btstack_linked_list_iterator_t it; 1196 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1197 while (btstack_linked_list_iterator_has_next(&it)){ 1198 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1199 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 1200 if (&channel->rtx == ts) { 1201 return channel; 1202 } 1203 } 1204 return NULL; 1205 } 1206 1207 static void l2cap_rtx_timeout(btstack_timer_source_t * ts){ 1208 l2cap_channel_t * channel = l2cap_channel_for_rtx_timer(ts); 1209 if (!channel) return; 1210 1211 log_info("l2cap_rtx_timeout for local cid 0x%02x", channel->local_cid); 1212 1213 // "When terminating the channel, it is not necessary to send a L2CAP_DisconnectReq 1214 // and enter WAIT_DISCONNECT state. Channels can be transitioned directly to the CLOSED state." 1215 // notify client 1216 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_RTX_TIMEOUT); 1217 1218 // discard channel 1219 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1220 l2cap_free_channel_entry(channel); 1221 } 1222 1223 #endif 1224 1225 #ifdef L2CAP_USES_CHANNELS 1226 static void l2cap_stop_rtx(l2cap_channel_t * channel){ 1227 log_info("l2cap_stop_rtx for local cid 0x%02x", channel->local_cid); 1228 btstack_run_loop_remove_timer(&channel->rtx); 1229 } 1230 #endif 1231 1232 #ifdef ENABLE_CLASSIC 1233 1234 static void l2cap_start_rtx(l2cap_channel_t * channel){ 1235 l2cap_stop_rtx(channel); 1236 log_info("l2cap_start_rtx for local cid 0x%02x", channel->local_cid); 1237 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 1238 btstack_run_loop_set_timer(&channel->rtx, L2CAP_RTX_TIMEOUT_MS); 1239 btstack_run_loop_add_timer(&channel->rtx); 1240 } 1241 1242 static void l2cap_start_ertx(l2cap_channel_t * channel){ 1243 log_info("l2cap_start_ertx for local cid 0x%02x", channel->local_cid); 1244 l2cap_stop_rtx(channel); 1245 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 1246 btstack_run_loop_set_timer(&channel->rtx, L2CAP_ERTX_TIMEOUT_MS); 1247 btstack_run_loop_add_timer(&channel->rtx); 1248 } 1249 1250 void l2cap_require_security_level_2_for_outgoing_sdp(void){ 1251 require_security_level2_for_outgoing_sdp = 1; 1252 } 1253 1254 static int l2cap_security_level_0_allowed_for_PSM(uint16_t psm){ 1255 return (psm == BLUETOOTH_PSM_SDP) && (!require_security_level2_for_outgoing_sdp); 1256 } 1257 1258 static int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, int identifier, ...){ 1259 if (!hci_can_send_acl_packet_now(handle)){ 1260 log_info("l2cap_send_signaling_packet, cannot send"); 1261 return BTSTACK_ACL_BUFFERS_FULL; 1262 } 1263 1264 // log_info("l2cap_send_signaling_packet type %u", cmd); 1265 hci_reserve_packet_buffer(); 1266 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1267 va_list argptr; 1268 va_start(argptr, identifier); 1269 uint16_t len = l2cap_create_signaling_classic(acl_buffer, handle, cmd, identifier, argptr); 1270 va_end(argptr); 1271 // log_info("l2cap_send_signaling_packet con %u!", handle); 1272 return hci_send_acl_packet_buffer(len); 1273 } 1274 1275 // assumption - only on Classic connections 1276 // cannot be used for L2CAP ERTM 1277 int l2cap_send_prepared(uint16_t local_cid, uint16_t len){ 1278 1279 if (!hci_is_packet_buffer_reserved()){ 1280 log_error("l2cap_send_prepared called without reserving packet first"); 1281 return BTSTACK_ACL_BUFFERS_FULL; 1282 } 1283 1284 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1285 if (!channel) { 1286 log_error("l2cap_send_prepared no channel for cid 0x%02x", local_cid); 1287 return -1; // TODO: define error 1288 } 1289 1290 if (!hci_can_send_prepared_acl_packet_now(channel->con_handle)){ 1291 log_info("l2cap_send_prepared cid 0x%02x, cannot send", local_cid); 1292 return BTSTACK_ACL_BUFFERS_FULL; 1293 } 1294 1295 log_debug("l2cap_send_prepared cid 0x%02x, handle %u, 1 credit used", local_cid, channel->con_handle); 1296 1297 int fcs_size = 0; 1298 1299 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1300 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->fcs_option){ 1301 fcs_size = 2; 1302 } 1303 #endif 1304 1305 // set non-flushable packet boundary flag if supported on Controller 1306 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1307 uint8_t packet_boundary_flag = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 1308 l2cap_setup_header(acl_buffer, channel->con_handle, packet_boundary_flag, channel->remote_cid, len + fcs_size); 1309 1310 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1311 if (fcs_size){ 1312 // calculate FCS over l2cap data 1313 uint16_t fcs = crc16_calc(acl_buffer + 4, 4 + len); 1314 log_info("I-Frame: fcs 0x%04x", fcs); 1315 little_endian_store_16(acl_buffer, 8 + len, fcs); 1316 } 1317 #endif 1318 1319 // send 1320 return hci_send_acl_packet_buffer(len+8+fcs_size); 1321 } 1322 1323 // assumption - only on Classic connections 1324 int l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){ 1325 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1326 if (!channel) { 1327 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 1328 return -1; // TODO: define error 1329 } 1330 1331 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1332 // send in ERTM 1333 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1334 return l2cap_ertm_send(channel, data, len); 1335 } 1336 #endif 1337 1338 if (len > channel->remote_mtu){ 1339 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 1340 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 1341 } 1342 1343 if (!hci_can_send_acl_packet_now(channel->con_handle)){ 1344 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 1345 return BTSTACK_ACL_BUFFERS_FULL; 1346 } 1347 1348 hci_reserve_packet_buffer(); 1349 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1350 (void)memcpy(&acl_buffer[8], data, len); 1351 return l2cap_send_prepared(local_cid, len); 1352 } 1353 1354 int l2cap_send_echo_request(hci_con_handle_t con_handle, uint8_t *data, uint16_t len){ 1355 return l2cap_send_signaling_packet(con_handle, ECHO_REQUEST, 0x77, len, data); 1356 } 1357 1358 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 1359 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var | flag); 1360 } 1361 1362 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 1363 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var & ~flag); 1364 } 1365 #endif 1366 1367 1368 #ifdef ENABLE_BLE 1369 static int l2cap_send_le_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, int identifier, ...){ 1370 1371 if (!hci_can_send_acl_packet_now(handle)){ 1372 log_info("l2cap_send_le_signaling_packet, cannot send"); 1373 return BTSTACK_ACL_BUFFERS_FULL; 1374 } 1375 1376 // log_info("l2cap_send_le_signaling_packet type %u", cmd); 1377 hci_reserve_packet_buffer(); 1378 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 1379 va_list argptr; 1380 va_start(argptr, identifier); 1381 uint16_t len = l2cap_create_signaling_le(acl_buffer, handle, cmd, identifier, argptr); 1382 va_end(argptr); 1383 // log_info("l2cap_send_le_signaling_packet con %u!", handle); 1384 return hci_send_acl_packet_buffer(len); 1385 } 1386 #endif 1387 1388 uint16_t l2cap_max_mtu(void){ 1389 return HCI_ACL_PAYLOAD_SIZE - L2CAP_HEADER_SIZE; 1390 } 1391 1392 #ifdef ENABLE_BLE 1393 uint16_t l2cap_max_le_mtu(void){ 1394 if (l2cap_le_custom_max_mtu != 0u) return l2cap_le_custom_max_mtu; 1395 return l2cap_max_mtu(); 1396 } 1397 1398 void l2cap_set_max_le_mtu(uint16_t max_mtu){ 1399 if (max_mtu < l2cap_max_mtu()){ 1400 l2cap_le_custom_max_mtu = max_mtu; 1401 } 1402 } 1403 #endif 1404 1405 #ifdef ENABLE_CLASSIC 1406 1407 static uint16_t l2cap_setup_options_mtu(uint8_t * config_options, uint16_t mtu){ 1408 config_options[0] = L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT; // MTU 1409 config_options[1] = 2; // len param 1410 little_endian_store_16(config_options, 2, mtu); 1411 return 4; 1412 } 1413 1414 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1415 static int l2cap_ertm_mode(l2cap_channel_t * channel){ 1416 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1417 return ((connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_DONE) 1418 && (connection->l2cap_state.extended_feature_mask & 0x08)); 1419 } 1420 #endif 1421 1422 static uint16_t l2cap_setup_options_request(l2cap_channel_t * channel, uint8_t * config_options){ 1423 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1424 // use ERTM options if supported by remote and channel ready to use it 1425 if (l2cap_ertm_mode(channel) && channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1426 return l2cap_setup_options_ertm_request(channel, config_options); 1427 } 1428 #endif 1429 uint16_t mtu = channel->local_mtu; 1430 return l2cap_setup_options_mtu(config_options, mtu); 1431 } 1432 1433 static uint16_t l2cap_setup_options_mtu_response(l2cap_channel_t * channel, uint8_t * config_options){ 1434 uint16_t mtu = btstack_min(channel->local_mtu, channel->remote_mtu); 1435 return l2cap_setup_options_mtu(config_options, mtu); 1436 } 1437 1438 static uint32_t l2cap_extended_features_mask(void){ 1439 // extended features request supported, features: fixed channels, unicast connectionless data reception 1440 uint32_t features = 0x280; 1441 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1442 features |= 0x0028; 1443 #endif 1444 return features; 1445 } 1446 #endif 1447 1448 // 1449 #ifdef ENABLE_CLASSIC 1450 1451 // returns true if channel was finalized 1452 static bool l2cap_run_for_classic_channel(l2cap_channel_t * channel){ 1453 1454 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1455 uint8_t config_options[18]; 1456 #else 1457 uint8_t config_options[10]; 1458 #endif 1459 1460 switch (channel->state){ 1461 1462 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1463 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 1464 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1465 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 1466 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 1467 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0); 1468 } 1469 break; 1470 1471 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1472 if (!hci_can_send_command_packet_now()) break; 1473 // send connection request - set state first 1474 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 1475 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 1476 (void)memcpy(l2cap_outgoing_classic_addr, channel->address, 6); 1477 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, hci_get_allow_role_switch()); 1478 break; 1479 1480 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 1481 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1482 channel->state = L2CAP_STATE_INVALID; 1483 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0); 1484 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1485 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1486 l2cap_free_channel_entry(channel); 1487 channel = NULL; 1488 break; 1489 1490 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 1491 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1492 channel->state = L2CAP_STATE_CONFIG; 1493 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1494 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 1495 break; 1496 1497 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 1498 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1499 // success, start l2cap handshake 1500 channel->local_sig_id = l2cap_next_sig_id(); 1501 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 1502 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 1503 l2cap_start_rtx(channel); 1504 break; 1505 1506 case L2CAP_STATE_CONFIG: 1507 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1508 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1509 // fallback to basic mode if ERTM requested but not not supported by remote 1510 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1511 if (!l2cap_ertm_mode(channel)){ 1512 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 1513 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1514 } 1515 } 1516 #endif 1517 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 1518 uint16_t flags = 0; 1519 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1520 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 1521 flags = 1; 1522 } else { 1523 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1524 } 1525 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 1526 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1527 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL); 1528 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1529 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 1530 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1531 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1532 uint16_t options_size = l2cap_setup_options_ertm_response(channel, config_options); 1533 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); 1534 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM){ 1535 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 1536 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1537 uint16_t options_size = l2cap_setup_options_ertm_response(channel, config_options); 1538 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); 1539 #endif 1540 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 1541 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1542 uint16_t options_size = l2cap_setup_options_mtu_response(channel, config_options); 1543 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); 1544 } else { 1545 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 1546 } 1547 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1548 } 1549 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 1550 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1551 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 1552 channel->local_sig_id = l2cap_next_sig_id(); 1553 uint16_t options_size = l2cap_setup_options_request(channel, config_options); 1554 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, options_size, &config_options); 1555 l2cap_start_rtx(channel); 1556 } 1557 if (l2cap_channel_ready_for_open(channel)){ 1558 channel->state = L2CAP_STATE_OPEN; 1559 l2cap_emit_channel_opened(channel, 0); // success 1560 } 1561 break; 1562 1563 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1564 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1565 channel->state = L2CAP_STATE_INVALID; 1566 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1567 // 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 :) 1568 l2cap_finialize_channel_close(channel); // -- remove from list 1569 channel = NULL; 1570 break; 1571 1572 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1573 if (!hci_can_send_acl_packet_now(channel->con_handle)) return false; 1574 channel->local_sig_id = l2cap_next_sig_id(); 1575 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1576 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1577 break; 1578 default: 1579 break; 1580 } 1581 1582 // handle channel finalize on L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE and L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE 1583 return channel == NULL; 1584 } 1585 1586 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1587 static void l2cap_run_for_classic_channel_ertm(l2cap_channel_t * channel){ 1588 1589 // ERTM mode 1590 if (channel->mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) return; 1591 1592 // check if we can still send 1593 if (channel->con_handle == HCI_CON_HANDLE_INVALID) return; 1594 if (!hci_can_send_acl_packet_now(channel->con_handle)) return; 1595 1596 if (channel->send_supervisor_frame_receiver_ready){ 1597 channel->send_supervisor_frame_receiver_ready = 0; 1598 log_info("Send S-Frame: RR %u, final %u", channel->req_seq, channel->set_final_bit_after_packet_with_poll_bit_set); 1599 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); 1600 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1601 l2cap_ertm_send_supervisor_frame(channel, control); 1602 return; 1603 } 1604 if (channel->send_supervisor_frame_receiver_ready_poll){ 1605 channel->send_supervisor_frame_receiver_ready_poll = 0; 1606 log_info("Send S-Frame: RR %u with poll=1 ", channel->req_seq); 1607 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 1, 0, channel->req_seq); 1608 l2cap_ertm_send_supervisor_frame(channel, control); 1609 return; 1610 } 1611 if (channel->send_supervisor_frame_receiver_not_ready){ 1612 channel->send_supervisor_frame_receiver_not_ready = 0; 1613 log_info("Send S-Frame: RNR %u", channel->req_seq); 1614 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 1615 l2cap_ertm_send_supervisor_frame(channel, control); 1616 return; 1617 } 1618 if (channel->send_supervisor_frame_reject){ 1619 channel->send_supervisor_frame_reject = 0; 1620 log_info("Send S-Frame: REJ %u", channel->req_seq); 1621 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT, 0, 0, channel->req_seq); 1622 l2cap_ertm_send_supervisor_frame(channel, control); 1623 return; 1624 } 1625 if (channel->send_supervisor_frame_selective_reject){ 1626 channel->send_supervisor_frame_selective_reject = 0; 1627 log_info("Send S-Frame: SREJ %u", channel->expected_tx_seq); 1628 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); 1629 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1630 l2cap_ertm_send_supervisor_frame(channel, control); 1631 return; 1632 } 1633 1634 if (channel->srej_active){ 1635 int i; 1636 for (i=0;i<channel->num_tx_buffers;i++){ 1637 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[i]; 1638 if (tx_state->retransmission_requested) { 1639 tx_state->retransmission_requested = 0; 1640 uint8_t final = channel->set_final_bit_after_packet_with_poll_bit_set; 1641 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1642 l2cap_ertm_send_information_frame(channel, i, final); 1643 break; 1644 } 1645 } 1646 if (i == channel->num_tx_buffers){ 1647 // no retransmission request found 1648 channel->srej_active = 0; 1649 } else { 1650 // packet was sent 1651 return; 1652 } 1653 } 1654 } 1655 #endif /* ERTM */ 1656 #endif /* Classic */ 1657 1658 static void l2cap_run_signaling_response(void) { 1659 1660 // check pending signaling responses 1661 while (signaling_responses_pending){ 1662 1663 hci_con_handle_t handle = signaling_responses[0].handle; 1664 1665 if (!hci_can_send_acl_packet_now(handle)) break; 1666 1667 uint8_t sig_id = signaling_responses[0].sig_id; 1668 uint8_t response_code = signaling_responses[0].code; 1669 uint16_t result = signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT 1670 #ifdef ENABLE_CLASSIC 1671 uint16_t info_type = signaling_responses[0].data; // INFORMATION_REQUEST 1672 uint16_t source_cid = signaling_responses[0].cid; // CONNECTION_REQUEST 1673 #endif 1674 1675 // remove first item before sending (to avoid sending response mutliple times) 1676 signaling_responses_pending--; 1677 int i; 1678 for (i=0; i < signaling_responses_pending; i++){ 1679 (void)memcpy(&signaling_responses[i], 1680 &signaling_responses[i + 1], 1681 sizeof(l2cap_signaling_response_t)); 1682 } 1683 1684 switch (response_code){ 1685 #ifdef ENABLE_CLASSIC 1686 case CONNECTION_REQUEST: 1687 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 1688 // also disconnect if result is 0x0003 - security blocked 1689 if (result == 0x0003){ 1690 hci_disconnect_security_block(handle); 1691 } 1692 break; 1693 case ECHO_REQUEST: 1694 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 1695 break; 1696 case INFORMATION_REQUEST: 1697 switch (info_type){ 1698 case L2CAP_INFO_TYPE_CONNECTIONLESS_MTU: { 1699 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 1700 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(connectionless_mtu), &connectionless_mtu); 1701 } 1702 break; 1703 case L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED: { 1704 uint32_t features = l2cap_extended_features_mask(); 1705 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(features), &features); 1706 } 1707 break; 1708 case L2CAP_INFO_TYPE_FIXED_CHANNELS_SUPPORTED: { 1709 uint8_t map[8]; 1710 memset(map, 0, 8); 1711 map[0] = 0x06; // L2CAP Signaling Channel (0x02) + Connectionless reception (0x04) 1712 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(map), &map); 1713 } 1714 break; 1715 default: 1716 // all other types are not supported 1717 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 1, 0, NULL); 1718 break; 1719 } 1720 break; 1721 case COMMAND_REJECT: 1722 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1723 break; 1724 #endif 1725 #ifdef ENABLE_BLE 1726 case LE_CREDIT_BASED_CONNECTION_REQUEST: 1727 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 1728 break; 1729 case COMMAND_REJECT_LE: 1730 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1731 break; 1732 #endif 1733 default: 1734 // should not happen 1735 break; 1736 } 1737 } 1738 } 1739 1740 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1741 static bool l2ap_run_ertm(void){ 1742 // send l2cap information request if neccessary 1743 btstack_linked_list_iterator_t it; 1744 hci_connections_get_iterator(&it); 1745 while(btstack_linked_list_iterator_has_next(&it)){ 1746 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1747 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST){ 1748 if (!hci_can_send_acl_packet_now(connection->con_handle)) break; 1749 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE; 1750 uint8_t sig_id = l2cap_next_sig_id(); 1751 uint8_t info_type = L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED; 1752 l2cap_send_signaling_packet(connection->con_handle, INFORMATION_REQUEST, sig_id, info_type); 1753 return true; 1754 } 1755 } 1756 return false; 1757 } 1758 #endif 1759 1760 #ifdef ENABLE_LE_DATA_CHANNELS 1761 static void l2cap_run_le_data_channels(void){ 1762 btstack_linked_list_iterator_t it; 1763 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1764 while (btstack_linked_list_iterator_has_next(&it)){ 1765 uint16_t mps; 1766 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1767 1768 if (channel->channel_type != L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL) continue; 1769 1770 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1771 switch (channel->state){ 1772 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1773 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1774 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1775 // le psm, source cid, mtu, mps, initial credits 1776 channel->local_sig_id = l2cap_next_sig_id(); 1777 channel->credits_incoming = channel->new_credits_incoming; 1778 channel->new_credits_incoming = 0; 1779 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1780 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); 1781 break; 1782 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1783 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1784 // TODO: support larger MPS 1785 channel->state = L2CAP_STATE_OPEN; 1786 channel->credits_incoming = channel->new_credits_incoming; 1787 channel->new_credits_incoming = 0; 1788 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1789 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); 1790 // notify client 1791 l2cap_emit_le_channel_opened(channel, 0); 1792 break; 1793 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1794 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1795 channel->state = L2CAP_STATE_INVALID; 1796 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1797 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1798 btstack_linked_list_iterator_remove(&it); 1799 l2cap_free_channel_entry(channel); 1800 break; 1801 case L2CAP_STATE_OPEN: 1802 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1803 1804 // send credits 1805 if (channel->new_credits_incoming){ 1806 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1807 channel->local_sig_id = l2cap_next_sig_id(); 1808 uint16_t new_credits = channel->new_credits_incoming; 1809 channel->new_credits_incoming = 0; 1810 channel->credits_incoming += new_credits; 1811 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1812 } 1813 break; 1814 1815 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1816 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1817 channel->local_sig_id = l2cap_next_sig_id(); 1818 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1819 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1820 break; 1821 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1822 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1823 channel->state = L2CAP_STATE_INVALID; 1824 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1825 l2cap_le_finialize_channel_close(channel); // -- remove from list 1826 break; 1827 default: 1828 break; 1829 } 1830 } 1831 } 1832 #endif 1833 1834 // MARK: L2CAP_RUN 1835 // process outstanding signaling tasks 1836 static void l2cap_run(void){ 1837 1838 // log_info("l2cap_run: entered"); 1839 l2cap_run_signaling_response(); 1840 1841 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1842 bool done = l2ap_run_ertm(); 1843 if (done) return; 1844 #endif 1845 1846 #if defined(ENABLE_CLASSIC) || defined(ENABLE_BLE) 1847 btstack_linked_list_iterator_t it; 1848 #endif 1849 1850 #ifdef ENABLE_CLASSIC 1851 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1852 while (btstack_linked_list_iterator_has_next(&it)){ 1853 1854 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1855 1856 if (channel->channel_type != L2CAP_CHANNEL_TYPE_CLASSIC) continue; 1857 1858 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1859 bool finalized = l2cap_run_for_classic_channel(channel); 1860 1861 if (!finalized) { 1862 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1863 l2cap_run_for_classic_channel_ertm(channel); 1864 #endif 1865 } 1866 } 1867 #endif 1868 1869 #ifdef ENABLE_LE_DATA_CHANNELS 1870 l2cap_run_le_data_channels(); 1871 #endif 1872 1873 #ifdef ENABLE_BLE 1874 // send l2cap con paramter update if necessary 1875 hci_connections_get_iterator(&it); 1876 while(btstack_linked_list_iterator_has_next(&it)){ 1877 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1878 if ((connection->address_type != BD_ADDR_TYPE_LE_PUBLIC) && (connection->address_type != BD_ADDR_TYPE_LE_RANDOM)) continue; 1879 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1880 switch (connection->le_con_parameter_update_state){ 1881 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1882 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1883 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, l2cap_next_sig_id(), 1884 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1885 break; 1886 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1887 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1888 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1889 break; 1890 case CON_PARAMETER_UPDATE_DENY: 1891 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1892 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1893 break; 1894 default: 1895 break; 1896 } 1897 } 1898 #endif 1899 1900 // log_info("l2cap_run: exit"); 1901 } 1902 1903 #ifdef ENABLE_CLASSIC 1904 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1905 if ((channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE) || (channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION)) { 1906 log_info("connection complete con_handle %04x - for channel %p cid 0x%04x", (int) con_handle, channel, channel->local_cid); 1907 // success, start l2cap handshake 1908 channel->con_handle = con_handle; 1909 // check remote SSP feature first 1910 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1911 } 1912 } 1913 1914 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1915 1916 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1917 // assumption: outgoing connection 1918 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1919 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1920 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1921 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1922 return; 1923 } 1924 #endif 1925 1926 // fine, go ahead 1927 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1928 } 1929 1930 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1931 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1932 1933 // we have been waiting for remote supported features 1934 if (channel->required_security_level > LEVEL_0){ 1935 // request security level 1936 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 1937 gap_request_security_level(channel->con_handle, channel->required_security_level); 1938 return; 1939 } 1940 1941 l2cap_ready_to_connect(channel); 1942 } 1943 #endif 1944 1945 #ifdef L2CAP_USES_CHANNELS 1946 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, 1947 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 1948 1949 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 1950 if (!channel) { 1951 return NULL; 1952 } 1953 1954 // fill in 1955 channel->packet_handler = packet_handler; 1956 channel->channel_type = channel_type; 1957 bd_addr_copy(channel->address, address); 1958 channel->address_type = address_type; 1959 channel->psm = psm; 1960 channel->local_mtu = local_mtu; 1961 channel->remote_mtu = L2CAP_DEFAULT_MTU; 1962 channel->required_security_level = security_level; 1963 1964 // 1965 channel->local_cid = l2cap_next_local_cid(); 1966 channel->con_handle = HCI_CON_HANDLE_INVALID; 1967 1968 // set initial state 1969 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 1970 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 1971 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 1972 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 1973 1974 log_info("create channel %p, local_cid 0x%04x", channel, channel->local_cid); 1975 1976 return channel; 1977 } 1978 1979 static void l2cap_free_channel_entry(l2cap_channel_t * channel){ 1980 log_info("free channel %p, local_cid 0x%04x", channel, channel->local_cid); 1981 // assert all timers are stopped 1982 l2cap_stop_rtx(channel); 1983 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1984 l2cap_ertm_stop_retransmission_timer(channel); 1985 l2cap_ertm_stop_monitor_timer(channel); 1986 #endif 1987 // free memory 1988 btstack_memory_l2cap_channel_free(channel); 1989 } 1990 #endif 1991 1992 #ifdef ENABLE_CLASSIC 1993 1994 /** 1995 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 1996 * @param packet_handler 1997 * @param address 1998 * @param psm 1999 * @param mtu 2000 * @param local_cid 2001 */ 2002 2003 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){ 2004 // limit MTU to the size of our outgoing HCI buffer 2005 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 2006 2007 // determine security level based on psm 2008 const gap_security_level_t security_level = l2cap_security_level_0_allowed_for_PSM(psm) ? LEVEL_0 : gap_get_security_level(); 2009 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); 2010 2011 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); 2012 if (!channel) { 2013 return BTSTACK_MEMORY_ALLOC_FAILED; 2014 } 2015 2016 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2017 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2018 #endif 2019 2020 // add to connections list 2021 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2022 2023 // store local_cid 2024 if (out_local_cid){ 2025 *out_local_cid = channel->local_cid; 2026 } 2027 2028 // state: L2CAP_STATE_WILL_SEND_CREATE_CONNECTION 2029 2030 // check if hci connection is already usable, 2031 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_ACL); 2032 if (conn && conn->con_handle != HCI_CON_HANDLE_INVALID){ 2033 // simulate connection complete 2034 l2cap_handle_connection_complete(conn->con_handle, channel); 2035 2036 // state: L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES 2037 2038 // check if remote supported features are already received 2039 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 2040 // simulate remote features received 2041 l2cap_handle_remote_supported_features_received(channel); 2042 } 2043 } 2044 2045 l2cap_run(); 2046 2047 return ERROR_CODE_SUCCESS; 2048 } 2049 2050 void l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 2051 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 2052 // find channel for local_cid 2053 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2054 if (channel) { 2055 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2056 } 2057 // process 2058 l2cap_run(); 2059 } 2060 2061 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 2062 // mark all channels before emitting open events as these could trigger new connetion requests to the same device 2063 btstack_linked_list_iterator_t it; 2064 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2065 while (btstack_linked_list_iterator_has_next(&it)){ 2066 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2067 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2068 if (bd_addr_cmp( channel->address, address) != 0) continue; 2069 // channel for this address found 2070 switch (channel->state){ 2071 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2072 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2073 channel->state = L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD; 2074 break; 2075 default: 2076 break; 2077 } 2078 } 2079 // emit and free marked entries. restart loop to deal with list changes 2080 int done = 0; 2081 while (!done) { 2082 done = 1; 2083 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2084 while (btstack_linked_list_iterator_has_next(&it)){ 2085 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2086 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2087 if (channel->state == L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD){ 2088 done = 0; 2089 // failure, forward error code 2090 l2cap_handle_channel_open_failed(channel, status); 2091 // discard channel 2092 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2093 l2cap_free_channel_entry(channel); 2094 break; 2095 } 2096 } 2097 } 2098 2099 } 2100 2101 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 2102 btstack_linked_list_iterator_t it; 2103 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2104 while (btstack_linked_list_iterator_has_next(&it)){ 2105 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2106 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2107 if ( ! bd_addr_cmp( channel->address, address) ){ 2108 l2cap_handle_connection_complete(handle, channel); 2109 } 2110 } 2111 // process 2112 l2cap_run(); 2113 } 2114 #endif 2115 2116 static bool l2cap_channel_ready_to_send(l2cap_channel_t * channel){ 2117 switch (channel->channel_type){ 2118 #ifdef ENABLE_CLASSIC 2119 case L2CAP_CHANNEL_TYPE_CLASSIC: 2120 if (channel->state != L2CAP_STATE_OPEN) return false; 2121 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2122 // send if we have more data and remote windows isn't full yet 2123 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2124 if (channel->unacked_frames >= btstack_min(channel->num_stored_tx_frames, channel->remote_tx_window_size)) return false; 2125 return hci_can_send_acl_classic_packet_now() != 0; 2126 } 2127 #endif 2128 if (!channel->waiting_for_can_send_now) return false; 2129 return (hci_can_send_acl_classic_packet_now() != 0); 2130 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2131 if (!channel->waiting_for_can_send_now) return false; 2132 return hci_can_send_acl_classic_packet_now() != 0; 2133 #endif 2134 #ifdef ENABLE_BLE 2135 case L2CAP_CHANNEL_TYPE_LE_FIXED: 2136 if (!channel->waiting_for_can_send_now) return false; 2137 return hci_can_send_acl_le_packet_now() != 0; 2138 #ifdef ENABLE_LE_DATA_CHANNELS 2139 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2140 if (channel->state != L2CAP_STATE_OPEN) return false; 2141 if (channel->send_sdu_buffer == NULL) return false; 2142 if (channel->credits_outgoing == 0u) return false; 2143 return hci_can_send_acl_le_packet_now() != 0; 2144 #endif 2145 #endif 2146 default: 2147 return false; 2148 } 2149 } 2150 2151 static void l2cap_channel_trigger_send(l2cap_channel_t * channel){ 2152 switch (channel->channel_type){ 2153 #ifdef ENABLE_CLASSIC 2154 case L2CAP_CHANNEL_TYPE_CLASSIC: 2155 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2156 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2157 l2cap_ertm_channel_send_information_frame(channel); 2158 return; 2159 } 2160 #endif 2161 channel->waiting_for_can_send_now = 0; 2162 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2163 break; 2164 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2165 channel->waiting_for_can_send_now = 0; 2166 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2167 break; 2168 #endif 2169 #ifdef ENABLE_BLE 2170 case L2CAP_CHANNEL_TYPE_LE_FIXED: 2171 channel->waiting_for_can_send_now = 0; 2172 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2173 break; 2174 #ifdef ENABLE_LE_DATA_CHANNELS 2175 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2176 l2cap_le_send_pdu(channel); 2177 break; 2178 #endif 2179 #endif 2180 default: 2181 break; 2182 } 2183 } 2184 2185 static void l2cap_notify_channel_can_send(void){ 2186 bool done = false; 2187 while (!done){ 2188 done = true; 2189 btstack_linked_list_iterator_t it; 2190 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2191 while (btstack_linked_list_iterator_has_next(&it)){ 2192 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2193 bool ready = l2cap_channel_ready_to_send(channel); 2194 if (!ready) continue; 2195 2196 // requeue channel for fairness 2197 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2198 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2199 2200 // trigger sending 2201 l2cap_channel_trigger_send(channel); 2202 2203 // exit inner loop as we just broke the iterator, but try again 2204 done = false; 2205 break; 2206 } 2207 } 2208 } 2209 2210 #ifdef L2CAP_USES_CHANNELS 2211 2212 static int l2cap_send_open_failed_on_hci_disconnect(l2cap_channel_t * channel){ 2213 // open cannot fail for for incoming connections 2214 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) return 0; 2215 2216 // check state 2217 switch (channel->state){ 2218 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2219 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2220 case L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES: 2221 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2222 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 2223 case L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES: 2224 case L2CAP_STATE_WAIT_CONNECT_RSP: 2225 case L2CAP_STATE_CONFIG: 2226 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 2227 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 2228 case L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE: 2229 case L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD: 2230 return 1; 2231 2232 case L2CAP_STATE_OPEN: 2233 case L2CAP_STATE_CLOSED: 2234 case L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES: 2235 case L2CAP_STATE_WAIT_DISCONNECT: 2236 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_INSUFFICIENT_SECURITY: 2237 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 2238 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 2239 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2240 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 2241 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 2242 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 2243 case L2CAP_STATE_INVALID: 2244 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2245 return 0; 2246 2247 default: 2248 // get a "warning" about new states 2249 btstack_assert(false); 2250 return 0; 2251 } 2252 } 2253 #endif 2254 2255 #ifdef ENABLE_CLASSIC 2256 static void l2cap_handle_hci_disconnect_event(l2cap_channel_t * channel){ 2257 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2258 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2259 } else { 2260 l2cap_handle_channel_closed(channel); 2261 } 2262 l2cap_free_channel_entry(channel); 2263 } 2264 #endif 2265 2266 #ifdef ENABLE_LE_DATA_CHANNELS 2267 static void l2cap_handle_hci_le_disconnect_event(l2cap_channel_t * channel){ 2268 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2269 l2cap_emit_le_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2270 } else { 2271 l2cap_emit_le_channel_closed(channel); 2272 } 2273 l2cap_free_channel_entry(channel); 2274 } 2275 #endif 2276 2277 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 2278 2279 UNUSED(packet_type); // ok: registered with hci_event_callback_registration 2280 UNUSED(cid); // ok: there is no channel 2281 UNUSED(size); // ok: fixed format events read from HCI buffer 2282 2283 #ifdef ENABLE_CLASSIC 2284 bd_addr_t address; 2285 hci_connection_t * hci_connection; 2286 int hci_con_used; 2287 #endif 2288 #ifdef L2CAP_USES_CHANNELS 2289 hci_con_handle_t handle; 2290 btstack_linked_list_iterator_t it; 2291 #endif 2292 2293 switch(hci_event_packet_get_type(packet)){ 2294 2295 // Notify channel packet handler if they can send now 2296 case HCI_EVENT_TRANSPORT_PACKET_SENT: 2297 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 2298 case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED: 2299 l2cap_run(); // try sending signaling packets first 2300 l2cap_notify_channel_can_send(); 2301 break; 2302 2303 case HCI_EVENT_COMMAND_STATUS: 2304 #ifdef ENABLE_CLASSIC 2305 // check command status for create connection for errors 2306 if (HCI_EVENT_IS_COMMAND_STATUS(packet, hci_create_connection)){ 2307 // cache outgoing address and reset 2308 (void)memcpy(address, l2cap_outgoing_classic_addr, 6); 2309 memset(l2cap_outgoing_classic_addr, 0, 6); 2310 // error => outgoing connection failed 2311 uint8_t status = hci_event_command_status_get_status(packet); 2312 if (status){ 2313 l2cap_handle_connection_failed_for_addr(address, status); 2314 } 2315 } 2316 #endif 2317 l2cap_run(); // try sending signaling packets first 2318 break; 2319 2320 #ifdef ENABLE_CLASSIC 2321 // handle connection complete events 2322 case HCI_EVENT_CONNECTION_COMPLETE: 2323 reverse_bd_addr(&packet[5], address); 2324 if (packet[2] == 0){ 2325 handle = little_endian_read_16(packet, 3); 2326 l2cap_handle_connection_success_for_addr(address, handle); 2327 } else { 2328 l2cap_handle_connection_failed_for_addr(address, packet[2]); 2329 } 2330 break; 2331 2332 // handle successful create connection cancel command 2333 case HCI_EVENT_COMMAND_COMPLETE: 2334 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 2335 if (packet[5] == 0){ 2336 reverse_bd_addr(&packet[6], address); 2337 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 2338 l2cap_handle_connection_failed_for_addr(address, 0x16); 2339 } 2340 } 2341 l2cap_run(); // try sending signaling packets first 2342 break; 2343 #endif 2344 2345 #ifdef L2CAP_USES_CHANNELS 2346 // handle disconnection complete events 2347 case HCI_EVENT_DISCONNECTION_COMPLETE: 2348 handle = little_endian_read_16(packet, 3); 2349 // send l2cap open failed or closed events for all channels on this handle and free them 2350 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2351 while (btstack_linked_list_iterator_has_next(&it)){ 2352 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2353 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2354 if (channel->con_handle != handle) continue; 2355 btstack_linked_list_iterator_remove(&it); 2356 switch(channel->channel_type){ 2357 #ifdef ENABLE_CLASSIC 2358 case L2CAP_CHANNEL_TYPE_CLASSIC: 2359 l2cap_handle_hci_disconnect_event(channel); 2360 break; 2361 #endif 2362 #ifdef ENABLE_LE_DATA_CHANNELS 2363 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2364 l2cap_handle_hci_le_disconnect_event(channel); 2365 break; 2366 #endif 2367 default: 2368 break; 2369 } 2370 } 2371 break; 2372 #endif 2373 2374 2375 // HCI Connection Timeouts 2376 #ifdef ENABLE_CLASSIC 2377 case L2CAP_EVENT_TIMEOUT_CHECK: 2378 handle = little_endian_read_16(packet, 2); 2379 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 2380 if (hci_authentication_active_for_handle(handle)) break; 2381 hci_con_used = 0; 2382 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2383 while (btstack_linked_list_iterator_has_next(&it)){ 2384 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2385 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2386 if (channel->con_handle != handle) continue; 2387 hci_con_used = 1; 2388 break; 2389 } 2390 if (hci_con_used) break; 2391 if (!hci_can_send_command_packet_now()) break; 2392 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 2393 break; 2394 2395 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 2396 case HCI_EVENT_READ_REMOTE_EXTENDED_FEATURES_COMPLETE: 2397 handle = little_endian_read_16(packet, 3); 2398 hci_connection = hci_connection_for_handle(handle); 2399 if (hci_connection == NULL) break; 2400 if ((hci_connection->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) == 0) break; 2401 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2402 while (btstack_linked_list_iterator_has_next(&it)){ 2403 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2404 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2405 if (channel->con_handle != handle) continue; 2406 log_info("remote supported features, channel %p, cid %04x - state %u", channel, channel->local_cid, channel->state); 2407 l2cap_handle_remote_supported_features_received(channel); 2408 } 2409 break; 2410 2411 case GAP_EVENT_SECURITY_LEVEL: 2412 handle = little_endian_read_16(packet, 2); 2413 log_info("l2cap - security level update for handle 0x%04x", handle); 2414 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2415 while (btstack_linked_list_iterator_has_next(&it)){ 2416 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2417 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2418 if (channel->con_handle != handle) continue; 2419 2420 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 2421 gap_security_level_t required_level = channel->required_security_level; 2422 2423 log_info("channel %p, cid %04x - state %u: actual %u >= required %u?", channel, channel->local_cid, channel->state, actual_level, required_level); 2424 2425 switch (channel->state){ 2426 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2427 if (actual_level >= required_level){ 2428 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2429 // we need to know if ERTM is supported before sending a config response 2430 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 2431 if (connection->l2cap_state.information_state != L2CAP_INFORMATION_STATE_DONE){ 2432 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2433 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 2434 break; 2435 } 2436 #endif 2437 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2438 l2cap_emit_incoming_connection(channel); 2439 } else { 2440 channel->reason = 0x0003; // security block 2441 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2442 } 2443 break; 2444 2445 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2446 if (actual_level >= required_level){ 2447 l2cap_ready_to_connect(channel); 2448 } else { 2449 // disconnnect, authentication not good enough 2450 hci_disconnect_security_block(handle); 2451 } 2452 break; 2453 2454 default: 2455 break; 2456 } 2457 } 2458 break; 2459 #endif 2460 2461 default: 2462 break; 2463 } 2464 2465 l2cap_run(); 2466 } 2467 2468 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 2469 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 2470 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 2471 signaling_responses[signaling_responses_pending].handle = handle; 2472 signaling_responses[signaling_responses_pending].code = code; 2473 signaling_responses[signaling_responses_pending].sig_id = sig_id; 2474 signaling_responses[signaling_responses_pending].cid = cid; 2475 signaling_responses[signaling_responses_pending].data = data; 2476 signaling_responses_pending++; 2477 l2cap_run(); 2478 } 2479 } 2480 2481 #ifdef ENABLE_CLASSIC 2482 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 2483 channel->remote_sig_id = identifier; 2484 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2485 l2cap_run(); 2486 } 2487 2488 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 2489 2490 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 2491 l2cap_service_t *service = l2cap_get_service(psm); 2492 if (!service) { 2493 // 0x0002 PSM not supported 2494 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2495 return; 2496 } 2497 2498 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 2499 if (!hci_connection) { 2500 // 2501 log_error("no hci_connection for handle %u", handle); 2502 return; 2503 } 2504 2505 // alloc structure 2506 // log_info("l2cap_handle_connection_request register channel"); 2507 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CLASSIC, hci_connection->address, BD_ADDR_TYPE_ACL, 2508 psm, service->mtu, service->required_security_level); 2509 if (!channel){ 2510 // 0x0004 No resources available 2511 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2512 return; 2513 } 2514 2515 channel->con_handle = handle; 2516 channel->remote_cid = source_cid; 2517 channel->remote_sig_id = sig_id; 2518 2519 // limit local mtu to max acl packet length - l2cap header 2520 if (channel->local_mtu > l2cap_max_mtu()) { 2521 channel->local_mtu = l2cap_max_mtu(); 2522 } 2523 2524 // set initial state 2525 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 2526 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 2527 2528 // add to connections list 2529 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2530 2531 // assert security requirements 2532 gap_request_security_level(handle, channel->required_security_level); 2533 } 2534 2535 void l2cap_accept_connection(uint16_t local_cid){ 2536 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 2537 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2538 if (!channel) { 2539 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 2540 return; 2541 } 2542 2543 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2544 // configure L2CAP Basic mode 2545 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2546 #endif 2547 2548 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 2549 2550 // process 2551 l2cap_run(); 2552 } 2553 2554 void l2cap_decline_connection(uint16_t local_cid){ 2555 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 2556 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 2557 if (!channel) { 2558 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 2559 return; 2560 } 2561 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2562 channel->reason = 0x04; // no resources available 2563 l2cap_run(); 2564 } 2565 2566 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2567 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 2568 2569 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2570 uint8_t use_fcs = 1; 2571 #endif 2572 2573 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2574 2575 uint16_t flags = little_endian_read_16(command, 6); 2576 if (flags & 1) { 2577 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 2578 } 2579 2580 // accept the other's configuration options 2581 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2582 uint16_t pos = 8; 2583 while (pos < end_pos){ 2584 uint8_t option_hint = command[pos] >> 7; 2585 uint8_t option_type = command[pos] & 0x7f; 2586 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2587 pos++; 2588 uint8_t length = command[pos++]; 2589 // MTU { type(8): 1, len(8):2, MTU(16) } 2590 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) && (length == 2)){ 2591 channel->remote_mtu = little_endian_read_16(command, pos); 2592 log_info("Remote MTU %u", channel->remote_mtu); 2593 if (channel->remote_mtu > l2cap_max_mtu()){ 2594 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 2595 channel->remote_mtu = l2cap_max_mtu(); 2596 } 2597 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2598 } 2599 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2600 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_FLUSH_TIMEOUT) && (length == 2)){ 2601 channel->flush_timeout = little_endian_read_16(command, pos); 2602 log_info("Flush timeout: %u ms", channel->flush_timeout); 2603 } 2604 2605 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2606 // Retransmission and Flow Control Option 2607 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2608 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2609 switch(channel->mode){ 2610 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2611 // Store remote config 2612 channel->remote_tx_window_size = command[pos+1]; 2613 channel->remote_max_transmit = command[pos+2]; 2614 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2615 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2616 channel->remote_mps = little_endian_read_16(command, pos + 7); 2617 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2618 channel->remote_tx_window_size, 2619 channel->remote_max_transmit, 2620 channel->remote_retransmission_timeout_ms, 2621 channel->remote_monitor_timeout_ms, 2622 channel->remote_mps); 2623 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2624 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2625 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2626 } else { 2627 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2628 } 2629 break; 2630 case L2CAP_CHANNEL_MODE_BASIC: 2631 switch (mode){ 2632 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2633 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2634 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2635 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2636 } 2637 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2638 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2639 break; 2640 default: // case L2CAP_CHANNEL_MODE_BASIC: 2641 // TODO store and evaluate configuration 2642 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2643 break; 2644 } 2645 break; 2646 default: 2647 break; 2648 } 2649 } 2650 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE && length == 1){ 2651 use_fcs = command[pos]; 2652 } 2653 #endif 2654 // check for unknown options 2655 if ((option_hint == 0) && ((option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) || (option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE))){ 2656 log_info("l2cap cid %u, unknown options", channel->local_cid); 2657 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2658 } 2659 pos += length; 2660 } 2661 2662 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2663 // "FCS" has precedence over "No FCS" 2664 uint8_t update = channel->fcs_option || use_fcs; 2665 log_info("local fcs: %u, remote fcs: %u -> %u", channel->fcs_option, use_fcs, update); 2666 channel->fcs_option = update; 2667 // If ERTM mandatory, but remote didn't send Retransmission and Flowcontrol options -> disconnect 2668 if (((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM) == 0) & (channel->ertm_mandatory)){ 2669 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2670 } 2671 #endif 2672 } 2673 2674 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2675 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2676 log_info("l2cap_signaling_handle_configure_response"); 2677 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2678 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2679 uint16_t pos = 10; 2680 while (pos < end_pos){ 2681 uint8_t option_hint = command[pos] >> 7; 2682 uint8_t option_type = command[pos] & 0x7f; 2683 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2684 pos++; 2685 uint8_t length = command[pos++]; 2686 2687 // Retransmission and Flow Control Option 2688 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2689 switch (channel->mode){ 2690 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2691 if (channel->ertm_mandatory){ 2692 // ?? 2693 } else { 2694 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2695 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2696 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2697 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2698 } 2699 } 2700 break; 2701 case L2CAP_CHANNEL_MODE_BASIC: 2702 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2703 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2704 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2705 } 2706 break; 2707 default: 2708 break; 2709 } 2710 } 2711 2712 // check for unknown options 2713 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2714 log_info("l2cap cid %u, unknown options", channel->local_cid); 2715 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2716 } 2717 2718 pos += length; 2719 } 2720 #else 2721 UNUSED(channel); // ok: no code 2722 UNUSED(result); // ok: no code 2723 UNUSED(command); // ok: no code 2724 #endif 2725 } 2726 2727 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2728 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2729 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2730 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2731 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2732 if (channel->state == L2CAP_STATE_OPEN) return 0; 2733 return 1; 2734 } 2735 2736 2737 // @pre command len is valid, see check in l2cap_signaling_handler_dispatch 2738 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2739 2740 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2741 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2742 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2743 uint16_t result = 0; 2744 2745 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2746 2747 // handle DISCONNECT REQUESTS seperately 2748 if (code == DISCONNECTION_REQUEST){ 2749 switch (channel->state){ 2750 case L2CAP_STATE_CONFIG: 2751 case L2CAP_STATE_OPEN: 2752 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2753 case L2CAP_STATE_WAIT_DISCONNECT: 2754 l2cap_handle_disconnect_request(channel, identifier); 2755 break; 2756 2757 default: 2758 // ignore in other states 2759 break; 2760 } 2761 return; 2762 } 2763 2764 // @STATEMACHINE(l2cap) 2765 switch (channel->state) { 2766 2767 case L2CAP_STATE_WAIT_CONNECT_RSP: 2768 switch (code){ 2769 case CONNECTION_RESPONSE: 2770 if (cmd_len < 8){ 2771 // command imcomplete 2772 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2773 break; 2774 } 2775 l2cap_stop_rtx(channel); 2776 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2777 switch (result) { 2778 case 0: 2779 // successful connection 2780 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2781 channel->state = L2CAP_STATE_CONFIG; 2782 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2783 break; 2784 case 1: 2785 // connection pending. get some coffee, but start the ERTX 2786 l2cap_start_ertx(channel); 2787 break; 2788 default: 2789 // channel closed 2790 channel->state = L2CAP_STATE_CLOSED; 2791 // map l2cap connection response result to BTstack status enumeration 2792 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2793 2794 // drop link key if security block 2795 if ((L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result) == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2796 gap_drop_link_key_for_bd_addr(channel->address); 2797 } 2798 2799 // discard channel 2800 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2801 l2cap_free_channel_entry(channel); 2802 break; 2803 } 2804 break; 2805 2806 default: 2807 //@TODO: implement other signaling packets 2808 break; 2809 } 2810 break; 2811 2812 case L2CAP_STATE_CONFIG: 2813 switch (code) { 2814 case CONFIGURE_REQUEST: 2815 if (cmd_len < 4){ 2816 // command incomplete 2817 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2818 break; 2819 } 2820 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 2821 l2cap_signaling_handle_configure_request(channel, command); 2822 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 2823 // only done if continuation not set 2824 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 2825 } 2826 break; 2827 case CONFIGURE_RESPONSE: 2828 if (cmd_len < 6){ 2829 // command incomplete 2830 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2831 break; 2832 } 2833 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2834 l2cap_stop_rtx(channel); 2835 l2cap_signaling_handle_configure_response(channel, result, command); 2836 switch (result){ 2837 case 0: // success 2838 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 2839 break; 2840 case 4: // pending 2841 l2cap_start_ertx(channel); 2842 break; 2843 default: 2844 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2845 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 2846 // remote does not offer ertm but it's required 2847 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2848 break; 2849 } 2850 #endif 2851 // retry on negative result 2852 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2853 break; 2854 } 2855 break; 2856 default: 2857 break; 2858 } 2859 if (l2cap_channel_ready_for_open(channel)){ 2860 2861 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2862 // assert that packet can be stored in fragment buffers in ertm 2863 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2864 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 2865 uint16_t usable_mtu = channel->num_tx_buffers == 1 ? effective_mps : channel->num_tx_buffers * effective_mps - 2; 2866 if (usable_mtu < channel->remote_mtu){ 2867 log_info("Remote MTU %u > max storable ERTM packet, only using MTU = %u", channel->remote_mtu, usable_mtu); 2868 channel->remote_mtu = usable_mtu; 2869 } 2870 } 2871 #endif 2872 // for open: 2873 channel->state = L2CAP_STATE_OPEN; 2874 l2cap_emit_channel_opened(channel, 0); 2875 } 2876 break; 2877 2878 case L2CAP_STATE_WAIT_DISCONNECT: 2879 switch (code) { 2880 case DISCONNECTION_RESPONSE: 2881 l2cap_finialize_channel_close(channel); 2882 break; 2883 default: 2884 //@TODO: implement other signaling packets 2885 break; 2886 } 2887 break; 2888 2889 case L2CAP_STATE_CLOSED: 2890 // @TODO handle incoming requests 2891 break; 2892 2893 case L2CAP_STATE_OPEN: 2894 //@TODO: implement other signaling packets, e.g. re-configure 2895 break; 2896 default: 2897 break; 2898 } 2899 // log_info("new state %u", channel->state); 2900 } 2901 2902 2903 // @pre command len is valid, see check in l2cap_acl_classic_handler 2904 static void l2cap_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command){ 2905 2906 btstack_linked_list_iterator_t it; 2907 2908 // get code, signalind identifier and command len 2909 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2910 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2911 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2912 2913 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 2914 if ((code < 1) || (code == ECHO_RESPONSE) || (code > INFORMATION_RESPONSE)){ 2915 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2916 return; 2917 } 2918 2919 // general commands without an assigned channel 2920 switch(code) { 2921 2922 case CONNECTION_REQUEST: 2923 if (cmd_len == 4){ 2924 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2925 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2926 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 2927 } else { 2928 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2929 } 2930 return; 2931 2932 case ECHO_REQUEST: 2933 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 2934 return; 2935 2936 case INFORMATION_REQUEST: 2937 if (cmd_len == 2) { 2938 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2939 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 2940 } else { 2941 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2942 } 2943 return; 2944 2945 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2946 case INFORMATION_RESPONSE: { 2947 hci_connection_t * connection = hci_connection_for_handle(handle); 2948 if (!connection) return; 2949 if (connection->l2cap_state.information_state != L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE) return; 2950 2951 // get extended features from response if valid 2952 connection->l2cap_state.extended_feature_mask = 0; 2953 if (cmd_len >= 6) { 2954 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2955 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2956 if (result == 0 && info_type == L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) { 2957 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2958 } 2959 } 2960 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2961 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2962 2963 // trigger connection request 2964 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2965 while (btstack_linked_list_iterator_has_next(&it)){ 2966 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2967 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2968 if (channel->con_handle != handle) continue; 2969 2970 // incoming connection: ask user for channel configuration, esp. if ertm will be mandatory 2971 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2972 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2973 l2cap_emit_incoming_connection(channel); 2974 continue; 2975 } 2976 2977 // outgoing connection 2978 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2979 2980 // if ERTM was requested, but is not listed in extended feature mask: 2981 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2982 2983 if (channel->ertm_mandatory){ 2984 // bail if ERTM is mandatory 2985 channel->state = L2CAP_STATE_CLOSED; 2986 // map l2cap connection response result to BTstack status enumeration 2987 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 2988 // discard channel 2989 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2990 l2cap_free_channel_entry(channel); 2991 continue; 2992 2993 } else { 2994 // fallback to Basic mode 2995 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2996 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2997 } 2998 } 2999 3000 // respond to connection request 3001 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 3002 continue; 3003 } 3004 } 3005 return; 3006 } 3007 #endif 3008 3009 default: 3010 break; 3011 } 3012 3013 // Get potential destination CID 3014 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3015 3016 // Find channel for this sig_id and connection handle 3017 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3018 while (btstack_linked_list_iterator_has_next(&it)){ 3019 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3020 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3021 if (channel->con_handle != handle) continue; 3022 if (code & 1) { 3023 // match odd commands (responses) by previous signaling identifier 3024 if (channel->local_sig_id == sig_id) { 3025 l2cap_signaling_handler_channel(channel, command); 3026 break; 3027 } 3028 } else { 3029 // match even commands (requests) by local channel id 3030 if (channel->local_cid == dest_cid) { 3031 l2cap_signaling_handler_channel(channel, command); 3032 break; 3033 } 3034 } 3035 } 3036 } 3037 #endif 3038 3039 #ifdef ENABLE_BLE 3040 3041 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 3042 uint8_t event[6]; 3043 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 3044 event[1] = 4; 3045 little_endian_store_16(event, 2, con_handle); 3046 little_endian_store_16(event, 4, result); 3047 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3048 if (!l2cap_event_packet_handler) return; 3049 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 3050 } 3051 3052 // @returns valid 3053 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 3054 hci_connection_t * connection; 3055 uint16_t result; 3056 uint8_t event[12]; 3057 3058 #ifdef ENABLE_LE_DATA_CHANNELS 3059 btstack_linked_list_iterator_t it; 3060 l2cap_channel_t * channel; 3061 uint16_t local_cid; 3062 uint16_t le_psm; 3063 uint16_t new_credits; 3064 uint16_t credits_before; 3065 l2cap_service_t * service; 3066 uint16_t source_cid; 3067 #endif 3068 3069 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3070 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3071 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 3072 3073 switch (code){ 3074 3075 case CONNECTION_PARAMETER_UPDATE_REQUEST: 3076 // check size 3077 if (len < 8u) return 0u; 3078 connection = hci_connection_for_handle(handle); 3079 if (connection){ 3080 if (connection->role != HCI_ROLE_MASTER){ 3081 // reject command without notifying upper layer when not in master role 3082 return 0; 3083 } 3084 le_connection_parameter_range_t existing_range; 3085 gap_get_connection_parameter_range(&existing_range); 3086 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3087 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3088 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3089 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 3090 3091 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 3092 if (update_parameter){ 3093 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 3094 connection->le_conn_interval_min = le_conn_interval_min; 3095 connection->le_conn_interval_max = le_conn_interval_max; 3096 connection->le_conn_latency = le_conn_latency; 3097 connection->le_supervision_timeout = le_supervision_timeout; 3098 } else { 3099 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 3100 } 3101 connection->le_con_param_update_identifier = sig_id; 3102 } 3103 3104 if (!l2cap_event_packet_handler) break; 3105 3106 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 3107 event[1] = 8; 3108 little_endian_store_16(event, 2, handle); 3109 (void)memcpy(&event[4], &command[4], 8); 3110 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3111 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3112 break; 3113 3114 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 3115 // check size 3116 if (len < 2u) return 0u; 3117 result = little_endian_read_16(command, 4); 3118 l2cap_emit_connection_parameter_update_response(handle, result); 3119 break; 3120 3121 #ifdef ENABLE_LE_DATA_CHANNELS 3122 3123 case COMMAND_REJECT: 3124 // Find channel for this sig_id and connection handle 3125 channel = NULL; 3126 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3127 while (btstack_linked_list_iterator_has_next(&it)){ 3128 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3129 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3130 if (a_channel->con_handle != handle) continue; 3131 if (a_channel->local_sig_id != sig_id) continue; 3132 channel = a_channel; 3133 break; 3134 } 3135 if (!channel) break; 3136 3137 // if received while waiting for le connection response, assume legacy device 3138 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 3139 channel->state = L2CAP_STATE_CLOSED; 3140 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 3141 l2cap_emit_le_channel_opened(channel, 0x0002); 3142 3143 // discard channel 3144 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3145 l2cap_free_channel_entry(channel); 3146 break; 3147 } 3148 break; 3149 3150 case LE_CREDIT_BASED_CONNECTION_REQUEST: 3151 // check size 3152 if (len < 10u) return 0u; 3153 3154 // get hci connection, bail if not found (must not happen) 3155 connection = hci_connection_for_handle(handle); 3156 if (!connection) return 0; 3157 3158 // check if service registered 3159 le_psm = little_endian_read_16(command, 4); 3160 service = l2cap_le_get_service(le_psm); 3161 source_cid = little_endian_read_16(command, 6); 3162 3163 if (service){ 3164 if (source_cid < 0x40u){ 3165 // 0x0009 Connection refused - Invalid Source CID 3166 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 3167 return 1; 3168 } 3169 3170 // go through list of channels for this ACL connection and check if we get a match 3171 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3172 while (btstack_linked_list_iterator_has_next(&it)){ 3173 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3174 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3175 if (a_channel->con_handle != handle) continue; 3176 if (a_channel->remote_cid != source_cid) continue; 3177 // 0x000a Connection refused - Source CID already allocated 3178 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 3179 return 1; 3180 } 3181 3182 // security: check encryption 3183 if (service->required_security_level >= LEVEL_2){ 3184 if (gap_encryption_key_size(handle) == 0){ 3185 // 0x0008 Connection refused - insufficient encryption 3186 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 3187 return 1; 3188 } 3189 // anything less than 16 byte key size is insufficient 3190 if (gap_encryption_key_size(handle) < 16){ 3191 // 0x0007 Connection refused – insufficient encryption key size 3192 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 3193 return 1; 3194 } 3195 } 3196 3197 // security: check authencation 3198 if (service->required_security_level >= LEVEL_3){ 3199 if (!gap_authenticated(handle)){ 3200 // 0x0005 Connection refused – insufficient authentication 3201 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 3202 return 1; 3203 } 3204 } 3205 3206 // security: check authorization 3207 if (service->required_security_level >= LEVEL_4){ 3208 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 3209 // 0x0006 Connection refused – insufficient authorization 3210 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 3211 return 1; 3212 } 3213 } 3214 3215 // allocate channel 3216 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 3217 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 3218 if (!channel){ 3219 // 0x0004 Connection refused – no resources available 3220 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 3221 return 1; 3222 } 3223 3224 channel->con_handle = handle; 3225 channel->remote_cid = source_cid; 3226 channel->remote_sig_id = sig_id; 3227 channel->remote_mtu = little_endian_read_16(command, 8); 3228 channel->remote_mps = little_endian_read_16(command, 10); 3229 channel->credits_outgoing = little_endian_read_16(command, 12); 3230 3231 // set initial state 3232 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3233 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3234 3235 // add to connections list 3236 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 3237 3238 // post connection request event 3239 l2cap_emit_le_incoming_connection(channel); 3240 3241 } else { 3242 // Connection refused – LE_PSM not supported 3243 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 3244 } 3245 break; 3246 3247 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 3248 // check size 3249 if (len < 10u) return 0u; 3250 3251 // Find channel for this sig_id and connection handle 3252 channel = NULL; 3253 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3254 while (btstack_linked_list_iterator_has_next(&it)){ 3255 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3256 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3257 if (a_channel->con_handle != handle) continue; 3258 if (a_channel->local_sig_id != sig_id) continue; 3259 channel = a_channel; 3260 break; 3261 } 3262 if (!channel) break; 3263 3264 // cid + 0 3265 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 3266 if (result){ 3267 channel->state = L2CAP_STATE_CLOSED; 3268 // map l2cap connection response result to BTstack status enumeration 3269 l2cap_emit_le_channel_opened(channel, result); 3270 3271 // discard channel 3272 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3273 l2cap_free_channel_entry(channel); 3274 break; 3275 } 3276 3277 // success 3278 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3279 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3280 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3281 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3282 channel->state = L2CAP_STATE_OPEN; 3283 l2cap_emit_le_channel_opened(channel, result); 3284 break; 3285 3286 case LE_FLOW_CONTROL_CREDIT: 3287 // check size 3288 if (len < 4u) return 0u; 3289 3290 // find channel 3291 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3292 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3293 if (!channel) { 3294 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3295 break; 3296 } 3297 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3298 credits_before = channel->credits_outgoing; 3299 channel->credits_outgoing += new_credits; 3300 // check for credit overrun 3301 if (credits_before > channel->credits_outgoing){ 3302 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 3303 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3304 break; 3305 } 3306 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 3307 break; 3308 3309 case DISCONNECTION_REQUEST: 3310 3311 // check size 3312 if (len < 4u) return 0u; 3313 3314 // find channel 3315 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3316 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3317 if (!channel) { 3318 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3319 break; 3320 } 3321 channel->remote_sig_id = sig_id; 3322 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3323 break; 3324 3325 #endif 3326 3327 case DISCONNECTION_RESPONSE: 3328 break; 3329 3330 default: 3331 // command unknown -> reject command 3332 return 0; 3333 } 3334 return 1; 3335 } 3336 #endif 3337 3338 #ifdef ENABLE_CLASSIC 3339 static void l2cap_acl_classic_handler_for_channel(l2cap_channel_t * l2cap_channel, uint8_t * packet, uint16_t size){ 3340 3341 // forward data only in OPEN state 3342 if (l2cap_channel->state != L2CAP_STATE_OPEN) return; 3343 3344 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3345 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3346 3347 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3348 3349 // assert control + FCS fields are inside 3350 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) return; 3351 3352 if (l2cap_channel->fcs_option){ 3353 // verify FCS (required if one side requested it) 3354 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3355 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3356 3357 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 3358 // simulate fcs error 3359 static int counter = 0; 3360 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 3361 log_info("Simulate fcs error"); 3362 fcs_calculated++; 3363 counter = 0; 3364 } 3365 #endif 3366 3367 if (fcs_calculated == fcs_packet){ 3368 log_info("Packet FCS 0x%04x verified", fcs_packet); 3369 } else { 3370 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3371 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 3372 return; 3373 } 3374 } 3375 3376 // switch on packet type 3377 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3378 uint8_t req_seq = (control >> 8) & 0x3f; 3379 int final = (control >> 7) & 0x01; 3380 if (control & 1){ 3381 // S-Frame 3382 int poll = (control >> 4) & 0x01; 3383 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3384 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3385 l2cap_ertm_tx_packet_state_t * tx_state; 3386 switch (s){ 3387 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3388 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3389 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3390 if (poll && final){ 3391 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3392 log_error("P=F=1 in S-Frame"); 3393 break; 3394 } 3395 if (poll){ 3396 // check if we did request selective retransmission before <==> we have stored SDU segments 3397 int i; 3398 int num_stored_out_of_order_packets = 0; 3399 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3400 int index = l2cap_channel->rx_store_index + i; 3401 if (index >= l2cap_channel->num_rx_buffers){ 3402 index -= l2cap_channel->num_rx_buffers; 3403 } 3404 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3405 if (!rx_state->valid) continue; 3406 num_stored_out_of_order_packets++; 3407 } 3408 if (num_stored_out_of_order_packets){ 3409 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3410 } else { 3411 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3412 } 3413 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3414 } 3415 if (final){ 3416 // Stop-MonitorTimer 3417 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3418 // If UnackedFrames > 0 then Start-RetransTimer 3419 if (l2cap_channel->unacked_frames){ 3420 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3421 } 3422 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3423 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3424 } 3425 break; 3426 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3427 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3428 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3429 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3430 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3431 break; 3432 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3433 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3434 break; 3435 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3436 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3437 if (poll){ 3438 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3439 } 3440 // find requested i-frame 3441 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3442 if (tx_state){ 3443 log_info("Retransmission for tx_seq %u requested", req_seq); 3444 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3445 tx_state->retransmission_requested = 1; 3446 l2cap_channel->srej_active = 1; 3447 } 3448 break; 3449 default: 3450 break; 3451 } 3452 } else { 3453 // I-Frame 3454 // get control 3455 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3456 uint8_t tx_seq = (control >> 1) & 0x3f; 3457 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3458 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3459 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3460 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3461 if (final){ 3462 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3463 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3464 } 3465 3466 // get SDU 3467 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3468 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3469 3470 // assert SDU size is smaller or equal to our buffers 3471 uint16_t max_payload_size = 0; 3472 switch (sar){ 3473 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 3474 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 3475 // SDU Length + MPS 3476 max_payload_size = l2cap_channel->local_mps + 2; 3477 break; 3478 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 3479 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 3480 max_payload_size = l2cap_channel->local_mps; 3481 break; 3482 default: 3483 btstack_assert(false); 3484 break; 3485 } 3486 if (payload_len > max_payload_size){ 3487 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 3488 return; 3489 } 3490 3491 // check ordering 3492 if (l2cap_channel->expected_tx_seq == tx_seq){ 3493 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3494 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3495 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3496 3497 // process SDU 3498 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 3499 3500 // process stored segments 3501 while (true){ 3502 int index = l2cap_channel->rx_store_index; 3503 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3504 if (!rx_state->valid) break; 3505 3506 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3507 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3508 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3509 3510 rx_state->valid = 0; 3511 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3512 3513 // update rx store index 3514 index++; 3515 if (index >= l2cap_channel->num_rx_buffers){ 3516 index = 0; 3517 } 3518 l2cap_channel->rx_store_index = index; 3519 } 3520 3521 // 3522 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3523 3524 } else { 3525 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3526 if (delta < 2){ 3527 // store segment 3528 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 3529 3530 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3531 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3532 } else { 3533 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3534 l2cap_channel->send_supervisor_frame_reject = 1; 3535 } 3536 } 3537 } 3538 return; 3539 } 3540 #endif 3541 3542 // check size 3543 if (l2cap_channel->local_mtu < size) return; 3544 3545 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3546 } 3547 #endif 3548 3549 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3550 #ifdef ENABLE_CLASSIC 3551 l2cap_channel_t * l2cap_channel; 3552 l2cap_fixed_channel_t * l2cap_fixed_channel; 3553 3554 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3555 uint8_t broadcast_flag = READ_ACL_FLAGS(packet) >> 2; 3556 switch (channel_id) { 3557 3558 case L2CAP_CID_SIGNALING: { 3559 if (broadcast_flag != 0) break; 3560 uint32_t command_offset = 8; 3561 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 3562 // assert signaling command is fully inside packet 3563 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3564 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3565 if (next_command_offset > size){ 3566 log_error("l2cap signaling command len invalid -> drop"); 3567 break; 3568 } 3569 // handle signaling command 3570 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3571 // go to next command 3572 command_offset = next_command_offset; 3573 } 3574 break; 3575 } 3576 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3577 if (broadcast_flag == 0) break; 3578 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3579 if (!l2cap_fixed_channel) break; 3580 if (!l2cap_fixed_channel->packet_handler) break; 3581 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3582 break; 3583 3584 default: 3585 if (broadcast_flag != 0) break; 3586 // Find channel for this channel_id and connection handle 3587 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3588 if (l2cap_channel != NULL){ 3589 l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size); 3590 } 3591 break; 3592 } 3593 #else 3594 UNUSED(handle); // ok: no code 3595 UNUSED(packet); // ok: no code 3596 UNUSED(size); // ok: no code 3597 #endif 3598 } 3599 3600 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3601 #ifdef ENABLE_BLE 3602 3603 l2cap_fixed_channel_t * l2cap_fixed_channel; 3604 3605 #ifdef ENABLE_LE_DATA_CHANNELS 3606 l2cap_channel_t * l2cap_channel; 3607 #endif 3608 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3609 switch (channel_id) { 3610 3611 case L2CAP_CID_SIGNALING_LE: { 3612 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3613 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3614 if ((COMPLETE_L2CAP_HEADER + 4u + len) > size) break; 3615 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3616 if (!valid){ 3617 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3618 } 3619 break; 3620 } 3621 3622 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3623 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3624 if (!l2cap_fixed_channel) break; 3625 if (!l2cap_fixed_channel->packet_handler) break; 3626 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3627 break; 3628 3629 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3630 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3631 if (!l2cap_fixed_channel) break; 3632 if (!l2cap_fixed_channel->packet_handler) break; 3633 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3634 break; 3635 3636 default: 3637 3638 #ifdef ENABLE_LE_DATA_CHANNELS 3639 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3640 if (l2cap_channel != NULL) { 3641 // credit counting 3642 if (l2cap_channel->credits_incoming == 0u){ 3643 log_info("LE Data Channel packet received but no incoming credits"); 3644 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3645 break; 3646 } 3647 l2cap_channel->credits_incoming--; 3648 3649 // automatic credits 3650 if ((l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK) && l2cap_channel->automatic_credits){ 3651 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3652 } 3653 3654 // first fragment 3655 uint16_t pos = 0; 3656 if (!l2cap_channel->receive_sdu_len){ 3657 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3658 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3659 l2cap_channel->receive_sdu_len = sdu_len; 3660 l2cap_channel->receive_sdu_pos = 0; 3661 pos += 2u; 3662 size -= 2u; 3663 } 3664 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3665 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3666 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3667 (void)memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], 3668 &packet[COMPLETE_L2CAP_HEADER + pos], 3669 fragment_size); 3670 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3671 // done? 3672 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3673 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3674 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3675 l2cap_channel->receive_sdu_len = 0; 3676 } 3677 } 3678 #endif 3679 break; 3680 } 3681 #else 3682 UNUSED(handle); // ok: no code 3683 UNUSED(packet); // ok: no code 3684 UNUSED(size); // ok: no code 3685 #endif 3686 } 3687 3688 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3689 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3690 UNUSED(channel); // ok: there is no channel 3691 3692 // Assert full L2CAP header present 3693 if (size < COMPLETE_L2CAP_HEADER) return; 3694 3695 // Dispatch to Classic or LE handler (SCO packets are not dispatched to L2CAP) 3696 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3697 hci_connection_t *conn = hci_connection_for_handle(handle); 3698 if (!conn) return; 3699 if (conn->address_type == BD_ADDR_TYPE_ACL){ 3700 l2cap_acl_classic_handler(handle, packet, size); 3701 } else { 3702 l2cap_acl_le_handler(handle, packet, size); 3703 } 3704 3705 l2cap_run(); 3706 } 3707 3708 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3709 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 3710 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3711 if (!channel) return; 3712 channel->packet_handler = the_packet_handler; 3713 } 3714 3715 #ifdef ENABLE_CLASSIC 3716 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3717 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3718 channel->state = L2CAP_STATE_CLOSED; 3719 l2cap_handle_channel_closed(channel); 3720 // discard channel 3721 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3722 l2cap_free_channel_entry(channel); 3723 } 3724 #endif 3725 3726 #ifdef L2CAP_USES_CHANNELS 3727 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3728 btstack_linked_list_iterator_t it; 3729 btstack_linked_list_iterator_init(&it, services); 3730 while (btstack_linked_list_iterator_has_next(&it)){ 3731 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3732 if ( service->psm == psm){ 3733 return service; 3734 }; 3735 } 3736 return NULL; 3737 } 3738 #endif 3739 3740 #ifdef ENABLE_CLASSIC 3741 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3742 return l2cap_get_service_internal(&l2cap_services, psm); 3743 } 3744 3745 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3746 3747 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3748 3749 // check for alread registered psm 3750 l2cap_service_t *service = l2cap_get_service(psm); 3751 if (service) { 3752 log_error("l2cap_register_service: PSM %u already registered", psm); 3753 return L2CAP_SERVICE_ALREADY_REGISTERED; 3754 } 3755 3756 // alloc structure 3757 service = btstack_memory_l2cap_service_get(); 3758 if (!service) { 3759 log_error("l2cap_register_service: no memory for l2cap_service_t"); 3760 return BTSTACK_MEMORY_ALLOC_FAILED; 3761 } 3762 3763 // fill in 3764 service->psm = psm; 3765 service->mtu = mtu; 3766 service->packet_handler = service_packet_handler; 3767 service->required_security_level = security_level; 3768 3769 // add to services list 3770 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3771 3772 // enable page scan 3773 gap_connectable_control(1); 3774 3775 return ERROR_CODE_SUCCESS; 3776 } 3777 3778 uint8_t l2cap_unregister_service(uint16_t psm){ 3779 3780 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 3781 3782 l2cap_service_t *service = l2cap_get_service(psm); 3783 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3784 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 3785 btstack_memory_l2cap_service_free(service); 3786 3787 // disable page scan when no services registered 3788 if (btstack_linked_list_empty(&l2cap_services)) { 3789 gap_connectable_control(0); 3790 } 3791 return ERROR_CODE_SUCCESS; 3792 } 3793 #endif 3794 3795 3796 #ifdef ENABLE_LE_DATA_CHANNELS 3797 3798 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 3799 if (!channel->waiting_for_can_send_now) return; 3800 if (channel->send_sdu_buffer) return; 3801 channel->waiting_for_can_send_now = 0; 3802 log_debug("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 3803 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 3804 } 3805 3806 // 1BH2222 3807 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 3808 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", 3809 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 3810 uint8_t event[19]; 3811 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 3812 event[1] = sizeof(event) - 2u; 3813 event[2] = channel->address_type; 3814 reverse_bd_addr(channel->address, &event[3]); 3815 little_endian_store_16(event, 9, channel->con_handle); 3816 little_endian_store_16(event, 11, channel->psm); 3817 little_endian_store_16(event, 13, channel->local_cid); 3818 little_endian_store_16(event, 15, channel->remote_cid); 3819 little_endian_store_16(event, 17, channel->remote_mtu); 3820 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3821 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3822 } 3823 // 11BH22222 3824 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 3825 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", 3826 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 3827 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 3828 uint8_t event[23]; 3829 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 3830 event[1] = sizeof(event) - 2u; 3831 event[2] = status; 3832 event[3] = channel->address_type; 3833 reverse_bd_addr(channel->address, &event[4]); 3834 little_endian_store_16(event, 10, channel->con_handle); 3835 event[12] = (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) ? 1 : 0; 3836 little_endian_store_16(event, 13, channel->psm); 3837 little_endian_store_16(event, 15, channel->local_cid); 3838 little_endian_store_16(event, 17, channel->remote_cid); 3839 little_endian_store_16(event, 19, channel->local_mtu); 3840 little_endian_store_16(event, 21, channel->remote_mtu); 3841 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3842 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3843 } 3844 // 2 3845 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 3846 log_info("L2CAP_EVENT_LE_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 3847 uint8_t event[4]; 3848 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 3849 event[1] = sizeof(event) - 2u; 3850 little_endian_store_16(event, 2, channel->local_cid); 3851 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3852 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3853 } 3854 3855 static void l2cap_le_send_pdu(l2cap_channel_t *channel){ 3856 btstack_assert(channel != NULL); 3857 btstack_assert(channel->send_sdu_buffer != NULL); 3858 btstack_assert(channel->credits_outgoing > 0); 3859 3860 // send part of SDU 3861 hci_reserve_packet_buffer(); 3862 uint8_t * acl_buffer = hci_get_outgoing_packet_buffer(); 3863 uint8_t * l2cap_payload = acl_buffer + 8; 3864 uint16_t pos = 0; 3865 if (!channel->send_sdu_pos){ 3866 // store SDU len 3867 channel->send_sdu_pos += 2u; 3868 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 3869 pos += 2u; 3870 } 3871 uint16_t payload_size = btstack_min(channel->send_sdu_len + 2u - channel->send_sdu_pos, channel->remote_mps - pos); 3872 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 3873 (void)memcpy(&l2cap_payload[pos], 3874 &channel->send_sdu_buffer[channel->send_sdu_pos - 2u], 3875 payload_size); // -2 for virtual SDU len 3876 pos += payload_size; 3877 channel->send_sdu_pos += payload_size; 3878 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 3879 3880 channel->credits_outgoing--; 3881 3882 hci_send_acl_packet_buffer(8u + pos); 3883 3884 if (channel->send_sdu_pos >= (channel->send_sdu_len + 2u)){ 3885 channel->send_sdu_buffer = NULL; 3886 // send done event 3887 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 3888 // inform about can send now 3889 l2cap_le_notify_channel_can_send(channel); 3890 } 3891 } 3892 3893 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3894 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 3895 channel->state = L2CAP_STATE_CLOSED; 3896 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 3897 // discard channel 3898 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3899 l2cap_free_channel_entry(channel); 3900 } 3901 3902 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 3903 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 3904 } 3905 3906 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 3907 3908 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 3909 3910 // check for alread registered psm 3911 l2cap_service_t *service = l2cap_le_get_service(psm); 3912 if (service) { 3913 return L2CAP_SERVICE_ALREADY_REGISTERED; 3914 } 3915 3916 // alloc structure 3917 service = btstack_memory_l2cap_service_get(); 3918 if (!service) { 3919 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 3920 return BTSTACK_MEMORY_ALLOC_FAILED; 3921 } 3922 3923 // fill in 3924 service->psm = psm; 3925 service->mtu = 0; 3926 service->packet_handler = packet_handler; 3927 service->required_security_level = security_level; 3928 3929 // add to services list 3930 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 3931 3932 // done 3933 return ERROR_CODE_SUCCESS; 3934 } 3935 3936 uint8_t l2cap_le_unregister_service(uint16_t psm) { 3937 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 3938 l2cap_service_t *service = l2cap_le_get_service(psm); 3939 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3940 3941 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 3942 btstack_memory_l2cap_service_free(service); 3943 return ERROR_CODE_SUCCESS; 3944 } 3945 3946 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 3947 // get channel 3948 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3949 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3950 3951 // validate state 3952 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3953 return ERROR_CODE_COMMAND_DISALLOWED; 3954 } 3955 3956 // set state accept connection 3957 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 3958 channel->receive_sdu_buffer = receive_sdu_buffer; 3959 channel->local_mtu = mtu; 3960 channel->new_credits_incoming = initial_credits; 3961 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3962 3963 // test 3964 // channel->new_credits_incoming = 1; 3965 3966 // go 3967 l2cap_run(); 3968 return ERROR_CODE_SUCCESS; 3969 } 3970 3971 /** 3972 * @brief Deny incoming LE Data Channel connection due to resource constraints 3973 * @param local_cid L2CAP LE Data Channel Identifier 3974 */ 3975 3976 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 3977 // get channel 3978 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3979 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3980 3981 // validate state 3982 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3983 return ERROR_CODE_COMMAND_DISALLOWED; 3984 } 3985 3986 // set state decline connection 3987 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 3988 channel->reason = 0x04; // no resources available 3989 l2cap_run(); 3990 return ERROR_CODE_SUCCESS; 3991 } 3992 3993 static gap_security_level_t l2cap_le_security_level_for_connection(hci_con_handle_t con_handle){ 3994 uint8_t encryption_key_size = gap_encryption_key_size(con_handle); 3995 if (encryption_key_size == 0) return LEVEL_0; 3996 3997 uint8_t authenticated = gap_authenticated(con_handle); 3998 if (!authenticated) return LEVEL_2; 3999 4000 return encryption_key_size == 16 ? LEVEL_4 : LEVEL_3; 4001 } 4002 4003 // used to handle pairing complete after triggering to increase 4004 static void l2cap_sm_packet_handler(uint8_t packet_type, uint16_t channel_nr, uint8_t *packet, uint16_t size) { 4005 UNUSED(channel_nr); 4006 UNUSED(size); 4007 UNUSED(packet_type); 4008 btstack_assert(packet_type == HCI_EVENT_PACKET); 4009 if (hci_event_packet_get_type(packet) != SM_EVENT_PAIRING_COMPLETE) return; 4010 hci_con_handle_t con_handle = sm_event_pairing_complete_get_handle(packet); 4011 btstack_linked_list_iterator_t it; 4012 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4013 while (btstack_linked_list_iterator_has_next(&it)) { 4014 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4015 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 4016 if (channel->con_handle != con_handle) continue; 4017 if (channel->state != L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE) continue; 4018 4019 // found channel, check security level 4020 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4021 // pairing failed or wasn't good enough, inform user 4022 l2cap_emit_le_channel_opened(channel, ERROR_CODE_INSUFFICIENT_SECURITY); 4023 // discard channel 4024 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4025 l2cap_free_channel_entry(channel); 4026 } else { 4027 // send conn request now 4028 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4029 l2cap_run(); 4030 } 4031 } 4032 } 4033 4034 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 4035 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 4036 uint16_t * out_local_cid) { 4037 4038 static btstack_packet_callback_registration_t sm_event_callback_registration; 4039 static bool sm_callback_registered = false; 4040 4041 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 4042 4043 hci_connection_t * connection = hci_connection_for_handle(con_handle); 4044 if (!connection) { 4045 log_error("no hci_connection for handle 0x%04x", con_handle); 4046 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 4047 } 4048 4049 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); 4050 if (!channel) { 4051 return BTSTACK_MEMORY_ALLOC_FAILED; 4052 } 4053 log_info("l2cap_le_create_channel %p", channel); 4054 4055 // store local_cid 4056 if (out_local_cid){ 4057 *out_local_cid = channel->local_cid; 4058 } 4059 4060 // setup channel entry 4061 channel->con_handle = con_handle; 4062 channel->receive_sdu_buffer = receive_sdu_buffer; 4063 channel->new_credits_incoming = initial_credits; 4064 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4065 4066 // add to connections list 4067 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 4068 4069 // check security level 4070 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4071 if (!sm_callback_registered){ 4072 sm_callback_registered = true; 4073 // lazy registration for SM events 4074 sm_event_callback_registration.callback = &l2cap_sm_packet_handler; 4075 sm_add_event_handler(&sm_event_callback_registration); 4076 } 4077 4078 // start pairing 4079 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 4080 sm_request_pairing(con_handle); 4081 } else { 4082 // send conn request right away 4083 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4084 l2cap_run(); 4085 } 4086 4087 return ERROR_CODE_SUCCESS; 4088 } 4089 4090 /** 4091 * @brief Provide credtis for LE Data Channel 4092 * @param local_cid L2CAP LE Data Channel Identifier 4093 * @param credits Number additional credits for peer 4094 */ 4095 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 4096 4097 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4098 if (!channel) { 4099 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 4100 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4101 } 4102 4103 // check state 4104 if (channel->state != L2CAP_STATE_OPEN){ 4105 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 4106 } 4107 4108 // assert incoming credits + credits <= 0xffff 4109 uint32_t total_credits = channel->credits_incoming; 4110 total_credits += channel->new_credits_incoming; 4111 total_credits += credits; 4112 if (total_credits > 0xffffu){ 4113 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 4114 channel->new_credits_incoming, credits); 4115 } 4116 4117 // set credits_granted 4118 channel->new_credits_incoming += credits; 4119 4120 // go 4121 l2cap_run(); 4122 return ERROR_CODE_SUCCESS; 4123 } 4124 4125 /** 4126 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 4127 * @param local_cid L2CAP LE Data Channel Identifier 4128 */ 4129 int l2cap_le_can_send_now(uint16_t local_cid){ 4130 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4131 if (!channel) { 4132 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 4133 return 0; 4134 } 4135 4136 // check state 4137 if (channel->state != L2CAP_STATE_OPEN) return 0; 4138 4139 // check queue 4140 if (channel->send_sdu_buffer) return 0; 4141 4142 // fine, go ahead 4143 return 1; 4144 } 4145 4146 /** 4147 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 4148 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 4149 * so packet handler should be ready to handle it 4150 * @param local_cid L2CAP LE Data Channel Identifier 4151 */ 4152 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 4153 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4154 if (!channel) { 4155 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 4156 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4157 } 4158 channel->waiting_for_can_send_now = 1; 4159 l2cap_le_notify_channel_can_send(channel); 4160 return ERROR_CODE_SUCCESS; 4161 } 4162 4163 /** 4164 * @brief Send data via LE Data Channel 4165 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 4166 * @param local_cid L2CAP LE Data Channel Identifier 4167 * @param data data to send 4168 * @param size data size 4169 */ 4170 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 4171 4172 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4173 if (!channel) { 4174 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 4175 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4176 } 4177 4178 if (len > channel->remote_mtu){ 4179 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 4180 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 4181 } 4182 4183 if (channel->send_sdu_buffer){ 4184 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 4185 return BTSTACK_ACL_BUFFERS_FULL; 4186 } 4187 4188 channel->send_sdu_buffer = data; 4189 channel->send_sdu_len = len; 4190 channel->send_sdu_pos = 0; 4191 4192 l2cap_notify_channel_can_send(); 4193 return ERROR_CODE_SUCCESS; 4194 } 4195 4196 /** 4197 * @brief Disconnect from LE Data Channel 4198 * @param local_cid L2CAP LE Data Channel Identifier 4199 */ 4200 uint8_t l2cap_le_disconnect(uint16_t local_cid) 4201 { 4202 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4203 if (!channel) { 4204 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 4205 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4206 } 4207 4208 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 4209 l2cap_run(); 4210 return ERROR_CODE_SUCCESS; 4211 } 4212 4213 #endif 4214