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