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 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2121 // send if we have more data and remote windows isn't full yet 2122 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2123 if (channel->unacked_frames >= btstack_min(channel->num_stored_tx_frames, channel->remote_tx_window_size)) return false; 2124 return hci_can_send_acl_classic_packet_now() != 0; 2125 } 2126 #endif 2127 if (!channel->waiting_for_can_send_now) return false; 2128 return (hci_can_send_acl_classic_packet_now() != 0); 2129 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2130 if (!channel->waiting_for_can_send_now) return false; 2131 return hci_can_send_acl_classic_packet_now() != 0; 2132 #endif 2133 #ifdef ENABLE_BLE 2134 case L2CAP_CHANNEL_TYPE_LE_FIXED: 2135 if (!channel->waiting_for_can_send_now) return false; 2136 return hci_can_send_acl_le_packet_now() != 0; 2137 #ifdef ENABLE_LE_DATA_CHANNELS 2138 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2139 if (channel->send_sdu_buffer == NULL) return false; 2140 if (channel->credits_outgoing == 0u) return false; 2141 return hci_can_send_acl_le_packet_now() != 0; 2142 #endif 2143 #endif 2144 default: 2145 return false; 2146 } 2147 } 2148 2149 static void l2cap_channel_trigger_send(l2cap_channel_t * channel){ 2150 switch (channel->channel_type){ 2151 #ifdef ENABLE_CLASSIC 2152 case L2CAP_CHANNEL_TYPE_CLASSIC: 2153 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2154 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) { 2155 l2cap_ertm_channel_send_information_frame(channel); 2156 return; 2157 } 2158 #endif 2159 channel->waiting_for_can_send_now = 0; 2160 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2161 break; 2162 case L2CAP_CHANNEL_TYPE_CONNECTIONLESS: 2163 channel->waiting_for_can_send_now = 0; 2164 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2165 break; 2166 #endif 2167 #ifdef ENABLE_BLE 2168 case L2CAP_CHANNEL_TYPE_LE_FIXED: 2169 channel->waiting_for_can_send_now = 0; 2170 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2171 break; 2172 #ifdef ENABLE_LE_DATA_CHANNELS 2173 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2174 l2cap_le_send_pdu(channel); 2175 break; 2176 #endif 2177 #endif 2178 default: 2179 break; 2180 } 2181 } 2182 2183 static void l2cap_notify_channel_can_send(void){ 2184 bool done = false; 2185 while (!done){ 2186 done = true; 2187 btstack_linked_list_iterator_t it; 2188 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2189 while (btstack_linked_list_iterator_has_next(&it)){ 2190 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2191 bool ready = l2cap_channel_ready_to_send(channel); 2192 if (!ready) continue; 2193 2194 // requeue channel for fairness 2195 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2196 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2197 2198 // trigger sending 2199 l2cap_channel_trigger_send(channel); 2200 2201 // exit inner loop as we just broke the iterator, but try again 2202 done = false; 2203 break; 2204 } 2205 } 2206 } 2207 2208 #ifdef L2CAP_USES_CHANNELS 2209 2210 static int l2cap_send_open_failed_on_hci_disconnect(l2cap_channel_t * channel){ 2211 // open cannot fail for for incoming connections 2212 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) return 0; 2213 2214 // check state 2215 switch (channel->state){ 2216 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2217 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2218 case L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES: 2219 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2220 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 2221 case L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES: 2222 case L2CAP_STATE_WAIT_CONNECT_RSP: 2223 case L2CAP_STATE_CONFIG: 2224 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 2225 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 2226 case L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE: 2227 case L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD: 2228 return 1; 2229 2230 case L2CAP_STATE_OPEN: 2231 case L2CAP_STATE_CLOSED: 2232 case L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES: 2233 case L2CAP_STATE_WAIT_DISCONNECT: 2234 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_INSUFFICIENT_SECURITY: 2235 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 2236 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 2237 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2238 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 2239 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 2240 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 2241 case L2CAP_STATE_INVALID: 2242 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2243 return 0; 2244 2245 default: 2246 // get a "warning" about new states 2247 btstack_assert(false); 2248 return 0; 2249 } 2250 } 2251 #endif 2252 2253 #ifdef ENABLE_CLASSIC 2254 static void l2cap_handle_hci_disconnect_event(l2cap_channel_t * channel){ 2255 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2256 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2257 } else { 2258 l2cap_handle_channel_closed(channel); 2259 } 2260 l2cap_free_channel_entry(channel); 2261 } 2262 #endif 2263 2264 #ifdef ENABLE_LE_DATA_CHANNELS 2265 static void l2cap_handle_hci_le_disconnect_event(l2cap_channel_t * channel){ 2266 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2267 l2cap_emit_le_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2268 } else { 2269 l2cap_emit_le_channel_closed(channel); 2270 } 2271 l2cap_free_channel_entry(channel); 2272 } 2273 #endif 2274 2275 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 2276 2277 UNUSED(packet_type); // ok: registered with hci_event_callback_registration 2278 UNUSED(cid); // ok: there is no channel 2279 UNUSED(size); // ok: fixed format events read from HCI buffer 2280 2281 #ifdef ENABLE_CLASSIC 2282 bd_addr_t address; 2283 hci_connection_t * hci_connection; 2284 int hci_con_used; 2285 #endif 2286 #ifdef L2CAP_USES_CHANNELS 2287 hci_con_handle_t handle; 2288 btstack_linked_list_iterator_t it; 2289 #endif 2290 2291 switch(hci_event_packet_get_type(packet)){ 2292 2293 // Notify channel packet handler if they can send now 2294 case HCI_EVENT_TRANSPORT_PACKET_SENT: 2295 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 2296 case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED: 2297 l2cap_run(); // try sending signaling packets first 2298 l2cap_notify_channel_can_send(); 2299 break; 2300 2301 case HCI_EVENT_COMMAND_STATUS: 2302 #ifdef ENABLE_CLASSIC 2303 // check command status for create connection for errors 2304 if (HCI_EVENT_IS_COMMAND_STATUS(packet, hci_create_connection)){ 2305 // cache outgoing address and reset 2306 (void)memcpy(address, l2cap_outgoing_classic_addr, 6); 2307 memset(l2cap_outgoing_classic_addr, 0, 6); 2308 // error => outgoing connection failed 2309 uint8_t status = hci_event_command_status_get_status(packet); 2310 if (status){ 2311 l2cap_handle_connection_failed_for_addr(address, status); 2312 } 2313 } 2314 #endif 2315 l2cap_run(); // try sending signaling packets first 2316 break; 2317 2318 #ifdef ENABLE_CLASSIC 2319 // handle connection complete events 2320 case HCI_EVENT_CONNECTION_COMPLETE: 2321 reverse_bd_addr(&packet[5], address); 2322 if (packet[2] == 0){ 2323 handle = little_endian_read_16(packet, 3); 2324 l2cap_handle_connection_success_for_addr(address, handle); 2325 } else { 2326 l2cap_handle_connection_failed_for_addr(address, packet[2]); 2327 } 2328 break; 2329 2330 // handle successful create connection cancel command 2331 case HCI_EVENT_COMMAND_COMPLETE: 2332 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 2333 if (packet[5] == 0){ 2334 reverse_bd_addr(&packet[6], address); 2335 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 2336 l2cap_handle_connection_failed_for_addr(address, 0x16); 2337 } 2338 } 2339 l2cap_run(); // try sending signaling packets first 2340 break; 2341 #endif 2342 2343 #ifdef L2CAP_USES_CHANNELS 2344 // handle disconnection complete events 2345 case HCI_EVENT_DISCONNECTION_COMPLETE: 2346 handle = little_endian_read_16(packet, 3); 2347 // send l2cap open failed or closed events for all channels on this handle and free them 2348 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2349 while (btstack_linked_list_iterator_has_next(&it)){ 2350 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2351 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2352 if (channel->con_handle != handle) continue; 2353 btstack_linked_list_iterator_remove(&it); 2354 switch(channel->channel_type){ 2355 #ifdef ENABLE_CLASSIC 2356 case L2CAP_CHANNEL_TYPE_CLASSIC: 2357 l2cap_handle_hci_disconnect_event(channel); 2358 break; 2359 #endif 2360 #ifdef ENABLE_LE_DATA_CHANNELS 2361 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2362 l2cap_handle_hci_le_disconnect_event(channel); 2363 break; 2364 #endif 2365 default: 2366 break; 2367 } 2368 } 2369 break; 2370 #endif 2371 2372 2373 // HCI Connection Timeouts 2374 #ifdef ENABLE_CLASSIC 2375 case L2CAP_EVENT_TIMEOUT_CHECK: 2376 handle = little_endian_read_16(packet, 2); 2377 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 2378 if (hci_authentication_active_for_handle(handle)) break; 2379 hci_con_used = 0; 2380 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2381 while (btstack_linked_list_iterator_has_next(&it)){ 2382 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2383 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2384 if (channel->con_handle != handle) continue; 2385 hci_con_used = 1; 2386 break; 2387 } 2388 if (hci_con_used) break; 2389 if (!hci_can_send_command_packet_now()) break; 2390 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 2391 break; 2392 2393 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 2394 case HCI_EVENT_READ_REMOTE_EXTENDED_FEATURES_COMPLETE: 2395 handle = little_endian_read_16(packet, 3); 2396 hci_connection = hci_connection_for_handle(handle); 2397 if (hci_connection == NULL) break; 2398 if ((hci_connection->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) == 0) break; 2399 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2400 while (btstack_linked_list_iterator_has_next(&it)){ 2401 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2402 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2403 if (channel->con_handle != handle) continue; 2404 log_info("remote supported features, channel %p, cid %04x - state %u", channel, channel->local_cid, channel->state); 2405 l2cap_handle_remote_supported_features_received(channel); 2406 } 2407 break; 2408 2409 case GAP_EVENT_SECURITY_LEVEL: 2410 handle = little_endian_read_16(packet, 2); 2411 log_info("l2cap - security level update for handle 0x%04x", handle); 2412 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2413 while (btstack_linked_list_iterator_has_next(&it)){ 2414 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2415 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2416 if (channel->con_handle != handle) continue; 2417 2418 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 2419 gap_security_level_t required_level = channel->required_security_level; 2420 2421 log_info("channel %p, cid %04x - state %u: actual %u >= required %u?", channel, channel->local_cid, channel->state, actual_level, required_level); 2422 2423 switch (channel->state){ 2424 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2425 if (actual_level >= required_level){ 2426 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2427 // we need to know if ERTM is supported before sending a config response 2428 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 2429 if (connection->l2cap_state.information_state != L2CAP_INFORMATION_STATE_DONE){ 2430 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2431 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 2432 break; 2433 } 2434 #endif 2435 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2436 l2cap_emit_incoming_connection(channel); 2437 } else { 2438 channel->reason = 0x0003; // security block 2439 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2440 } 2441 break; 2442 2443 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2444 if (actual_level >= required_level){ 2445 l2cap_ready_to_connect(channel); 2446 } else { 2447 // disconnnect, authentication not good enough 2448 hci_disconnect_security_block(handle); 2449 } 2450 break; 2451 2452 default: 2453 break; 2454 } 2455 } 2456 break; 2457 #endif 2458 2459 default: 2460 break; 2461 } 2462 2463 l2cap_run(); 2464 } 2465 2466 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 2467 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 2468 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 2469 signaling_responses[signaling_responses_pending].handle = handle; 2470 signaling_responses[signaling_responses_pending].code = code; 2471 signaling_responses[signaling_responses_pending].sig_id = sig_id; 2472 signaling_responses[signaling_responses_pending].cid = cid; 2473 signaling_responses[signaling_responses_pending].data = data; 2474 signaling_responses_pending++; 2475 l2cap_run(); 2476 } 2477 } 2478 2479 #ifdef ENABLE_CLASSIC 2480 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 2481 channel->remote_sig_id = identifier; 2482 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2483 l2cap_run(); 2484 } 2485 2486 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 2487 2488 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 2489 l2cap_service_t *service = l2cap_get_service(psm); 2490 if (!service) { 2491 // 0x0002 PSM not supported 2492 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2493 return; 2494 } 2495 2496 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 2497 if (!hci_connection) { 2498 // 2499 log_error("no hci_connection for handle %u", handle); 2500 return; 2501 } 2502 2503 // alloc structure 2504 // log_info("l2cap_handle_connection_request register channel"); 2505 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CLASSIC, hci_connection->address, BD_ADDR_TYPE_ACL, 2506 psm, service->mtu, service->required_security_level); 2507 if (!channel){ 2508 // 0x0004 No resources available 2509 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2510 return; 2511 } 2512 2513 channel->con_handle = handle; 2514 channel->remote_cid = source_cid; 2515 channel->remote_sig_id = sig_id; 2516 2517 // limit local mtu to max acl packet length - l2cap header 2518 if (channel->local_mtu > l2cap_max_mtu()) { 2519 channel->local_mtu = l2cap_max_mtu(); 2520 } 2521 2522 // set initial state 2523 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 2524 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 2525 2526 // add to connections list 2527 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2528 2529 // assert security requirements 2530 gap_request_security_level(handle, channel->required_security_level); 2531 } 2532 2533 void l2cap_accept_connection(uint16_t local_cid){ 2534 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 2535 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2536 if (!channel) { 2537 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 2538 return; 2539 } 2540 2541 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2542 // configure L2CAP Basic mode 2543 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2544 #endif 2545 2546 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 2547 2548 // process 2549 l2cap_run(); 2550 } 2551 2552 void l2cap_decline_connection(uint16_t local_cid){ 2553 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 2554 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 2555 if (!channel) { 2556 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 2557 return; 2558 } 2559 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2560 channel->reason = 0x04; // no resources available 2561 l2cap_run(); 2562 } 2563 2564 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2565 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 2566 2567 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2568 uint8_t use_fcs = 1; 2569 #endif 2570 2571 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2572 2573 uint16_t flags = little_endian_read_16(command, 6); 2574 if (flags & 1) { 2575 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 2576 } 2577 2578 // accept the other's configuration options 2579 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2580 uint16_t pos = 8; 2581 while (pos < end_pos){ 2582 uint8_t option_hint = command[pos] >> 7; 2583 uint8_t option_type = command[pos] & 0x7f; 2584 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2585 pos++; 2586 uint8_t length = command[pos++]; 2587 // MTU { type(8): 1, len(8):2, MTU(16) } 2588 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) && (length == 2)){ 2589 channel->remote_mtu = little_endian_read_16(command, pos); 2590 log_info("Remote MTU %u", channel->remote_mtu); 2591 if (channel->remote_mtu > l2cap_max_mtu()){ 2592 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 2593 channel->remote_mtu = l2cap_max_mtu(); 2594 } 2595 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2596 } 2597 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2598 if ((option_type == L2CAP_CONFIG_OPTION_TYPE_FLUSH_TIMEOUT) && (length == 2)){ 2599 channel->flush_timeout = little_endian_read_16(command, pos); 2600 log_info("Flush timeout: %u ms", channel->flush_timeout); 2601 } 2602 2603 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2604 // Retransmission and Flow Control Option 2605 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2606 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2607 switch(channel->mode){ 2608 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2609 // Store remote config 2610 channel->remote_tx_window_size = command[pos+1]; 2611 channel->remote_max_transmit = command[pos+2]; 2612 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2613 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2614 channel->remote_mps = little_endian_read_16(command, pos + 7); 2615 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2616 channel->remote_tx_window_size, 2617 channel->remote_max_transmit, 2618 channel->remote_retransmission_timeout_ms, 2619 channel->remote_monitor_timeout_ms, 2620 channel->remote_mps); 2621 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2622 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2623 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2624 } else { 2625 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2626 } 2627 break; 2628 case L2CAP_CHANNEL_MODE_BASIC: 2629 switch (mode){ 2630 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2631 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2632 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2633 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2634 } 2635 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2636 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2637 break; 2638 default: // case L2CAP_CHANNEL_MODE_BASIC: 2639 // TODO store and evaluate configuration 2640 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM); 2641 break; 2642 } 2643 break; 2644 default: 2645 break; 2646 } 2647 } 2648 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE && length == 1){ 2649 use_fcs = command[pos]; 2650 } 2651 #endif 2652 // check for unknown options 2653 if ((option_hint == 0) && ((option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT) || (option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE))){ 2654 log_info("l2cap cid %u, unknown options", channel->local_cid); 2655 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2656 } 2657 pos += length; 2658 } 2659 2660 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2661 // "FCS" has precedence over "No FCS" 2662 uint8_t update = channel->fcs_option || use_fcs; 2663 log_info("local fcs: %u, remote fcs: %u -> %u", channel->fcs_option, use_fcs, update); 2664 channel->fcs_option = update; 2665 // If ERTM mandatory, but remote didn't send Retransmission and Flowcontrol options -> disconnect 2666 if (((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_ERTM) == 0) & (channel->ertm_mandatory)){ 2667 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2668 } 2669 #endif 2670 } 2671 2672 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2673 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2674 log_info("l2cap_signaling_handle_configure_response"); 2675 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2676 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2677 uint16_t pos = 10; 2678 while (pos < end_pos){ 2679 uint8_t option_hint = command[pos] >> 7; 2680 uint8_t option_type = command[pos] & 0x7f; 2681 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2682 pos++; 2683 uint8_t length = command[pos++]; 2684 2685 // Retransmission and Flow Control Option 2686 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2687 switch (channel->mode){ 2688 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2689 if (channel->ertm_mandatory){ 2690 // ?? 2691 } else { 2692 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2693 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2694 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2695 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2696 } 2697 } 2698 break; 2699 case L2CAP_CHANNEL_MODE_BASIC: 2700 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2701 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2702 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2703 } 2704 break; 2705 default: 2706 break; 2707 } 2708 } 2709 2710 // check for unknown options 2711 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2712 log_info("l2cap cid %u, unknown options", channel->local_cid); 2713 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2714 } 2715 2716 pos += length; 2717 } 2718 #else 2719 UNUSED(channel); // ok: no code 2720 UNUSED(result); // ok: no code 2721 UNUSED(command); // ok: no code 2722 #endif 2723 } 2724 2725 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2726 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2727 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2728 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2729 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2730 if (channel->state == L2CAP_STATE_OPEN) return 0; 2731 return 1; 2732 } 2733 2734 2735 // @pre command len is valid, see check in l2cap_signaling_handler_dispatch 2736 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2737 2738 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2739 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2740 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2741 uint16_t result = 0; 2742 2743 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2744 2745 // handle DISCONNECT REQUESTS seperately 2746 if (code == DISCONNECTION_REQUEST){ 2747 switch (channel->state){ 2748 case L2CAP_STATE_CONFIG: 2749 case L2CAP_STATE_OPEN: 2750 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2751 case L2CAP_STATE_WAIT_DISCONNECT: 2752 l2cap_handle_disconnect_request(channel, identifier); 2753 break; 2754 2755 default: 2756 // ignore in other states 2757 break; 2758 } 2759 return; 2760 } 2761 2762 // @STATEMACHINE(l2cap) 2763 switch (channel->state) { 2764 2765 case L2CAP_STATE_WAIT_CONNECT_RSP: 2766 switch (code){ 2767 case CONNECTION_RESPONSE: 2768 if (cmd_len < 8){ 2769 // command imcomplete 2770 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2771 break; 2772 } 2773 l2cap_stop_rtx(channel); 2774 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2775 switch (result) { 2776 case 0: 2777 // successful connection 2778 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2779 channel->state = L2CAP_STATE_CONFIG; 2780 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2781 break; 2782 case 1: 2783 // connection pending. get some coffee, but start the ERTX 2784 l2cap_start_ertx(channel); 2785 break; 2786 default: 2787 // channel closed 2788 channel->state = L2CAP_STATE_CLOSED; 2789 // map l2cap connection response result to BTstack status enumeration 2790 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2791 2792 // drop link key if security block 2793 if ((L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result) == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2794 gap_drop_link_key_for_bd_addr(channel->address); 2795 } 2796 2797 // discard channel 2798 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2799 l2cap_free_channel_entry(channel); 2800 break; 2801 } 2802 break; 2803 2804 default: 2805 //@TODO: implement other signaling packets 2806 break; 2807 } 2808 break; 2809 2810 case L2CAP_STATE_CONFIG: 2811 switch (code) { 2812 case CONFIGURE_REQUEST: 2813 if (cmd_len < 4){ 2814 // command incomplete 2815 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2816 break; 2817 } 2818 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 2819 l2cap_signaling_handle_configure_request(channel, command); 2820 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 2821 // only done if continuation not set 2822 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 2823 } 2824 break; 2825 case CONFIGURE_RESPONSE: 2826 if (cmd_len < 6){ 2827 // command incomplete 2828 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2829 break; 2830 } 2831 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2832 l2cap_stop_rtx(channel); 2833 l2cap_signaling_handle_configure_response(channel, result, command); 2834 switch (result){ 2835 case 0: // success 2836 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 2837 break; 2838 case 4: // pending 2839 l2cap_start_ertx(channel); 2840 break; 2841 default: 2842 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2843 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 2844 // remote does not offer ertm but it's required 2845 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2846 break; 2847 } 2848 #endif 2849 // retry on negative result 2850 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2851 break; 2852 } 2853 break; 2854 default: 2855 break; 2856 } 2857 if (l2cap_channel_ready_for_open(channel)){ 2858 2859 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2860 // assert that packet can be stored in fragment buffers in ertm 2861 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2862 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 2863 uint16_t usable_mtu = channel->num_tx_buffers == 1 ? effective_mps : channel->num_tx_buffers * effective_mps - 2; 2864 if (usable_mtu < channel->remote_mtu){ 2865 log_info("Remote MTU %u > max storable ERTM packet, only using MTU = %u", channel->remote_mtu, usable_mtu); 2866 channel->remote_mtu = usable_mtu; 2867 } 2868 } 2869 #endif 2870 // for open: 2871 channel->state = L2CAP_STATE_OPEN; 2872 l2cap_emit_channel_opened(channel, 0); 2873 } 2874 break; 2875 2876 case L2CAP_STATE_WAIT_DISCONNECT: 2877 switch (code) { 2878 case DISCONNECTION_RESPONSE: 2879 l2cap_finialize_channel_close(channel); 2880 break; 2881 default: 2882 //@TODO: implement other signaling packets 2883 break; 2884 } 2885 break; 2886 2887 case L2CAP_STATE_CLOSED: 2888 // @TODO handle incoming requests 2889 break; 2890 2891 case L2CAP_STATE_OPEN: 2892 //@TODO: implement other signaling packets, e.g. re-configure 2893 break; 2894 default: 2895 break; 2896 } 2897 // log_info("new state %u", channel->state); 2898 } 2899 2900 2901 // @pre command len is valid, see check in l2cap_acl_classic_handler 2902 static void l2cap_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command){ 2903 2904 btstack_linked_list_iterator_t it; 2905 2906 // get code, signalind identifier and command len 2907 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2908 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2909 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2910 2911 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 2912 if ((code < 1) || (code == ECHO_RESPONSE) || (code > INFORMATION_RESPONSE)){ 2913 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2914 return; 2915 } 2916 2917 // general commands without an assigned channel 2918 switch(code) { 2919 2920 case CONNECTION_REQUEST: 2921 if (cmd_len == 4){ 2922 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2923 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2924 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 2925 } else { 2926 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2927 } 2928 return; 2929 2930 case ECHO_REQUEST: 2931 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 2932 return; 2933 2934 case INFORMATION_REQUEST: 2935 if (cmd_len == 2) { 2936 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2937 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 2938 } else { 2939 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2940 } 2941 return; 2942 2943 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2944 case INFORMATION_RESPONSE: { 2945 hci_connection_t * connection = hci_connection_for_handle(handle); 2946 if (!connection) return; 2947 if (connection->l2cap_state.information_state != L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE) return; 2948 2949 // get extended features from response if valid 2950 connection->l2cap_state.extended_feature_mask = 0; 2951 if (cmd_len >= 6) { 2952 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2953 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2954 if (result == 0 && info_type == L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) { 2955 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2956 } 2957 } 2958 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2959 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2960 2961 // trigger connection request 2962 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2963 while (btstack_linked_list_iterator_has_next(&it)){ 2964 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2965 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2966 if (channel->con_handle != handle) continue; 2967 2968 // incoming connection: ask user for channel configuration, esp. if ertm will be mandatory 2969 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2970 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2971 l2cap_emit_incoming_connection(channel); 2972 continue; 2973 } 2974 2975 // outgoing connection 2976 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2977 2978 // if ERTM was requested, but is not listed in extended feature mask: 2979 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2980 2981 if (channel->ertm_mandatory){ 2982 // bail if ERTM is mandatory 2983 channel->state = L2CAP_STATE_CLOSED; 2984 // map l2cap connection response result to BTstack status enumeration 2985 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 2986 // discard channel 2987 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2988 l2cap_free_channel_entry(channel); 2989 continue; 2990 2991 } else { 2992 // fallback to Basic mode 2993 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2994 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2995 } 2996 } 2997 2998 // respond to connection request 2999 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 3000 continue; 3001 } 3002 } 3003 return; 3004 } 3005 #endif 3006 3007 default: 3008 break; 3009 } 3010 3011 // Get potential destination CID 3012 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3013 3014 // Find channel for this sig_id and connection handle 3015 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3016 while (btstack_linked_list_iterator_has_next(&it)){ 3017 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3018 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 3019 if (channel->con_handle != handle) continue; 3020 if (code & 1) { 3021 // match odd commands (responses) by previous signaling identifier 3022 if (channel->local_sig_id == sig_id) { 3023 l2cap_signaling_handler_channel(channel, command); 3024 break; 3025 } 3026 } else { 3027 // match even commands (requests) by local channel id 3028 if (channel->local_cid == dest_cid) { 3029 l2cap_signaling_handler_channel(channel, command); 3030 break; 3031 } 3032 } 3033 } 3034 } 3035 #endif 3036 3037 #ifdef ENABLE_BLE 3038 3039 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 3040 uint8_t event[6]; 3041 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 3042 event[1] = 4; 3043 little_endian_store_16(event, 2, con_handle); 3044 little_endian_store_16(event, 4, result); 3045 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3046 if (!l2cap_event_packet_handler) return; 3047 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 3048 } 3049 3050 // @returns valid 3051 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 3052 hci_connection_t * connection; 3053 uint16_t result; 3054 uint8_t event[12]; 3055 3056 #ifdef ENABLE_LE_DATA_CHANNELS 3057 btstack_linked_list_iterator_t it; 3058 l2cap_channel_t * channel; 3059 uint16_t local_cid; 3060 uint16_t le_psm; 3061 uint16_t new_credits; 3062 uint16_t credits_before; 3063 l2cap_service_t * service; 3064 uint16_t source_cid; 3065 #endif 3066 3067 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 3068 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3069 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 3070 3071 switch (code){ 3072 3073 case CONNECTION_PARAMETER_UPDATE_REQUEST: 3074 // check size 3075 if (len < 8u) return 0u; 3076 connection = hci_connection_for_handle(handle); 3077 if (connection){ 3078 if (connection->role != HCI_ROLE_MASTER){ 3079 // reject command without notifying upper layer when not in master role 3080 return 0; 3081 } 3082 le_connection_parameter_range_t existing_range; 3083 gap_get_connection_parameter_range(&existing_range); 3084 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3085 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3086 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3087 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 3088 3089 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 3090 if (update_parameter){ 3091 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 3092 connection->le_conn_interval_min = le_conn_interval_min; 3093 connection->le_conn_interval_max = le_conn_interval_max; 3094 connection->le_conn_latency = le_conn_latency; 3095 connection->le_supervision_timeout = le_supervision_timeout; 3096 } else { 3097 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 3098 } 3099 connection->le_con_param_update_identifier = sig_id; 3100 } 3101 3102 if (!l2cap_event_packet_handler) break; 3103 3104 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 3105 event[1] = 8; 3106 little_endian_store_16(event, 2, handle); 3107 (void)memcpy(&event[4], &command[4], 8); 3108 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3109 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3110 break; 3111 3112 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 3113 // check size 3114 if (len < 2u) return 0u; 3115 result = little_endian_read_16(command, 4); 3116 l2cap_emit_connection_parameter_update_response(handle, result); 3117 break; 3118 3119 #ifdef ENABLE_LE_DATA_CHANNELS 3120 3121 case COMMAND_REJECT: 3122 // Find channel for this sig_id and connection handle 3123 channel = NULL; 3124 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3125 while (btstack_linked_list_iterator_has_next(&it)){ 3126 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3127 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3128 if (a_channel->con_handle != handle) continue; 3129 if (a_channel->local_sig_id != sig_id) continue; 3130 channel = a_channel; 3131 break; 3132 } 3133 if (!channel) break; 3134 3135 // if received while waiting for le connection response, assume legacy device 3136 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 3137 channel->state = L2CAP_STATE_CLOSED; 3138 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 3139 l2cap_emit_le_channel_opened(channel, 0x0002); 3140 3141 // discard channel 3142 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3143 l2cap_free_channel_entry(channel); 3144 break; 3145 } 3146 break; 3147 3148 case LE_CREDIT_BASED_CONNECTION_REQUEST: 3149 // check size 3150 if (len < 10u) return 0u; 3151 3152 // get hci connection, bail if not found (must not happen) 3153 connection = hci_connection_for_handle(handle); 3154 if (!connection) return 0; 3155 3156 // check if service registered 3157 le_psm = little_endian_read_16(command, 4); 3158 service = l2cap_le_get_service(le_psm); 3159 source_cid = little_endian_read_16(command, 6); 3160 3161 if (service){ 3162 if (source_cid < 0x40u){ 3163 // 0x0009 Connection refused - Invalid Source CID 3164 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 3165 return 1; 3166 } 3167 3168 // go through list of channels for this ACL connection and check if we get a match 3169 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3170 while (btstack_linked_list_iterator_has_next(&it)){ 3171 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3172 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3173 if (a_channel->con_handle != handle) continue; 3174 if (a_channel->remote_cid != source_cid) continue; 3175 // 0x000a Connection refused - Source CID already allocated 3176 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 3177 return 1; 3178 } 3179 3180 // security: check encryption 3181 if (service->required_security_level >= LEVEL_2){ 3182 if (gap_encryption_key_size(handle) == 0){ 3183 // 0x0008 Connection refused - insufficient encryption 3184 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 3185 return 1; 3186 } 3187 // anything less than 16 byte key size is insufficient 3188 if (gap_encryption_key_size(handle) < 16){ 3189 // 0x0007 Connection refused – insufficient encryption key size 3190 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 3191 return 1; 3192 } 3193 } 3194 3195 // security: check authencation 3196 if (service->required_security_level >= LEVEL_3){ 3197 if (!gap_authenticated(handle)){ 3198 // 0x0005 Connection refused – insufficient authentication 3199 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 3200 return 1; 3201 } 3202 } 3203 3204 // security: check authorization 3205 if (service->required_security_level >= LEVEL_4){ 3206 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 3207 // 0x0006 Connection refused – insufficient authorization 3208 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 3209 return 1; 3210 } 3211 } 3212 3213 // allocate channel 3214 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 3215 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 3216 if (!channel){ 3217 // 0x0004 Connection refused – no resources available 3218 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 3219 return 1; 3220 } 3221 3222 channel->con_handle = handle; 3223 channel->remote_cid = source_cid; 3224 channel->remote_sig_id = sig_id; 3225 channel->remote_mtu = little_endian_read_16(command, 8); 3226 channel->remote_mps = little_endian_read_16(command, 10); 3227 channel->credits_outgoing = little_endian_read_16(command, 12); 3228 3229 // set initial state 3230 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3231 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3232 3233 // add to connections list 3234 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 3235 3236 // post connection request event 3237 l2cap_emit_le_incoming_connection(channel); 3238 3239 } else { 3240 // Connection refused – LE_PSM not supported 3241 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 3242 } 3243 break; 3244 3245 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 3246 // check size 3247 if (len < 10u) return 0u; 3248 3249 // Find channel for this sig_id and connection handle 3250 channel = NULL; 3251 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3252 while (btstack_linked_list_iterator_has_next(&it)){ 3253 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3254 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3255 if (a_channel->con_handle != handle) continue; 3256 if (a_channel->local_sig_id != sig_id) continue; 3257 channel = a_channel; 3258 break; 3259 } 3260 if (!channel) break; 3261 3262 // cid + 0 3263 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 3264 if (result){ 3265 channel->state = L2CAP_STATE_CLOSED; 3266 // map l2cap connection response result to BTstack status enumeration 3267 l2cap_emit_le_channel_opened(channel, result); 3268 3269 // discard channel 3270 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3271 l2cap_free_channel_entry(channel); 3272 break; 3273 } 3274 3275 // success 3276 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3277 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3278 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3279 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3280 channel->state = L2CAP_STATE_OPEN; 3281 l2cap_emit_le_channel_opened(channel, result); 3282 break; 3283 3284 case LE_FLOW_CONTROL_CREDIT: 3285 // check size 3286 if (len < 4u) return 0u; 3287 3288 // find channel 3289 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3290 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3291 if (!channel) { 3292 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3293 break; 3294 } 3295 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3296 credits_before = channel->credits_outgoing; 3297 channel->credits_outgoing += new_credits; 3298 // check for credit overrun 3299 if (credits_before > channel->credits_outgoing){ 3300 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 3301 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3302 break; 3303 } 3304 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 3305 break; 3306 3307 case DISCONNECTION_REQUEST: 3308 3309 // check size 3310 if (len < 4u) return 0u; 3311 3312 // find channel 3313 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3314 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle); 3315 if (!channel) { 3316 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3317 break; 3318 } 3319 channel->remote_sig_id = sig_id; 3320 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3321 break; 3322 3323 #endif 3324 3325 case DISCONNECTION_RESPONSE: 3326 break; 3327 3328 default: 3329 // command unknown -> reject command 3330 return 0; 3331 } 3332 return 1; 3333 } 3334 #endif 3335 3336 #ifdef ENABLE_CLASSIC 3337 static void l2cap_acl_classic_handler_for_channel(l2cap_channel_t * l2cap_channel, uint8_t * packet, uint16_t size){ 3338 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3339 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3340 3341 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3342 3343 // assert control + FCS fields are inside 3344 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) return; 3345 3346 if (l2cap_channel->fcs_option){ 3347 // verify FCS (required if one side requested it) 3348 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3349 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3350 3351 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 3352 // simulate fcs error 3353 static int counter = 0; 3354 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 3355 log_info("Simulate fcs error"); 3356 fcs_calculated++; 3357 counter = 0; 3358 } 3359 #endif 3360 3361 if (fcs_calculated == fcs_packet){ 3362 log_info("Packet FCS 0x%04x verified", fcs_packet); 3363 } else { 3364 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3365 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 3366 return; 3367 } 3368 } 3369 3370 // switch on packet type 3371 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3372 uint8_t req_seq = (control >> 8) & 0x3f; 3373 int final = (control >> 7) & 0x01; 3374 if (control & 1){ 3375 // S-Frame 3376 int poll = (control >> 4) & 0x01; 3377 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3378 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3379 l2cap_ertm_tx_packet_state_t * tx_state; 3380 switch (s){ 3381 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3382 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3383 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3384 if (poll && final){ 3385 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3386 log_error("P=F=1 in S-Frame"); 3387 break; 3388 } 3389 if (poll){ 3390 // check if we did request selective retransmission before <==> we have stored SDU segments 3391 int i; 3392 int num_stored_out_of_order_packets = 0; 3393 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3394 int index = l2cap_channel->rx_store_index + i; 3395 if (index >= l2cap_channel->num_rx_buffers){ 3396 index -= l2cap_channel->num_rx_buffers; 3397 } 3398 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3399 if (!rx_state->valid) continue; 3400 num_stored_out_of_order_packets++; 3401 } 3402 if (num_stored_out_of_order_packets){ 3403 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3404 } else { 3405 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3406 } 3407 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3408 } 3409 if (final){ 3410 // Stop-MonitorTimer 3411 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3412 // If UnackedFrames > 0 then Start-RetransTimer 3413 if (l2cap_channel->unacked_frames){ 3414 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3415 } 3416 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3417 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3418 } 3419 break; 3420 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3421 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3422 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3423 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3424 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3425 break; 3426 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3427 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3428 break; 3429 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3430 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3431 if (poll){ 3432 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3433 } 3434 // find requested i-frame 3435 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3436 if (tx_state){ 3437 log_info("Retransmission for tx_seq %u requested", req_seq); 3438 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3439 tx_state->retransmission_requested = 1; 3440 l2cap_channel->srej_active = 1; 3441 } 3442 break; 3443 default: 3444 break; 3445 } 3446 } else { 3447 // I-Frame 3448 // get control 3449 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3450 uint8_t tx_seq = (control >> 1) & 0x3f; 3451 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3452 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3453 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3454 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3455 if (final){ 3456 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3457 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3458 } 3459 3460 // get SDU 3461 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3462 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3463 3464 // assert SDU size is smaller or equal to our buffers 3465 uint16_t max_payload_size = 0; 3466 switch (sar){ 3467 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 3468 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 3469 // SDU Length + MPS 3470 max_payload_size = l2cap_channel->local_mps + 2; 3471 break; 3472 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 3473 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 3474 max_payload_size = l2cap_channel->local_mps; 3475 break; 3476 default: 3477 btstack_assert(false); 3478 break; 3479 } 3480 if (payload_len > max_payload_size){ 3481 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 3482 return; 3483 } 3484 3485 // check ordering 3486 if (l2cap_channel->expected_tx_seq == tx_seq){ 3487 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3488 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3489 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3490 3491 // process SDU 3492 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 3493 3494 // process stored segments 3495 while (true){ 3496 int index = l2cap_channel->rx_store_index; 3497 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3498 if (!rx_state->valid) break; 3499 3500 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3501 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3502 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3503 3504 rx_state->valid = 0; 3505 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3506 3507 // update rx store index 3508 index++; 3509 if (index >= l2cap_channel->num_rx_buffers){ 3510 index = 0; 3511 } 3512 l2cap_channel->rx_store_index = index; 3513 } 3514 3515 // 3516 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3517 3518 } else { 3519 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3520 if (delta < 2){ 3521 // store segment 3522 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 3523 3524 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3525 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3526 } else { 3527 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3528 l2cap_channel->send_supervisor_frame_reject = 1; 3529 } 3530 } 3531 } 3532 return; 3533 } 3534 #endif 3535 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3536 3537 } 3538 #endif 3539 3540 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3541 #ifdef ENABLE_CLASSIC 3542 l2cap_channel_t * l2cap_channel; 3543 l2cap_fixed_channel_t * l2cap_fixed_channel; 3544 3545 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3546 uint8_t broadcast_flag = READ_ACL_FLAGS(packet) >> 2; 3547 switch (channel_id) { 3548 3549 case L2CAP_CID_SIGNALING: { 3550 if (broadcast_flag != 0) break; 3551 uint32_t command_offset = 8; 3552 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 3553 // assert signaling command is fully inside packet 3554 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3555 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3556 if (next_command_offset > size){ 3557 log_error("l2cap signaling command len invalid -> drop"); 3558 break; 3559 } 3560 // handle signaling command 3561 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3562 // go to next command 3563 command_offset = next_command_offset; 3564 } 3565 break; 3566 } 3567 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3568 if (broadcast_flag == 0) break; 3569 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3570 if (!l2cap_fixed_channel) break; 3571 if (!l2cap_fixed_channel->packet_handler) break; 3572 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3573 break; 3574 3575 default: 3576 if (broadcast_flag != 0) break; 3577 // Find channel for this channel_id and connection handle 3578 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3579 if (l2cap_channel != NULL){ 3580 l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size); 3581 } 3582 break; 3583 } 3584 #else 3585 UNUSED(handle); // ok: no code 3586 UNUSED(packet); // ok: no code 3587 UNUSED(size); // ok: no code 3588 #endif 3589 } 3590 3591 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3592 #ifdef ENABLE_BLE 3593 3594 l2cap_fixed_channel_t * l2cap_fixed_channel; 3595 3596 #ifdef ENABLE_LE_DATA_CHANNELS 3597 l2cap_channel_t * l2cap_channel; 3598 #endif 3599 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3600 switch (channel_id) { 3601 3602 case L2CAP_CID_SIGNALING_LE: { 3603 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3604 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3605 if ((COMPLETE_L2CAP_HEADER + 4u + len) > size) break; 3606 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3607 if (!valid){ 3608 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3609 } 3610 break; 3611 } 3612 3613 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3614 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3615 if (!l2cap_fixed_channel) break; 3616 if (!l2cap_fixed_channel->packet_handler) break; 3617 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3618 break; 3619 3620 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3621 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3622 if (!l2cap_fixed_channel) break; 3623 if (!l2cap_fixed_channel->packet_handler) break; 3624 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3625 break; 3626 3627 default: 3628 3629 #ifdef ENABLE_LE_DATA_CHANNELS 3630 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle); 3631 if (l2cap_channel != NULL) { 3632 // credit counting 3633 if (l2cap_channel->credits_incoming == 0u){ 3634 log_info("LE Data Channel packet received but no incoming credits"); 3635 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3636 break; 3637 } 3638 l2cap_channel->credits_incoming--; 3639 3640 // automatic credits 3641 if ((l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK) && l2cap_channel->automatic_credits){ 3642 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3643 } 3644 3645 // first fragment 3646 uint16_t pos = 0; 3647 if (!l2cap_channel->receive_sdu_len){ 3648 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3649 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3650 l2cap_channel->receive_sdu_len = sdu_len; 3651 l2cap_channel->receive_sdu_pos = 0; 3652 pos += 2u; 3653 size -= 2u; 3654 } 3655 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3656 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3657 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3658 (void)memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], 3659 &packet[COMPLETE_L2CAP_HEADER + pos], 3660 fragment_size); 3661 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3662 // done? 3663 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3664 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3665 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3666 l2cap_channel->receive_sdu_len = 0; 3667 } 3668 } 3669 #endif 3670 break; 3671 } 3672 #else 3673 UNUSED(handle); // ok: no code 3674 UNUSED(packet); // ok: no code 3675 UNUSED(size); // ok: no code 3676 #endif 3677 } 3678 3679 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3680 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3681 UNUSED(channel); // ok: there is no channel 3682 3683 // Assert full L2CAP header present 3684 if (size < COMPLETE_L2CAP_HEADER) return; 3685 3686 // Dispatch to Classic or LE handler (SCO packets are not dispatched to L2CAP) 3687 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3688 hci_connection_t *conn = hci_connection_for_handle(handle); 3689 if (!conn) return; 3690 if (conn->address_type == BD_ADDR_TYPE_ACL){ 3691 l2cap_acl_classic_handler(handle, packet, size); 3692 } else { 3693 l2cap_acl_le_handler(handle, packet, size); 3694 } 3695 3696 l2cap_run(); 3697 } 3698 3699 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3700 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 3701 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3702 if (!channel) return; 3703 channel->packet_handler = the_packet_handler; 3704 } 3705 3706 #ifdef ENABLE_CLASSIC 3707 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3708 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3709 channel->state = L2CAP_STATE_CLOSED; 3710 l2cap_handle_channel_closed(channel); 3711 // discard channel 3712 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3713 l2cap_free_channel_entry(channel); 3714 } 3715 #endif 3716 3717 #ifdef L2CAP_USES_CHANNELS 3718 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3719 btstack_linked_list_iterator_t it; 3720 btstack_linked_list_iterator_init(&it, services); 3721 while (btstack_linked_list_iterator_has_next(&it)){ 3722 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3723 if ( service->psm == psm){ 3724 return service; 3725 }; 3726 } 3727 return NULL; 3728 } 3729 #endif 3730 3731 #ifdef ENABLE_CLASSIC 3732 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3733 return l2cap_get_service_internal(&l2cap_services, psm); 3734 } 3735 3736 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3737 3738 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3739 3740 // check for alread registered psm 3741 l2cap_service_t *service = l2cap_get_service(psm); 3742 if (service) { 3743 log_error("l2cap_register_service: PSM %u already registered", psm); 3744 return L2CAP_SERVICE_ALREADY_REGISTERED; 3745 } 3746 3747 // alloc structure 3748 service = btstack_memory_l2cap_service_get(); 3749 if (!service) { 3750 log_error("l2cap_register_service: no memory for l2cap_service_t"); 3751 return BTSTACK_MEMORY_ALLOC_FAILED; 3752 } 3753 3754 // fill in 3755 service->psm = psm; 3756 service->mtu = mtu; 3757 service->packet_handler = service_packet_handler; 3758 service->required_security_level = security_level; 3759 3760 // add to services list 3761 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3762 3763 // enable page scan 3764 gap_connectable_control(1); 3765 3766 return ERROR_CODE_SUCCESS; 3767 } 3768 3769 uint8_t l2cap_unregister_service(uint16_t psm){ 3770 3771 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 3772 3773 l2cap_service_t *service = l2cap_get_service(psm); 3774 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3775 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 3776 btstack_memory_l2cap_service_free(service); 3777 3778 // disable page scan when no services registered 3779 if (btstack_linked_list_empty(&l2cap_services)) { 3780 gap_connectable_control(0); 3781 } 3782 return ERROR_CODE_SUCCESS; 3783 } 3784 #endif 3785 3786 3787 #ifdef ENABLE_LE_DATA_CHANNELS 3788 3789 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 3790 if (!channel->waiting_for_can_send_now) return; 3791 if (channel->send_sdu_buffer) return; 3792 channel->waiting_for_can_send_now = 0; 3793 log_debug("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 3794 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 3795 } 3796 3797 // 1BH2222 3798 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 3799 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", 3800 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 3801 uint8_t event[19]; 3802 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 3803 event[1] = sizeof(event) - 2u; 3804 event[2] = channel->address_type; 3805 reverse_bd_addr(channel->address, &event[3]); 3806 little_endian_store_16(event, 9, channel->con_handle); 3807 little_endian_store_16(event, 11, channel->psm); 3808 little_endian_store_16(event, 13, channel->local_cid); 3809 little_endian_store_16(event, 15, channel->remote_cid); 3810 little_endian_store_16(event, 17, channel->remote_mtu); 3811 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3812 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3813 } 3814 // 11BH22222 3815 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 3816 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", 3817 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 3818 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 3819 uint8_t event[23]; 3820 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 3821 event[1] = sizeof(event) - 2u; 3822 event[2] = status; 3823 event[3] = channel->address_type; 3824 reverse_bd_addr(channel->address, &event[4]); 3825 little_endian_store_16(event, 10, channel->con_handle); 3826 event[12] = (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) ? 1 : 0; 3827 little_endian_store_16(event, 13, channel->psm); 3828 little_endian_store_16(event, 15, channel->local_cid); 3829 little_endian_store_16(event, 17, channel->remote_cid); 3830 little_endian_store_16(event, 19, channel->local_mtu); 3831 little_endian_store_16(event, 21, channel->remote_mtu); 3832 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3833 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3834 } 3835 // 2 3836 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 3837 log_info("L2CAP_EVENT_LE_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 3838 uint8_t event[4]; 3839 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 3840 event[1] = sizeof(event) - 2u; 3841 little_endian_store_16(event, 2, channel->local_cid); 3842 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3843 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3844 } 3845 3846 static void l2cap_le_send_pdu(l2cap_channel_t *channel){ 3847 btstack_assert(channel != NULL); 3848 btstack_assert(channel->send_sdu_buffer != NULL); 3849 btstack_assert(channel->credits_outgoing > 0); 3850 3851 // send part of SDU 3852 hci_reserve_packet_buffer(); 3853 uint8_t * acl_buffer = hci_get_outgoing_packet_buffer(); 3854 uint8_t * l2cap_payload = acl_buffer + 8; 3855 uint16_t pos = 0; 3856 if (!channel->send_sdu_pos){ 3857 // store SDU len 3858 channel->send_sdu_pos += 2u; 3859 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 3860 pos += 2u; 3861 } 3862 uint16_t payload_size = btstack_min(channel->send_sdu_len + 2u - channel->send_sdu_pos, channel->remote_mps - pos); 3863 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 3864 (void)memcpy(&l2cap_payload[pos], 3865 &channel->send_sdu_buffer[channel->send_sdu_pos - 2u], 3866 payload_size); // -2 for virtual SDU len 3867 pos += payload_size; 3868 channel->send_sdu_pos += payload_size; 3869 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 3870 3871 channel->credits_outgoing--; 3872 3873 hci_send_acl_packet_buffer(8u + pos); 3874 3875 if (channel->send_sdu_pos >= (channel->send_sdu_len + 2u)){ 3876 channel->send_sdu_buffer = NULL; 3877 // send done event 3878 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 3879 // inform about can send now 3880 l2cap_le_notify_channel_can_send(channel); 3881 } 3882 } 3883 3884 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3885 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 3886 channel->state = L2CAP_STATE_CLOSED; 3887 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 3888 // discard channel 3889 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3890 l2cap_free_channel_entry(channel); 3891 } 3892 3893 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 3894 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 3895 } 3896 3897 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 3898 3899 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 3900 3901 // check for alread registered psm 3902 l2cap_service_t *service = l2cap_le_get_service(psm); 3903 if (service) { 3904 return L2CAP_SERVICE_ALREADY_REGISTERED; 3905 } 3906 3907 // alloc structure 3908 service = btstack_memory_l2cap_service_get(); 3909 if (!service) { 3910 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 3911 return BTSTACK_MEMORY_ALLOC_FAILED; 3912 } 3913 3914 // fill in 3915 service->psm = psm; 3916 service->mtu = 0; 3917 service->packet_handler = packet_handler; 3918 service->required_security_level = security_level; 3919 3920 // add to services list 3921 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 3922 3923 // done 3924 return ERROR_CODE_SUCCESS; 3925 } 3926 3927 uint8_t l2cap_le_unregister_service(uint16_t psm) { 3928 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 3929 l2cap_service_t *service = l2cap_le_get_service(psm); 3930 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3931 3932 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 3933 btstack_memory_l2cap_service_free(service); 3934 return ERROR_CODE_SUCCESS; 3935 } 3936 3937 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 3938 // get channel 3939 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3940 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3941 3942 // validate state 3943 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3944 return ERROR_CODE_COMMAND_DISALLOWED; 3945 } 3946 3947 // set state accept connection 3948 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 3949 channel->receive_sdu_buffer = receive_sdu_buffer; 3950 channel->local_mtu = mtu; 3951 channel->new_credits_incoming = initial_credits; 3952 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3953 3954 // test 3955 // channel->new_credits_incoming = 1; 3956 3957 // go 3958 l2cap_run(); 3959 return ERROR_CODE_SUCCESS; 3960 } 3961 3962 /** 3963 * @brief Deny incoming LE Data Channel connection due to resource constraints 3964 * @param local_cid L2CAP LE Data Channel Identifier 3965 */ 3966 3967 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 3968 // get channel 3969 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3970 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3971 3972 // validate state 3973 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3974 return ERROR_CODE_COMMAND_DISALLOWED; 3975 } 3976 3977 // set state decline connection 3978 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 3979 channel->reason = 0x04; // no resources available 3980 l2cap_run(); 3981 return ERROR_CODE_SUCCESS; 3982 } 3983 3984 static gap_security_level_t l2cap_le_security_level_for_connection(hci_con_handle_t con_handle){ 3985 uint8_t encryption_key_size = gap_encryption_key_size(con_handle); 3986 if (encryption_key_size == 0) return LEVEL_0; 3987 3988 uint8_t authenticated = gap_authenticated(con_handle); 3989 if (!authenticated) return LEVEL_2; 3990 3991 return encryption_key_size == 16 ? LEVEL_4 : LEVEL_3; 3992 } 3993 3994 // used to handle pairing complete after triggering to increase 3995 static void l2cap_sm_packet_handler(uint8_t packet_type, uint16_t channel_nr, uint8_t *packet, uint16_t size) { 3996 UNUSED(channel_nr); 3997 UNUSED(size); 3998 UNUSED(packet_type); 3999 btstack_assert(packet_type = HCI_EVENT_PACKET); 4000 if (hci_event_packet_get_type(packet) != SM_EVENT_PAIRING_COMPLETE) return; 4001 hci_con_handle_t con_handle = sm_event_pairing_complete_get_handle(packet); 4002 btstack_linked_list_iterator_t it; 4003 btstack_linked_list_iterator_init(&it, &l2cap_channels); 4004 while (btstack_linked_list_iterator_has_next(&it)) { 4005 l2cap_channel_t *channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 4006 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 4007 if (channel->con_handle != con_handle) continue; 4008 if (channel->state != L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE) continue; 4009 4010 // found channel, check security level 4011 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4012 // pairing failed or wasn't good enough, inform user 4013 l2cap_emit_le_channel_opened(channel, ERROR_CODE_INSUFFICIENT_SECURITY); 4014 // discard channel 4015 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 4016 l2cap_free_channel_entry(channel); 4017 } else { 4018 // send conn request now 4019 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4020 l2cap_run(); 4021 } 4022 } 4023 } 4024 4025 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 4026 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 4027 uint16_t * out_local_cid) { 4028 4029 static btstack_packet_callback_registration_t sm_event_callback_registration; 4030 static bool sm_callback_registered = false; 4031 4032 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 4033 4034 hci_connection_t * connection = hci_connection_for_handle(con_handle); 4035 if (!connection) { 4036 log_error("no hci_connection for handle 0x%04x", con_handle); 4037 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 4038 } 4039 4040 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); 4041 if (!channel) { 4042 return BTSTACK_MEMORY_ALLOC_FAILED; 4043 } 4044 log_info("l2cap_le_create_channel %p", channel); 4045 4046 // store local_cid 4047 if (out_local_cid){ 4048 *out_local_cid = channel->local_cid; 4049 } 4050 4051 // setup channel entry 4052 channel->con_handle = con_handle; 4053 channel->receive_sdu_buffer = receive_sdu_buffer; 4054 channel->new_credits_incoming = initial_credits; 4055 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 4056 4057 // add to connections list 4058 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 4059 4060 // check security level 4061 if (l2cap_le_security_level_for_connection(con_handle) < channel->required_security_level){ 4062 if (!sm_callback_registered){ 4063 sm_callback_registered = true; 4064 // lazy registration for SM events 4065 sm_event_callback_registration.callback = &l2cap_sm_packet_handler; 4066 sm_add_event_handler(&sm_event_callback_registration); 4067 } 4068 4069 // start pairing 4070 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 4071 sm_request_pairing(con_handle); 4072 } else { 4073 // send conn request right away 4074 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 4075 l2cap_run(); 4076 } 4077 4078 return ERROR_CODE_SUCCESS; 4079 } 4080 4081 /** 4082 * @brief Provide credtis for LE Data Channel 4083 * @param local_cid L2CAP LE Data Channel Identifier 4084 * @param credits Number additional credits for peer 4085 */ 4086 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 4087 4088 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4089 if (!channel) { 4090 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 4091 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4092 } 4093 4094 // check state 4095 if (channel->state != L2CAP_STATE_OPEN){ 4096 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 4097 } 4098 4099 // assert incoming credits + credits <= 0xffff 4100 uint32_t total_credits = channel->credits_incoming; 4101 total_credits += channel->new_credits_incoming; 4102 total_credits += credits; 4103 if (total_credits > 0xffffu){ 4104 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 4105 channel->new_credits_incoming, credits); 4106 } 4107 4108 // set credits_granted 4109 channel->new_credits_incoming += credits; 4110 4111 // go 4112 l2cap_run(); 4113 return ERROR_CODE_SUCCESS; 4114 } 4115 4116 /** 4117 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 4118 * @param local_cid L2CAP LE Data Channel Identifier 4119 */ 4120 int l2cap_le_can_send_now(uint16_t local_cid){ 4121 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4122 if (!channel) { 4123 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 4124 return 0; 4125 } 4126 4127 // check state 4128 if (channel->state != L2CAP_STATE_OPEN) return 0; 4129 4130 // check queue 4131 if (channel->send_sdu_buffer) return 0; 4132 4133 // fine, go ahead 4134 return 1; 4135 } 4136 4137 /** 4138 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 4139 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 4140 * so packet handler should be ready to handle it 4141 * @param local_cid L2CAP LE Data Channel Identifier 4142 */ 4143 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 4144 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4145 if (!channel) { 4146 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 4147 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4148 } 4149 channel->waiting_for_can_send_now = 1; 4150 l2cap_le_notify_channel_can_send(channel); 4151 return ERROR_CODE_SUCCESS; 4152 } 4153 4154 /** 4155 * @brief Send data via LE Data Channel 4156 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 4157 * @param local_cid L2CAP LE Data Channel Identifier 4158 * @param data data to send 4159 * @param size data size 4160 */ 4161 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 4162 4163 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4164 if (!channel) { 4165 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 4166 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4167 } 4168 4169 if (len > channel->remote_mtu){ 4170 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 4171 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 4172 } 4173 4174 if (channel->send_sdu_buffer){ 4175 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 4176 return BTSTACK_ACL_BUFFERS_FULL; 4177 } 4178 4179 channel->send_sdu_buffer = data; 4180 channel->send_sdu_len = len; 4181 channel->send_sdu_pos = 0; 4182 4183 l2cap_notify_channel_can_send(); 4184 return ERROR_CODE_SUCCESS; 4185 } 4186 4187 /** 4188 * @brief Disconnect from LE Data Channel 4189 * @param local_cid L2CAP LE Data Channel Identifier 4190 */ 4191 uint8_t l2cap_le_disconnect(uint16_t local_cid) 4192 { 4193 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4194 if (!channel) { 4195 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 4196 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4197 } 4198 4199 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 4200 l2cap_run(); 4201 return ERROR_CODE_SUCCESS; 4202 } 4203 4204 #endif 4205