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