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