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