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