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 1410 if (l2cap_ertm_mode(channel)){ 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_response(l2cap_channel_t * channel, uint8_t * config_options){ 1419 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1420 // use ERTM options if supported 1421 if (l2cap_ertm_mode(channel)){ 1422 return l2cap_setup_options_ertm_response(channel, config_options); 1423 } 1424 #endif 1425 uint16_t mtu = btstack_min(channel->local_mtu, channel->remote_mtu); 1426 return l2cap_setup_options_mtu(config_options, mtu); 1427 } 1428 1429 static uint32_t l2cap_extended_features_mask(void){ 1430 // extended features request supported, features: fixed channels, unicast connectionless data reception 1431 uint32_t features = 0x280; 1432 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1433 features |= 0x0028; 1434 #endif 1435 return features; 1436 } 1437 #endif 1438 1439 // MARK: L2CAP_RUN 1440 // process outstanding signaling tasks 1441 static void l2cap_run(void){ 1442 1443 // log_info("l2cap_run: entered"); 1444 1445 // check pending signaling responses 1446 while (signaling_responses_pending){ 1447 1448 hci_con_handle_t handle = signaling_responses[0].handle; 1449 1450 if (!hci_can_send_acl_packet_now(handle)) break; 1451 1452 uint8_t sig_id = signaling_responses[0].sig_id; 1453 uint8_t response_code = signaling_responses[0].code; 1454 uint16_t result = signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT 1455 #ifdef ENABLE_CLASSIC 1456 uint16_t info_type = signaling_responses[0].data; // INFORMATION_REQUEST 1457 uint16_t source_cid = signaling_responses[0].cid; // CONNECTION_REQUEST 1458 #endif 1459 1460 // remove first item before sending (to avoid sending response mutliple times) 1461 signaling_responses_pending--; 1462 int i; 1463 for (i=0; i < signaling_responses_pending; i++){ 1464 memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t)); 1465 } 1466 1467 switch (response_code){ 1468 #ifdef ENABLE_CLASSIC 1469 case CONNECTION_REQUEST: 1470 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 1471 // also disconnect if result is 0x0003 - security blocked 1472 if (result == 0x0003){ 1473 hci_disconnect_security_block(handle); 1474 } 1475 break; 1476 case ECHO_REQUEST: 1477 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 1478 break; 1479 case INFORMATION_REQUEST: 1480 switch (info_type){ 1481 case L2CAP_INFO_TYPE_CONNECTIONLESS_MTU: { 1482 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 1483 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(connectionless_mtu), &connectionless_mtu); 1484 } 1485 break; 1486 case L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED: { 1487 uint32_t features = l2cap_extended_features_mask(); 1488 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(features), &features); 1489 } 1490 break; 1491 case L2CAP_INFO_TYPE_FIXED_CHANNELS_SUPPORTED: { 1492 uint8_t map[8]; 1493 memset(map, 0, 8); 1494 map[0] = 0x06; // L2CAP Signaling Channel (0x02) + Connectionless reception (0x04) 1495 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 0, sizeof(map), &map); 1496 } 1497 break; 1498 default: 1499 // all other types are not supported 1500 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, info_type, 1, 0, NULL); 1501 break; 1502 } 1503 break; 1504 case COMMAND_REJECT: 1505 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1506 break; 1507 #endif 1508 #ifdef ENABLE_BLE 1509 case LE_CREDIT_BASED_CONNECTION_REQUEST: 1510 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 1511 break; 1512 case COMMAND_REJECT_LE: 1513 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 1514 break; 1515 #endif 1516 default: 1517 // should not happen 1518 break; 1519 } 1520 } 1521 1522 #if defined(ENABLE_CLASSIC) || defined(ENABLE_BLE) 1523 btstack_linked_list_iterator_t it; 1524 #endif 1525 1526 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1527 // send l2cap information request if neccessary 1528 hci_connections_get_iterator(&it); 1529 while(btstack_linked_list_iterator_has_next(&it)){ 1530 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1531 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST){ 1532 if (!hci_can_send_acl_packet_now(connection->con_handle)) break; 1533 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE; 1534 uint8_t sig_id = l2cap_next_sig_id(); 1535 uint8_t info_type = L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED; 1536 l2cap_send_signaling_packet(connection->con_handle, INFORMATION_REQUEST, sig_id, info_type); 1537 return; 1538 } 1539 } 1540 #endif 1541 1542 #ifdef ENABLE_CLASSIC 1543 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1544 uint8_t config_options[18]; 1545 #else 1546 uint8_t config_options[10]; 1547 #endif 1548 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1549 while (btstack_linked_list_iterator_has_next(&it)){ 1550 1551 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1552 1553 if (channel->channel_type != L2CAP_CHANNEL_TYPE_CLASSIC) continue; 1554 1555 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1556 switch (channel->state){ 1557 1558 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1559 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 1560 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1561 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 1562 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 1563 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0); 1564 } 1565 break; 1566 1567 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1568 if (!hci_can_send_command_packet_now()) break; 1569 // send connection request - set state first 1570 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 1571 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 1572 memcpy(l2cap_outgoing_classic_addr, channel->address, 6); 1573 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1); 1574 break; 1575 1576 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 1577 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1578 channel->state = L2CAP_STATE_INVALID; 1579 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0); 1580 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1581 btstack_linked_list_iterator_remove(&it); 1582 l2cap_free_channel_entry(channel); 1583 break; 1584 1585 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 1586 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1587 channel->state = L2CAP_STATE_CONFIG; 1588 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1589 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 1590 break; 1591 1592 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 1593 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1594 // success, start l2cap handshake 1595 channel->local_sig_id = l2cap_next_sig_id(); 1596 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 1597 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 1598 l2cap_start_rtx(channel); 1599 break; 1600 1601 case L2CAP_STATE_CONFIG: 1602 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1603 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1604 // fallback to basic mode if ERTM requested but not not supported by remote 1605 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1606 if (!l2cap_ertm_mode(channel)){ 1607 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 1608 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1609 } 1610 } 1611 #endif 1612 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 1613 uint16_t flags = 0; 1614 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1615 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 1616 flags = 1; 1617 } else { 1618 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1619 } 1620 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 1621 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1622 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL); 1623 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1624 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 1625 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1626 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 1627 uint16_t options_size = l2cap_setup_options_response(channel, config_options); 1628 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); 1629 #endif 1630 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 1631 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1632 uint16_t options_size = l2cap_setup_options_response(channel, config_options); 1633 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); 1634 } else { 1635 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 1636 } 1637 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1638 } 1639 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 1640 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1641 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 1642 channel->local_sig_id = l2cap_next_sig_id(); 1643 uint16_t options_size = l2cap_setup_options_request(channel, config_options); 1644 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, options_size, &config_options); 1645 l2cap_start_rtx(channel); 1646 } 1647 if (l2cap_channel_ready_for_open(channel)){ 1648 channel->state = L2CAP_STATE_OPEN; 1649 l2cap_emit_channel_opened(channel, 0); // success 1650 } 1651 break; 1652 1653 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1654 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1655 channel->state = L2CAP_STATE_INVALID; 1656 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1657 // 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 :) 1658 l2cap_finialize_channel_close(channel); // -- remove from list 1659 channel = NULL; 1660 break; 1661 1662 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1663 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1664 channel->local_sig_id = l2cap_next_sig_id(); 1665 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1666 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1667 break; 1668 default: 1669 break; 1670 } 1671 1672 1673 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1674 1675 // handle channel finalize on L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE 1676 if (!channel) continue; 1677 1678 // ERTM mode 1679 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1680 1681 // check if we can still send 1682 if (channel->con_handle == HCI_CON_HANDLE_INVALID) continue; 1683 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1684 1685 // send if we have more data and remote windows isn't full yet 1686 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); 1687 if (channel->unacked_frames < btstack_min(channel->num_stored_tx_frames, channel->remote_tx_window_size)){ 1688 channel->unacked_frames++; 1689 int index = channel->tx_send_index; 1690 channel->tx_send_index++; 1691 if (channel->tx_send_index >= channel->num_tx_buffers){ 1692 channel->tx_send_index = 0; 1693 } 1694 l2cap_ertm_send_information_frame(channel, index, 0); // final = 0 1695 continue; 1696 } 1697 1698 if (channel->send_supervisor_frame_receiver_ready){ 1699 channel->send_supervisor_frame_receiver_ready = 0; 1700 log_info("Send S-Frame: RR %u, final %u", channel->req_seq, channel->set_final_bit_after_packet_with_poll_bit_set); 1701 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); 1702 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1703 l2cap_ertm_send_supervisor_frame(channel, control); 1704 continue; 1705 } 1706 if (channel->send_supervisor_frame_receiver_ready_poll){ 1707 channel->send_supervisor_frame_receiver_ready_poll = 0; 1708 log_info("Send S-Frame: RR %u with poll=1 ", channel->req_seq); 1709 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY, 1, 0, channel->req_seq); 1710 l2cap_ertm_send_supervisor_frame(channel, control); 1711 continue; 1712 } 1713 if (channel->send_supervisor_frame_receiver_not_ready){ 1714 channel->send_supervisor_frame_receiver_not_ready = 0; 1715 log_info("Send S-Frame: RNR %u", channel->req_seq); 1716 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 1717 l2cap_ertm_send_supervisor_frame(channel, control); 1718 continue; 1719 } 1720 if (channel->send_supervisor_frame_reject){ 1721 channel->send_supervisor_frame_reject = 0; 1722 log_info("Send S-Frame: REJ %u", channel->req_seq); 1723 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT, 0, 0, channel->req_seq); 1724 l2cap_ertm_send_supervisor_frame(channel, control); 1725 continue; 1726 } 1727 if (channel->send_supervisor_frame_selective_reject){ 1728 channel->send_supervisor_frame_selective_reject = 0; 1729 log_info("Send S-Frame: SREJ %u", channel->expected_tx_seq); 1730 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); 1731 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1732 l2cap_ertm_send_supervisor_frame(channel, control); 1733 continue; 1734 } 1735 1736 if (channel->srej_active){ 1737 int i; 1738 for (i=0;i<channel->num_tx_buffers;i++){ 1739 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[i]; 1740 if (tx_state->retransmission_requested) { 1741 tx_state->retransmission_requested = 0; 1742 uint8_t final = channel->set_final_bit_after_packet_with_poll_bit_set; 1743 channel->set_final_bit_after_packet_with_poll_bit_set = 0; 1744 l2cap_ertm_send_information_frame(channel, i, final); 1745 break; 1746 } 1747 } 1748 if (i == channel->num_tx_buffers){ 1749 // no retransmission request found 1750 channel->srej_active = 0; 1751 } else { 1752 // packet was sent 1753 continue; 1754 } 1755 } 1756 } 1757 #endif 1758 1759 } 1760 #endif 1761 1762 #ifdef ENABLE_LE_DATA_CHANNELS 1763 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1764 while (btstack_linked_list_iterator_has_next(&it)){ 1765 uint8_t * acl_buffer; 1766 uint8_t * l2cap_payload; 1767 uint16_t pos; 1768 uint16_t payload_size; 1769 uint16_t mps; 1770 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1771 1772 if (channel->channel_type != L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL) continue; 1773 1774 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 1775 switch (channel->state){ 1776 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 1777 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1778 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1779 // le psm, source cid, mtu, mps, initial credits 1780 channel->local_sig_id = l2cap_next_sig_id(); 1781 channel->credits_incoming = channel->new_credits_incoming; 1782 channel->new_credits_incoming = 0; 1783 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1784 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); 1785 break; 1786 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1787 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1788 // TODO: support larger MPS 1789 channel->state = L2CAP_STATE_OPEN; 1790 channel->credits_incoming = channel->new_credits_incoming; 1791 channel->new_credits_incoming = 0; 1792 mps = btstack_min(l2cap_max_le_mtu(), channel->local_mtu); 1793 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); 1794 // notify client 1795 l2cap_emit_le_channel_opened(channel, 0); 1796 break; 1797 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1798 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1799 channel->state = L2CAP_STATE_INVALID; 1800 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1801 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1802 btstack_linked_list_iterator_remove(&it); 1803 l2cap_free_channel_entry(channel); 1804 break; 1805 case L2CAP_STATE_OPEN: 1806 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1807 1808 // send credits 1809 if (channel->new_credits_incoming){ 1810 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1811 channel->local_sig_id = l2cap_next_sig_id(); 1812 uint16_t new_credits = channel->new_credits_incoming; 1813 channel->new_credits_incoming = 0; 1814 channel->credits_incoming += new_credits; 1815 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1816 break; 1817 } 1818 1819 // send data 1820 if (!channel->send_sdu_buffer) break; 1821 if (!channel->credits_outgoing) break; 1822 1823 // send part of SDU 1824 hci_reserve_packet_buffer(); 1825 acl_buffer = hci_get_outgoing_packet_buffer(); 1826 l2cap_payload = acl_buffer + 8; 1827 pos = 0; 1828 if (!channel->send_sdu_pos){ 1829 // store SDU len 1830 channel->send_sdu_pos += 2; 1831 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 1832 pos += 2; 1833 } 1834 payload_size = btstack_min(channel->send_sdu_len + 2 - channel->send_sdu_pos, channel->remote_mps - pos); 1835 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 1836 memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len 1837 pos += payload_size; 1838 channel->send_sdu_pos += payload_size; 1839 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 1840 // done 1841 1842 channel->credits_outgoing--; 1843 1844 if (channel->send_sdu_pos >= channel->send_sdu_len + 2){ 1845 channel->send_sdu_buffer = NULL; 1846 // send done event 1847 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 1848 // inform about can send now 1849 l2cap_le_notify_channel_can_send(channel); 1850 } 1851 hci_send_acl_packet_buffer(8 + pos); 1852 break; 1853 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1854 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1855 channel->local_sig_id = l2cap_next_sig_id(); 1856 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1857 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1858 break; 1859 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1860 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1861 channel->state = L2CAP_STATE_INVALID; 1862 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1863 l2cap_le_finialize_channel_close(channel); // -- remove from list 1864 break; 1865 default: 1866 break; 1867 } 1868 } 1869 #endif 1870 1871 #ifdef ENABLE_BLE 1872 // send l2cap con paramter update if necessary 1873 hci_connections_get_iterator(&it); 1874 while(btstack_linked_list_iterator_has_next(&it)){ 1875 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1876 if (connection->address_type != BD_ADDR_TYPE_LE_PUBLIC && connection->address_type != BD_ADDR_TYPE_LE_RANDOM) continue; 1877 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1878 switch (connection->le_con_parameter_update_state){ 1879 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1880 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1881 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, l2cap_next_sig_id(), 1882 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1883 break; 1884 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1885 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1886 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1887 break; 1888 case CON_PARAMETER_UPDATE_DENY: 1889 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1890 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1891 break; 1892 default: 1893 break; 1894 } 1895 } 1896 #endif 1897 1898 // log_info("l2cap_run: exit"); 1899 } 1900 1901 #ifdef ENABLE_CLASSIC 1902 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1903 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 1904 log_info("connection complete con_handle %04x - for channel %p cid 0x%04x", (int) con_handle, channel, channel->local_cid); 1905 // success, start l2cap handshake 1906 channel->con_handle = con_handle; 1907 // check remote SSP feature first 1908 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1909 } 1910 } 1911 1912 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1913 1914 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1915 // assumption: outgoing connection 1916 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1917 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1918 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1919 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1920 return; 1921 } 1922 #endif 1923 1924 // fine, go ahead 1925 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1926 } 1927 1928 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1929 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1930 1931 // we have been waiting for remote supported features, if both support SSP, 1932 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)); 1933 if (gap_ssp_supported_on_both_sides(channel->con_handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){ 1934 // request security level 2 1935 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 1936 channel->required_security_level = LEVEL_2; 1937 gap_request_security_level(channel->con_handle, LEVEL_2); 1938 return; 1939 } 1940 1941 l2cap_ready_to_connect(channel); 1942 } 1943 #endif 1944 1945 #ifdef L2CAP_USES_CHANNELS 1946 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, 1947 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 1948 1949 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 1950 if (!channel) { 1951 return NULL; 1952 } 1953 1954 // fill in 1955 channel->packet_handler = packet_handler; 1956 channel->channel_type = channel_type; 1957 bd_addr_copy(channel->address, address); 1958 channel->address_type = address_type; 1959 channel->psm = psm; 1960 channel->local_mtu = local_mtu; 1961 channel->remote_mtu = L2CAP_DEFAULT_MTU; 1962 channel->required_security_level = security_level; 1963 1964 // 1965 channel->local_cid = l2cap_next_local_cid(); 1966 channel->con_handle = HCI_CON_HANDLE_INVALID; 1967 1968 // set initial state 1969 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 1970 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 1971 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 1972 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 1973 1974 log_info("create channel %p, local_cid 0x%04x", channel, channel->local_cid); 1975 1976 return channel; 1977 } 1978 1979 static void l2cap_free_channel_entry(l2cap_channel_t * channel){ 1980 log_info("free channel %p, local_cid 0x%04x", channel, channel->local_cid); 1981 // assert all timers are stopped 1982 l2cap_stop_rtx(channel); 1983 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1984 l2cap_ertm_stop_retransmission_timer(channel); 1985 l2cap_ertm_stop_monitor_timer(channel); 1986 #endif 1987 // free memory 1988 btstack_memory_l2cap_channel_free(channel); 1989 } 1990 #endif 1991 1992 #ifdef ENABLE_CLASSIC 1993 1994 /** 1995 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 1996 * @param packet_handler 1997 * @param address 1998 * @param psm 1999 * @param mtu 2000 * @param local_cid 2001 */ 2002 2003 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){ 2004 // limit MTU to the size of our outtgoing HCI buffer 2005 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 2006 2007 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x mtu %u -> local mtu %u", bd_addr_to_str(address), psm, mtu, local_mtu); 2008 2009 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); 2010 if (!channel) { 2011 return BTSTACK_MEMORY_ALLOC_FAILED; 2012 } 2013 2014 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2015 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2016 #endif 2017 2018 // add to connections list 2019 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 2020 2021 // store local_cid 2022 if (out_local_cid){ 2023 *out_local_cid = channel->local_cid; 2024 } 2025 2026 // check if hci connection is already usable 2027 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 2028 if (conn){ 2029 log_info("l2cap_create_channel, hci connection 0x%04x already exists", conn->con_handle); 2030 l2cap_handle_connection_complete(conn->con_handle, channel); 2031 // check if remote supported fearures are already received 2032 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 2033 l2cap_handle_remote_supported_features_received(channel); 2034 } 2035 } 2036 2037 l2cap_run(); 2038 2039 return 0; 2040 } 2041 2042 void l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 2043 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 2044 // find channel for local_cid 2045 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2046 if (channel) { 2047 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2048 } 2049 // process 2050 l2cap_run(); 2051 } 2052 2053 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 2054 // mark all channels before emitting open events as these could trigger new connetion requests to the same device 2055 btstack_linked_list_iterator_t it; 2056 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2057 while (btstack_linked_list_iterator_has_next(&it)){ 2058 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2059 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2060 if (bd_addr_cmp( channel->address, address) != 0) continue; 2061 // channel for this address found 2062 switch (channel->state){ 2063 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2064 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2065 channel->state = L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD; 2066 break; 2067 default: 2068 break; 2069 } 2070 } 2071 // emit and free marked entries. restart loop to deal with list changes 2072 int done = 0; 2073 while (!done) { 2074 done = 1; 2075 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2076 while (btstack_linked_list_iterator_has_next(&it)){ 2077 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2078 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2079 if (channel->state == L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD){ 2080 done = 0; 2081 // failure, forward error code 2082 l2cap_handle_channel_open_failed(channel, status); 2083 // discard channel 2084 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2085 l2cap_free_channel_entry(channel); 2086 break; 2087 } 2088 } 2089 } 2090 2091 } 2092 2093 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 2094 btstack_linked_list_iterator_t it; 2095 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2096 while (btstack_linked_list_iterator_has_next(&it)){ 2097 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2098 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2099 if ( ! bd_addr_cmp( channel->address, address) ){ 2100 l2cap_handle_connection_complete(handle, channel); 2101 } 2102 } 2103 // process 2104 l2cap_run(); 2105 } 2106 #endif 2107 2108 static void l2cap_notify_channel_can_send(void){ 2109 int done = 0; 2110 while (!done){ 2111 done = 1; 2112 btstack_linked_list_iterator_t it; 2113 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2114 while (btstack_linked_list_iterator_has_next(&it)){ 2115 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2116 if (!channel->waiting_for_can_send_now) continue; 2117 int can_send = 0; 2118 if (l2cap_is_le_channel_type(channel->channel_type)){ 2119 #ifdef ENABLE_BLE 2120 can_send = hci_can_send_acl_le_packet_now(); 2121 #endif 2122 } else { 2123 #ifdef ENABLE_CLASSIC 2124 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2125 // skip ertm channels as they only depend on free buffers in storage 2126 if (channel->mode == L2CAP_CHANNEL_MODE_BASIC){ 2127 can_send = hci_can_send_acl_classic_packet_now(); 2128 } 2129 #else 2130 can_send = hci_can_send_acl_classic_packet_now(); 2131 #endif /* ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE */ 2132 #endif /* ENABLE_CLASSIC */ 2133 } 2134 if (!can_send) continue; 2135 // requeue for fairness 2136 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2137 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel); 2138 // emit can send 2139 channel->waiting_for_can_send_now = 0; 2140 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 2141 // exit inner loop as we just broke the iterator, but try again 2142 done = 0; 2143 break; 2144 } 2145 } 2146 } 2147 2148 #ifdef L2CAP_USES_CHANNELS 2149 2150 static int l2cap_send_open_failed_on_hci_disconnect(l2cap_channel_t * channel){ 2151 // open cannot fail for for incoming connections 2152 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING) return 0; 2153 2154 // check state 2155 switch (channel->state){ 2156 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 2157 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 2158 case L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES: 2159 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2160 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 2161 case L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES: 2162 case L2CAP_STATE_WAIT_CONNECT_RSP: 2163 case L2CAP_STATE_CONFIG: 2164 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 2165 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 2166 case L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE: 2167 case L2CAP_STATE_EMIT_OPEN_FAILED_AND_DISCARD: 2168 return 1; 2169 2170 case L2CAP_STATE_OPEN: 2171 case L2CAP_STATE_CLOSED: 2172 case L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES: 2173 case L2CAP_STATE_WAIT_DISCONNECT: 2174 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_INSUFFICIENT_SECURITY: 2175 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 2176 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 2177 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2178 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 2179 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 2180 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 2181 case L2CAP_STATE_INVALID: 2182 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2183 return 0; 2184 // no default here, to get a warning about new states 2185 } 2186 // still, the compiler insists on a return value 2187 return 0; 2188 } 2189 #endif 2190 2191 #ifdef ENABLE_CLASSIC 2192 static void l2cap_handle_hci_disconnect_event(l2cap_channel_t * channel){ 2193 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2194 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2195 } else { 2196 l2cap_handle_channel_closed(channel); 2197 } 2198 l2cap_free_channel_entry(channel); 2199 } 2200 #endif 2201 2202 #ifdef ENABLE_LE_DATA_CHANNELS 2203 static void l2cap_handle_hci_le_disconnect_event(l2cap_channel_t * channel){ 2204 if (l2cap_send_open_failed_on_hci_disconnect(channel)){ 2205 l2cap_emit_le_channel_opened(channel, L2CAP_CONNECTION_BASEBAND_DISCONNECT); 2206 } else { 2207 l2cap_emit_le_channel_closed(channel); 2208 } 2209 l2cap_free_channel_entry(channel); 2210 } 2211 #endif 2212 2213 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 2214 2215 UNUSED(packet_type); // ok: registered with hci_event_callback_registration 2216 UNUSED(cid); // ok: there is no channel 2217 UNUSED(size); // ok: fixed format events read from HCI buffer 2218 2219 #ifdef ENABLE_CLASSIC 2220 bd_addr_t address; 2221 int hci_con_used; 2222 #endif 2223 #ifdef L2CAP_USES_CHANNELS 2224 hci_con_handle_t handle; 2225 btstack_linked_list_iterator_t it; 2226 #endif 2227 2228 switch(hci_event_packet_get_type(packet)){ 2229 2230 // Notify channel packet handler if they can send now 2231 case HCI_EVENT_TRANSPORT_PACKET_SENT: 2232 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 2233 case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED: 2234 l2cap_run(); // try sending signaling packets first 2235 l2cap_notify_channel_can_send(); 2236 break; 2237 2238 case HCI_EVENT_COMMAND_STATUS: 2239 #ifdef ENABLE_CLASSIC 2240 // check command status for create connection for errors 2241 if (HCI_EVENT_IS_COMMAND_STATUS(packet, hci_create_connection)){ 2242 // cache outgoing address and reset 2243 memcpy(address, l2cap_outgoing_classic_addr, 6); 2244 memset(l2cap_outgoing_classic_addr, 0, 6); 2245 // error => outgoing connection failed 2246 uint8_t status = hci_event_command_status_get_status(packet); 2247 if (status){ 2248 l2cap_handle_connection_failed_for_addr(address, status); 2249 } 2250 } 2251 #endif 2252 l2cap_run(); // try sending signaling packets first 2253 break; 2254 2255 #ifdef ENABLE_CLASSIC 2256 // handle connection complete events 2257 case HCI_EVENT_CONNECTION_COMPLETE: 2258 reverse_bd_addr(&packet[5], address); 2259 if (packet[2] == 0){ 2260 handle = little_endian_read_16(packet, 3); 2261 l2cap_handle_connection_success_for_addr(address, handle); 2262 } else { 2263 l2cap_handle_connection_failed_for_addr(address, packet[2]); 2264 } 2265 break; 2266 2267 // handle successful create connection cancel command 2268 case HCI_EVENT_COMMAND_COMPLETE: 2269 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 2270 if (packet[5] == 0){ 2271 reverse_bd_addr(&packet[6], address); 2272 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 2273 l2cap_handle_connection_failed_for_addr(address, 0x16); 2274 } 2275 } 2276 l2cap_run(); // try sending signaling packets first 2277 break; 2278 #endif 2279 2280 #ifdef L2CAP_USES_CHANNELS 2281 // handle disconnection complete events 2282 case HCI_EVENT_DISCONNECTION_COMPLETE: 2283 handle = little_endian_read_16(packet, 3); 2284 // send l2cap open failed or closed events for all channels on this handle and free them 2285 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2286 while (btstack_linked_list_iterator_has_next(&it)){ 2287 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2288 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2289 if (channel->con_handle != handle) continue; 2290 btstack_linked_list_iterator_remove(&it); 2291 switch(channel->channel_type){ 2292 #ifdef ENABLE_CLASSIC 2293 case L2CAP_CHANNEL_TYPE_CLASSIC: 2294 l2cap_handle_hci_disconnect_event(channel); 2295 break; 2296 #endif 2297 #ifdef ENABLE_LE_DATA_CHANNELS 2298 case L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL: 2299 l2cap_handle_hci_le_disconnect_event(channel); 2300 break; 2301 #endif 2302 default: 2303 break; 2304 } 2305 } 2306 break; 2307 #endif 2308 2309 2310 // HCI Connection Timeouts 2311 #ifdef ENABLE_CLASSIC 2312 case L2CAP_EVENT_TIMEOUT_CHECK: 2313 handle = little_endian_read_16(packet, 2); 2314 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 2315 if (hci_authentication_active_for_handle(handle)) break; 2316 hci_con_used = 0; 2317 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2318 while (btstack_linked_list_iterator_has_next(&it)){ 2319 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2320 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2321 if (channel->con_handle != handle) continue; 2322 hci_con_used = 1; 2323 break; 2324 } 2325 if (hci_con_used) break; 2326 if (!hci_can_send_command_packet_now()) break; 2327 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 2328 break; 2329 2330 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 2331 handle = little_endian_read_16(packet, 3); 2332 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2333 while (btstack_linked_list_iterator_has_next(&it)){ 2334 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2335 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2336 if (channel->con_handle != handle) continue; 2337 log_info("remote supported features, channel %p, cid %04x - state %u", channel, channel->local_cid, channel->state); 2338 l2cap_handle_remote_supported_features_received(channel); 2339 } 2340 break; 2341 2342 case GAP_EVENT_SECURITY_LEVEL: 2343 handle = little_endian_read_16(packet, 2); 2344 log_info("l2cap - security level update for handle 0x%04x", handle); 2345 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2346 while (btstack_linked_list_iterator_has_next(&it)){ 2347 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2348 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2349 if (channel->con_handle != handle) continue; 2350 2351 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 2352 gap_security_level_t required_level = channel->required_security_level; 2353 2354 log_info("channel %p, cid %04x - state %u: actual %u >= required %u?", channel, channel->local_cid, channel->state, actual_level, required_level); 2355 2356 switch (channel->state){ 2357 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 2358 if (actual_level >= required_level){ 2359 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2360 // we need to know if ERTM is supported before sending a config response 2361 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 2362 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 2363 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 2364 #else 2365 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2366 l2cap_emit_incoming_connection(channel); 2367 #endif 2368 } else { 2369 channel->reason = 0x0003; // security block 2370 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2371 } 2372 break; 2373 2374 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 2375 if (actual_level >= required_level){ 2376 l2cap_ready_to_connect(channel); 2377 } else { 2378 // disconnnect, authentication not good enough 2379 hci_disconnect_security_block(handle); 2380 } 2381 break; 2382 2383 default: 2384 break; 2385 } 2386 } 2387 break; 2388 #endif 2389 2390 default: 2391 break; 2392 } 2393 2394 l2cap_run(); 2395 } 2396 2397 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 2398 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 2399 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 2400 signaling_responses[signaling_responses_pending].handle = handle; 2401 signaling_responses[signaling_responses_pending].code = code; 2402 signaling_responses[signaling_responses_pending].sig_id = sig_id; 2403 signaling_responses[signaling_responses_pending].cid = cid; 2404 signaling_responses[signaling_responses_pending].data = data; 2405 signaling_responses_pending++; 2406 l2cap_run(); 2407 } 2408 } 2409 2410 #ifdef ENABLE_CLASSIC 2411 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 2412 channel->remote_sig_id = identifier; 2413 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2414 l2cap_run(); 2415 } 2416 2417 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 2418 2419 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 2420 l2cap_service_t *service = l2cap_get_service(psm); 2421 if (!service) { 2422 // 0x0002 PSM not supported 2423 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2424 return; 2425 } 2426 2427 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 2428 if (!hci_connection) { 2429 // 2430 log_error("no hci_connection for handle %u", handle); 2431 return; 2432 } 2433 2434 // alloc structure 2435 // log_info("l2cap_handle_connection_request register channel"); 2436 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_CLASSIC, hci_connection->address, BD_ADDR_TYPE_CLASSIC, 2437 psm, service->mtu, service->required_security_level); 2438 if (!channel){ 2439 // 0x0004 No resources available 2440 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2441 return; 2442 } 2443 2444 channel->con_handle = handle; 2445 channel->remote_cid = source_cid; 2446 channel->remote_sig_id = sig_id; 2447 2448 // limit local mtu to max acl packet length - l2cap header 2449 if (channel->local_mtu > l2cap_max_mtu()) { 2450 channel->local_mtu = l2cap_max_mtu(); 2451 } 2452 2453 // set initial state 2454 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 2455 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 2456 2457 // add to connections list 2458 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 2459 2460 // assert security requirements 2461 gap_request_security_level(handle, channel->required_security_level); 2462 } 2463 2464 void l2cap_accept_connection(uint16_t local_cid){ 2465 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 2466 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 2467 if (!channel) { 2468 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 2469 return; 2470 } 2471 2472 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2473 // configure L2CAP Basic mode 2474 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2475 #endif 2476 2477 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 2478 2479 // process 2480 l2cap_run(); 2481 } 2482 2483 void l2cap_decline_connection(uint16_t local_cid){ 2484 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 2485 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 2486 if (!channel) { 2487 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 2488 return; 2489 } 2490 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 2491 channel->reason = 0x04; // no resources available 2492 l2cap_run(); 2493 } 2494 2495 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2496 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 2497 2498 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2499 uint8_t use_fcs = 1; 2500 #endif 2501 2502 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2503 2504 uint16_t flags = little_endian_read_16(command, 6); 2505 if (flags & 1) { 2506 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 2507 } 2508 2509 // accept the other's configuration options 2510 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2511 uint16_t pos = 8; 2512 while (pos < end_pos){ 2513 uint8_t option_hint = command[pos] >> 7; 2514 uint8_t option_type = command[pos] & 0x7f; 2515 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2516 pos++; 2517 uint8_t length = command[pos++]; 2518 // MTU { type(8): 1, len(8):2, MTU(16) } 2519 if (option_type == L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT && length == 2){ 2520 channel->remote_mtu = little_endian_read_16(command, pos); 2521 log_info("Remote MTU %u", channel->remote_mtu); 2522 if (channel->remote_mtu > l2cap_max_mtu()){ 2523 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 2524 channel->remote_mtu = l2cap_max_mtu(); 2525 } 2526 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2527 } 2528 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 2529 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FLUSH_TIMEOUT && length == 2){ 2530 channel->flush_timeout = little_endian_read_16(command, pos); 2531 log_info("Flush timeout: %u ms", channel->flush_timeout); 2532 } 2533 2534 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2535 // Retransmission and Flow Control Option 2536 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2537 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 2538 switch(channel->mode){ 2539 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2540 // Store remote config 2541 channel->remote_tx_window_size = command[pos+1]; 2542 channel->remote_max_transmit = command[pos+2]; 2543 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 2544 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 2545 channel->remote_mps = little_endian_read_16(command, pos + 7); 2546 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u, mps %u", 2547 channel->remote_tx_window_size, 2548 channel->remote_max_transmit, 2549 channel->remote_retransmission_timeout_ms, 2550 channel->remote_monitor_timeout_ms, 2551 channel->remote_mps); 2552 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 2553 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2554 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2555 } else { 2556 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2557 } 2558 break; 2559 case L2CAP_CHANNEL_MODE_BASIC: 2560 switch (mode){ 2561 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2562 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 2563 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 2564 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2565 } 2566 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 2567 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 2568 break; 2569 default: // case L2CAP_CHANNEL_MODE_BASIC: 2570 // TODO store and evaluate configuration 2571 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 2572 break; 2573 } 2574 break; 2575 default: 2576 break; 2577 } 2578 } 2579 if (option_type == L2CAP_CONFIG_OPTION_TYPE_FRAME_CHECK_SEQUENCE && length == 1){ 2580 use_fcs = command[pos]; 2581 } 2582 #endif 2583 // check for unknown options 2584 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2585 log_info("l2cap cid %u, unknown options", channel->local_cid); 2586 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2587 } 2588 pos += length; 2589 } 2590 2591 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2592 // "FCS" has precedence over "No FCS" 2593 uint8_t update = channel->fcs_option || use_fcs; 2594 log_info("local fcs: %u, remote fcs: %u -> %u", channel->fcs_option, use_fcs, update); 2595 channel->fcs_option = update; 2596 #endif 2597 } 2598 2599 // @pre command len is valid, see check in l2cap_signaling_handler_channel 2600 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 2601 log_info("l2cap_signaling_handle_configure_response"); 2602 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2603 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2604 uint16_t pos = 10; 2605 while (pos < end_pos){ 2606 uint8_t option_hint = command[pos] >> 7; 2607 uint8_t option_type = command[pos] & 0x7f; 2608 // log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 2609 pos++; 2610 uint8_t length = command[pos++]; 2611 2612 // Retransmission and Flow Control Option 2613 if (option_type == L2CAP_CONFIG_OPTION_TYPE_RETRANSMISSION_AND_FLOW_CONTROL && length == 9){ 2614 switch (channel->mode){ 2615 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 2616 if (channel->ertm_mandatory){ 2617 // ?? 2618 } else { 2619 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 2620 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2621 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2622 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2623 } 2624 } 2625 break; 2626 case L2CAP_CHANNEL_MODE_BASIC: 2627 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 2628 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 2629 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2630 } 2631 break; 2632 default: 2633 break; 2634 } 2635 } 2636 2637 // check for unknown options 2638 if (option_hint == 0 && (option_type < L2CAP_CONFIG_OPTION_TYPE_MAX_TRANSMISSION_UNIT || option_type > L2CAP_CONFIG_OPTION_TYPE_EXTENDED_WINDOW_SIZE)){ 2639 log_info("l2cap cid %u, unknown options", channel->local_cid); 2640 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 2641 } 2642 2643 pos += length; 2644 } 2645 #else 2646 UNUSED(channel); // ok: no code 2647 UNUSED(result); // ok: no code 2648 UNUSED(command); // ok: no code 2649 #endif 2650 } 2651 2652 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 2653 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 2654 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 2655 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 2656 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 2657 if (channel->state == L2CAP_STATE_OPEN) return 0; 2658 return 1; 2659 } 2660 2661 2662 // @pre command len is valid, see check in l2cap_signaling_handler_dispatch 2663 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 2664 2665 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2666 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2667 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2668 uint16_t result = 0; 2669 2670 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 2671 2672 // handle DISCONNECT REQUESTS seperately 2673 if (code == DISCONNECTION_REQUEST){ 2674 switch (channel->state){ 2675 case L2CAP_STATE_CONFIG: 2676 case L2CAP_STATE_OPEN: 2677 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 2678 case L2CAP_STATE_WAIT_DISCONNECT: 2679 l2cap_handle_disconnect_request(channel, identifier); 2680 break; 2681 2682 default: 2683 // ignore in other states 2684 break; 2685 } 2686 return; 2687 } 2688 2689 // @STATEMACHINE(l2cap) 2690 switch (channel->state) { 2691 2692 case L2CAP_STATE_WAIT_CONNECT_RSP: 2693 switch (code){ 2694 case CONNECTION_RESPONSE: 2695 if (cmd_len < 8){ 2696 // command imcomplete 2697 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2698 break; 2699 } 2700 l2cap_stop_rtx(channel); 2701 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2702 switch (result) { 2703 case 0: 2704 // successful connection 2705 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2706 channel->state = L2CAP_STATE_CONFIG; 2707 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2708 break; 2709 case 1: 2710 // connection pending. get some coffee, but start the ERTX 2711 l2cap_start_ertx(channel); 2712 break; 2713 default: 2714 // channel closed 2715 channel->state = L2CAP_STATE_CLOSED; 2716 // map l2cap connection response result to BTstack status enumeration 2717 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 2718 2719 // drop link key if security block 2720 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 2721 gap_drop_link_key_for_bd_addr(channel->address); 2722 } 2723 2724 // discard channel 2725 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2726 l2cap_free_channel_entry(channel); 2727 break; 2728 } 2729 break; 2730 2731 default: 2732 //@TODO: implement other signaling packets 2733 break; 2734 } 2735 break; 2736 2737 case L2CAP_STATE_CONFIG: 2738 switch (code) { 2739 case CONFIGURE_REQUEST: 2740 if (cmd_len < 4){ 2741 // command incomplete 2742 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2743 break; 2744 } 2745 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 2746 l2cap_signaling_handle_configure_request(channel, command); 2747 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 2748 // only done if continuation not set 2749 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 2750 } 2751 break; 2752 case CONFIGURE_RESPONSE: 2753 if (cmd_len < 6){ 2754 // command incomplete 2755 l2cap_register_signaling_response(channel->con_handle, COMMAND_REJECT, identifier, 0, L2CAP_REJ_CMD_UNKNOWN); 2756 break; 2757 } 2758 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2759 l2cap_stop_rtx(channel); 2760 l2cap_signaling_handle_configure_response(channel, result, command); 2761 switch (result){ 2762 case 0: // success 2763 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 2764 break; 2765 case 4: // pending 2766 l2cap_start_ertx(channel); 2767 break; 2768 default: 2769 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2770 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 2771 // remote does not offer ertm but it's required 2772 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2773 break; 2774 } 2775 #endif 2776 // retry on negative result 2777 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2778 break; 2779 } 2780 break; 2781 default: 2782 break; 2783 } 2784 if (l2cap_channel_ready_for_open(channel)){ 2785 2786 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2787 // assert that packet can be stored in fragment buffers in ertm 2788 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2789 uint16_t effective_mps = btstack_min(channel->remote_mps, channel->local_mps); 2790 uint16_t usable_mtu = channel->num_tx_buffers == 1 ? effective_mps : channel->num_tx_buffers * effective_mps - 2; 2791 if (usable_mtu < channel->remote_mtu){ 2792 log_info("Remote MTU %u > max storable ERTM packet, only using MTU = %u", channel->remote_mtu, usable_mtu); 2793 channel->remote_mtu = usable_mtu; 2794 } 2795 } 2796 #endif 2797 // for open: 2798 channel->state = L2CAP_STATE_OPEN; 2799 l2cap_emit_channel_opened(channel, 0); 2800 } 2801 break; 2802 2803 case L2CAP_STATE_WAIT_DISCONNECT: 2804 switch (code) { 2805 case DISCONNECTION_RESPONSE: 2806 l2cap_finialize_channel_close(channel); 2807 break; 2808 default: 2809 //@TODO: implement other signaling packets 2810 break; 2811 } 2812 break; 2813 2814 case L2CAP_STATE_CLOSED: 2815 // @TODO handle incoming requests 2816 break; 2817 2818 case L2CAP_STATE_OPEN: 2819 //@TODO: implement other signaling packets, e.g. re-configure 2820 break; 2821 default: 2822 break; 2823 } 2824 // log_info("new state %u", channel->state); 2825 } 2826 2827 2828 // @pre command len is valid, see check in l2cap_acl_classic_handler 2829 static void l2cap_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command){ 2830 2831 btstack_linked_list_iterator_t it; 2832 2833 // get code, signalind identifier and command len 2834 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2835 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2836 uint16_t cmd_len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2837 2838 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 2839 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_RESPONSE){ 2840 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2841 return; 2842 } 2843 2844 // general commands without an assigned channel 2845 switch(code) { 2846 2847 case CONNECTION_REQUEST: 2848 if (cmd_len == 4){ 2849 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2850 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2851 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 2852 } else { 2853 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2854 } 2855 return; 2856 2857 case ECHO_REQUEST: 2858 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 2859 return; 2860 2861 case INFORMATION_REQUEST: 2862 if (cmd_len == 2) { 2863 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2864 l2cap_register_signaling_response(handle, code, sig_id, 0, info_type); 2865 } else { 2866 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2867 } 2868 return; 2869 2870 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2871 case INFORMATION_RESPONSE: { 2872 hci_connection_t * connection = hci_connection_for_handle(handle); 2873 if (!connection) return; 2874 if (connection->l2cap_state.information_state != L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE) return; 2875 2876 // get extended features from response if valid 2877 connection->l2cap_state.extended_feature_mask = 0; 2878 if (cmd_len >= 6) { 2879 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2880 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2881 if (result == 0 && info_type == L2CAP_INFO_TYPE_EXTENDED_FEATURES_SUPPORTED) { 2882 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2883 } 2884 } 2885 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2886 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2887 2888 // trigger connection request 2889 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2890 while (btstack_linked_list_iterator_has_next(&it)){ 2891 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2892 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2893 if (channel->con_handle != handle) continue; 2894 2895 // incoming connection: ask user for channel configuration, esp. if ertm will be mandatory 2896 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2897 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2898 l2cap_emit_incoming_connection(channel); 2899 continue; 2900 } 2901 2902 // outgoing connection 2903 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2904 2905 // if ERTM was requested, but is not listed in extended feature mask: 2906 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2907 2908 if (channel->ertm_mandatory){ 2909 // bail if ERTM is mandatory 2910 channel->state = L2CAP_STATE_CLOSED; 2911 // map l2cap connection response result to BTstack status enumeration 2912 l2cap_handle_channel_open_failed(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTED); 2913 // discard channel 2914 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2915 l2cap_free_channel_entry(channel); 2916 continue; 2917 2918 } else { 2919 // fallback to Basic mode 2920 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_ERTM_BUFFER_RELEASED); 2921 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2922 } 2923 } 2924 2925 // respond to connection request 2926 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 2927 continue; 2928 } 2929 } 2930 return; 2931 } 2932 #endif 2933 2934 default: 2935 break; 2936 } 2937 2938 // Get potential destination CID 2939 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2940 2941 // Find channel for this sig_id and connection handle 2942 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2943 while (btstack_linked_list_iterator_has_next(&it)){ 2944 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2945 if (!l2cap_is_dynamic_channel_type(channel->channel_type)) continue; 2946 if (channel->con_handle != handle) continue; 2947 if (code & 1) { 2948 // match odd commands (responses) by previous signaling identifier 2949 if (channel->local_sig_id == sig_id) { 2950 l2cap_signaling_handler_channel(channel, command); 2951 break; 2952 } 2953 } else { 2954 // match even commands (requests) by local channel id 2955 if (channel->local_cid == dest_cid) { 2956 l2cap_signaling_handler_channel(channel, command); 2957 break; 2958 } 2959 } 2960 } 2961 } 2962 #endif 2963 2964 #ifdef ENABLE_BLE 2965 2966 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 2967 uint8_t event[6]; 2968 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 2969 event[1] = 4; 2970 little_endian_store_16(event, 2, con_handle); 2971 little_endian_store_16(event, 4, result); 2972 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2973 if (!l2cap_event_packet_handler) return; 2974 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 2975 } 2976 2977 // @returns valid 2978 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 2979 hci_connection_t * connection; 2980 uint16_t result; 2981 uint8_t event[12]; 2982 2983 #ifdef ENABLE_LE_DATA_CHANNELS 2984 btstack_linked_list_iterator_t it; 2985 l2cap_channel_t * channel; 2986 uint16_t local_cid; 2987 uint16_t le_psm; 2988 uint16_t new_credits; 2989 uint16_t credits_before; 2990 l2cap_service_t * service; 2991 uint16_t source_cid; 2992 #endif 2993 2994 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2995 uint16_t len = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2996 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u, len %u", code, sig_id, len); 2997 2998 switch (code){ 2999 3000 case CONNECTION_PARAMETER_UPDATE_REQUEST: 3001 // check size 3002 if (len < 8) return 0; 3003 connection = hci_connection_for_handle(handle); 3004 if (connection){ 3005 if (connection->role != HCI_ROLE_MASTER){ 3006 // reject command without notifying upper layer when not in master role 3007 return 0; 3008 } 3009 le_connection_parameter_range_t existing_range; 3010 gap_get_connection_parameter_range(&existing_range); 3011 uint16_t le_conn_interval_min = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 3012 uint16_t le_conn_interval_max = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 3013 uint16_t le_conn_latency = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 3014 uint16_t le_supervision_timeout = little_endian_read_16(command,L2CAP_SIGNALING_COMMAND_DATA_OFFSET+6); 3015 3016 int update_parameter = gap_connection_parameter_range_included(&existing_range, le_conn_interval_min, le_conn_interval_max, le_conn_latency, le_supervision_timeout); 3017 if (update_parameter){ 3018 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 3019 connection->le_conn_interval_min = le_conn_interval_min; 3020 connection->le_conn_interval_max = le_conn_interval_max; 3021 connection->le_conn_latency = le_conn_latency; 3022 connection->le_supervision_timeout = le_supervision_timeout; 3023 } else { 3024 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 3025 } 3026 connection->le_con_param_update_identifier = sig_id; 3027 } 3028 3029 if (!l2cap_event_packet_handler) break; 3030 3031 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 3032 event[1] = 8; 3033 little_endian_store_16(event, 2, handle); 3034 memcpy(&event[4], &command[4], 8); 3035 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3036 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3037 break; 3038 3039 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 3040 // check size 3041 if (len < 2) return 0; 3042 result = little_endian_read_16(command, 4); 3043 l2cap_emit_connection_parameter_update_response(handle, result); 3044 break; 3045 3046 #ifdef ENABLE_LE_DATA_CHANNELS 3047 3048 case COMMAND_REJECT: 3049 // Find channel for this sig_id and connection handle 3050 channel = NULL; 3051 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3052 while (btstack_linked_list_iterator_has_next(&it)){ 3053 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3054 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3055 if (a_channel->con_handle != handle) continue; 3056 if (a_channel->local_sig_id != sig_id) continue; 3057 channel = a_channel; 3058 break; 3059 } 3060 if (!channel) break; 3061 3062 // if received while waiting for le connection response, assume legacy device 3063 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 3064 channel->state = L2CAP_STATE_CLOSED; 3065 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 3066 l2cap_emit_le_channel_opened(channel, 0x0002); 3067 3068 // discard channel 3069 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3070 l2cap_free_channel_entry(channel); 3071 break; 3072 } 3073 break; 3074 3075 case LE_CREDIT_BASED_CONNECTION_REQUEST: 3076 // check size 3077 if (len < 10) return 0; 3078 3079 // get hci connection, bail if not found (must not happen) 3080 connection = hci_connection_for_handle(handle); 3081 if (!connection) return 0; 3082 3083 // check if service registered 3084 le_psm = little_endian_read_16(command, 4); 3085 service = l2cap_le_get_service(le_psm); 3086 source_cid = little_endian_read_16(command, 6); 3087 3088 if (service){ 3089 if (source_cid < 0x40){ 3090 // 0x0009 Connection refused - Invalid Source CID 3091 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 3092 return 1; 3093 } 3094 3095 // go through list of channels for this ACL connection and check if we get a match 3096 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3097 while (btstack_linked_list_iterator_has_next(&it)){ 3098 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3099 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3100 if (a_channel->con_handle != handle) continue; 3101 if (a_channel->remote_cid != source_cid) continue; 3102 // 0x000a Connection refused - Source CID already allocated 3103 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 3104 return 1; 3105 } 3106 3107 // security: check encryption 3108 if (service->required_security_level >= LEVEL_2){ 3109 if (gap_encryption_key_size(handle) == 0){ 3110 // 0x0008 Connection refused - insufficient encryption 3111 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 3112 return 1; 3113 } 3114 // anything less than 16 byte key size is insufficient 3115 if (gap_encryption_key_size(handle) < 16){ 3116 // 0x0007 Connection refused – insufficient encryption key size 3117 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 3118 return 1; 3119 } 3120 } 3121 3122 // security: check authencation 3123 if (service->required_security_level >= LEVEL_3){ 3124 if (!gap_authenticated(handle)){ 3125 // 0x0005 Connection refused – insufficient authentication 3126 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 3127 return 1; 3128 } 3129 } 3130 3131 // security: check authorization 3132 if (service->required_security_level >= LEVEL_4){ 3133 if (gap_authorization_state(handle) != AUTHORIZATION_GRANTED){ 3134 // 0x0006 Connection refused – insufficient authorization 3135 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 3136 return 1; 3137 } 3138 } 3139 3140 // allocate channel 3141 channel = l2cap_create_channel_entry(service->packet_handler, L2CAP_CHANNEL_TYPE_LE_DATA_CHANNEL, connection->address, 3142 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 3143 if (!channel){ 3144 // 0x0004 Connection refused – no resources available 3145 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 3146 return 1; 3147 } 3148 3149 channel->con_handle = handle; 3150 channel->remote_cid = source_cid; 3151 channel->remote_sig_id = sig_id; 3152 channel->remote_mtu = little_endian_read_16(command, 8); 3153 channel->remote_mps = little_endian_read_16(command, 10); 3154 channel->credits_outgoing = little_endian_read_16(command, 12); 3155 3156 // set initial state 3157 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 3158 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 3159 3160 // add to connections list 3161 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 3162 3163 // post connection request event 3164 l2cap_emit_le_incoming_connection(channel); 3165 3166 } else { 3167 // Connection refused – LE_PSM not supported 3168 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 3169 } 3170 break; 3171 3172 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 3173 // check size 3174 if (len < 10) return 0; 3175 3176 // Find channel for this sig_id and connection handle 3177 channel = NULL; 3178 btstack_linked_list_iterator_init(&it, &l2cap_channels); 3179 while (btstack_linked_list_iterator_has_next(&it)){ 3180 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 3181 if (!l2cap_is_dynamic_channel_type(a_channel->channel_type)) continue; 3182 if (a_channel->con_handle != handle) continue; 3183 if (a_channel->local_sig_id != sig_id) continue; 3184 channel = a_channel; 3185 break; 3186 } 3187 if (!channel) break; 3188 3189 // cid + 0 3190 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 3191 if (result){ 3192 channel->state = L2CAP_STATE_CLOSED; 3193 // map l2cap connection response result to BTstack status enumeration 3194 l2cap_emit_le_channel_opened(channel, result); 3195 3196 // discard channel 3197 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3198 l2cap_free_channel_entry(channel); 3199 break; 3200 } 3201 3202 // success 3203 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3204 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3205 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 3206 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 3207 channel->state = L2CAP_STATE_OPEN; 3208 l2cap_emit_le_channel_opened(channel, result); 3209 break; 3210 3211 case LE_FLOW_CONTROL_CREDIT: 3212 // check size 3213 if (len < 4) return 0; 3214 3215 // find channel 3216 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3217 channel = l2cap_get_channel_for_local_cid(local_cid); 3218 if (!channel) { 3219 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3220 break; 3221 } 3222 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 3223 credits_before = channel->credits_outgoing; 3224 channel->credits_outgoing += new_credits; 3225 // check for credit overrun 3226 if (credits_before > channel->credits_outgoing){ 3227 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 3228 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3229 break; 3230 } 3231 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 3232 break; 3233 3234 case DISCONNECTION_REQUEST: 3235 3236 // check size 3237 if (len < 4) return 0; 3238 3239 // find channel 3240 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 3241 channel = l2cap_get_channel_for_local_cid(local_cid); 3242 if (!channel) { 3243 log_error("l2cap: no channel for cid 0x%02x", local_cid); 3244 break; 3245 } 3246 channel->remote_sig_id = sig_id; 3247 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 3248 break; 3249 3250 #endif 3251 3252 case DISCONNECTION_RESPONSE: 3253 break; 3254 3255 default: 3256 // command unknown -> reject command 3257 return 0; 3258 } 3259 return 1; 3260 } 3261 #endif 3262 3263 static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3264 #ifdef ENABLE_CLASSIC 3265 l2cap_channel_t * l2cap_channel; 3266 l2cap_fixed_channel_t * l2cap_fixed_channel; 3267 3268 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3269 switch (channel_id) { 3270 3271 case L2CAP_CID_SIGNALING: { 3272 uint32_t command_offset = 8; 3273 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) { 3274 // assert signaling command is fully inside packet 3275 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 3276 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len; 3277 if (next_command_offset > size){ 3278 log_error("l2cap signaling command len invalid -> drop"); 3279 break; 3280 } 3281 // handle signaling command 3282 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 3283 // go to next command 3284 command_offset = next_command_offset; 3285 } 3286 break; 3287 } 3288 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 3289 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL); 3290 if (!l2cap_fixed_channel) break; 3291 if (!l2cap_fixed_channel->packet_handler) break; 3292 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3293 break; 3294 3295 default: 3296 // Find channel for this channel_id and connection handle 3297 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 3298 if (l2cap_channel) { 3299 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 3300 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 3301 3302 int fcs_size = l2cap_channel->fcs_option ? 2 : 0; 3303 3304 // assert control + FCS fields are inside 3305 if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) break; 3306 3307 if (l2cap_channel->fcs_option){ 3308 // verify FCS (required if one side requested it) 3309 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 3310 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 3311 3312 #ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL 3313 // simulate fcs error 3314 static int counter = 0; 3315 if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) { 3316 log_info("Simulate fcs error"); 3317 fcs_calculated++; 3318 counter = 0; 3319 } 3320 #endif 3321 3322 if (fcs_calculated == fcs_packet){ 3323 log_info("Packet FCS 0x%04x verified", fcs_packet); 3324 } else { 3325 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 3326 // ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS' 3327 break; 3328 } 3329 } 3330 3331 // switch on packet type 3332 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3333 uint8_t req_seq = (control >> 8) & 0x3f; 3334 int final = (control >> 7) & 0x01; 3335 if (control & 1){ 3336 // S-Frame 3337 int poll = (control >> 4) & 0x01; 3338 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 3339 log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq); 3340 l2cap_ertm_tx_packet_state_t * tx_state; 3341 switch (s){ 3342 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 3343 log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY"); 3344 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3345 if (poll && final){ 3346 // S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time. 3347 log_error("P=F=1 in S-Frame"); 3348 break; 3349 } 3350 if (poll){ 3351 // check if we did request selective retransmission before <==> we have stored SDU segments 3352 int i; 3353 int num_stored_out_of_order_packets = 0; 3354 for (i=0;i<l2cap_channel->num_rx_buffers;i++){ 3355 int index = l2cap_channel->rx_store_index + i; 3356 if (index >= l2cap_channel->num_rx_buffers){ 3357 index -= l2cap_channel->num_rx_buffers; 3358 } 3359 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3360 if (!rx_state->valid) continue; 3361 num_stored_out_of_order_packets++; 3362 } 3363 if (num_stored_out_of_order_packets){ 3364 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3365 } else { 3366 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3367 } 3368 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1; 3369 } 3370 if (final){ 3371 // Stop-MonitorTimer 3372 l2cap_ertm_stop_monitor_timer(l2cap_channel); 3373 // If UnackedFrames > 0 then Start-RetransTimer 3374 if (l2cap_channel->unacked_frames){ 3375 l2cap_ertm_start_retransmission_timer(l2cap_channel); 3376 } 3377 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3378 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3379 } 3380 break; 3381 case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT: 3382 log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT"); 3383 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3384 // restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq) 3385 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3386 break; 3387 case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY: 3388 log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY"); 3389 break; 3390 case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT: 3391 log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT"); 3392 if (poll){ 3393 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3394 } 3395 // find requested i-frame 3396 tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq); 3397 if (tx_state){ 3398 log_info("Retransmission for tx_seq %u requested", req_seq); 3399 l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll; 3400 tx_state->retransmission_requested = 1; 3401 l2cap_channel->srej_active = 1; 3402 } 3403 break; 3404 default: 3405 break; 3406 } 3407 break; 3408 } else { 3409 // I-Frame 3410 // get control 3411 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 3412 uint8_t tx_seq = (control >> 1) & 0x3f; 3413 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 3414 log_info("SAR: pos %u", l2cap_channel->reassembly_pos); 3415 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 3416 l2cap_ertm_process_req_seq(l2cap_channel, req_seq); 3417 if (final){ 3418 // final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted 3419 l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel); 3420 } 3421 3422 // get SDU 3423 const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2]; 3424 uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size); 3425 3426 // assert SDU size is smaller or equal to our buffers 3427 uint16_t max_payload_size = 0; 3428 switch (sar){ 3429 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 3430 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 3431 // SDU Length + MPS 3432 max_payload_size = l2cap_channel->local_mps + 2; 3433 break; 3434 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 3435 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 3436 max_payload_size = l2cap_channel->local_mps; 3437 break; 3438 } 3439 if (payload_len > max_payload_size){ 3440 log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size); 3441 break; 3442 } 3443 3444 // check ordering 3445 if (l2cap_channel->expected_tx_seq == tx_seq){ 3446 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 3447 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3448 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3449 3450 // process SDU 3451 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len); 3452 3453 // process stored segments 3454 while (1){ 3455 int index = l2cap_channel->rx_store_index; 3456 l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index]; 3457 if (!rx_state->valid) break; 3458 3459 log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq); 3460 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 3461 l2cap_channel->req_seq = l2cap_channel->expected_tx_seq; 3462 3463 rx_state->valid = 0; 3464 l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len); 3465 3466 // update rx store index 3467 index++; 3468 if (index >= l2cap_channel->num_rx_buffers){ 3469 index = 0; 3470 } 3471 l2cap_channel->rx_store_index = index; 3472 } 3473 3474 // 3475 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 3476 3477 } else { 3478 int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f; 3479 if (delta < 2){ 3480 // store segment 3481 l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len); 3482 3483 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq); 3484 l2cap_channel->send_supervisor_frame_selective_reject = 1; 3485 } else { 3486 log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq); 3487 l2cap_channel->send_supervisor_frame_reject = 1; 3488 } 3489 } 3490 } 3491 break; 3492 } 3493 #endif 3494 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3495 } 3496 break; 3497 } 3498 #else 3499 UNUSED(handle); // ok: no code 3500 UNUSED(packet); // ok: no code 3501 UNUSED(size); // ok: no code 3502 #endif 3503 } 3504 3505 static void l2cap_acl_le_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){ 3506 #ifdef ENABLE_BLE 3507 3508 l2cap_fixed_channel_t * l2cap_fixed_channel; 3509 3510 #ifdef ENABLE_LE_DATA_CHANNELS 3511 l2cap_channel_t * l2cap_channel; 3512 #endif 3513 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 3514 switch (channel_id) { 3515 3516 case L2CAP_CID_SIGNALING_LE: { 3517 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 3518 uint16_t len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER + 2); 3519 if (COMPLETE_L2CAP_HEADER + 4 + len > size) break; 3520 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 3521 if (!valid){ 3522 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 3523 } 3524 break; 3525 } 3526 3527 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 3528 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_ATTRIBUTE_PROTOCOL); 3529 if (!l2cap_fixed_channel) break; 3530 if (!l2cap_fixed_channel->packet_handler) break; 3531 (*l2cap_fixed_channel->packet_handler)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3532 break; 3533 3534 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 3535 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_SECURITY_MANAGER_PROTOCOL); 3536 if (!l2cap_fixed_channel) break; 3537 if (!l2cap_fixed_channel->packet_handler) break; 3538 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 3539 break; 3540 3541 default: 3542 3543 #ifdef ENABLE_LE_DATA_CHANNELS 3544 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 3545 if (l2cap_channel) { 3546 // credit counting 3547 if (l2cap_channel->credits_incoming == 0){ 3548 log_error("LE Data Channel packet received but no incoming credits"); 3549 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3550 break; 3551 } 3552 l2cap_channel->credits_incoming--; 3553 3554 // automatic credits 3555 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 3556 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 3557 } 3558 3559 // first fragment 3560 uint16_t pos = 0; 3561 if (!l2cap_channel->receive_sdu_len){ 3562 uint16_t sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 3563 if(sdu_len > l2cap_channel->local_mtu) break; // SDU would be larger than our buffer 3564 l2cap_channel->receive_sdu_len = sdu_len; 3565 l2cap_channel->receive_sdu_pos = 0; 3566 pos += 2; 3567 size -= 2; 3568 } 3569 uint16_t fragment_size = size-COMPLETE_L2CAP_HEADER; 3570 uint16_t remaining_space = l2cap_channel->local_mtu - l2cap_channel->receive_sdu_pos; 3571 if (fragment_size > remaining_space) break; // SDU would cause buffer overrun 3572 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], fragment_size); 3573 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 3574 // done? 3575 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 3576 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 3577 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 3578 l2cap_channel->receive_sdu_len = 0; 3579 } 3580 } else { 3581 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 3582 } 3583 #endif 3584 break; 3585 } 3586 #else 3587 UNUSED(handle); // ok: no code 3588 UNUSED(packet); // ok: no code 3589 UNUSED(size); // ok: no code 3590 #endif 3591 } 3592 3593 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 3594 UNUSED(packet_type); // ok: registered with hci_register_acl_packet_handler 3595 UNUSED(channel); // ok: there is no channel 3596 3597 // Assert full L2CAP header present 3598 if (size < COMPLETE_L2CAP_HEADER) return; 3599 3600 // Dispatch to Classic or LE handler 3601 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 3602 hci_connection_t *conn = hci_connection_for_handle(handle); 3603 if (!conn) return; 3604 if (conn->address_type == BD_ADDR_TYPE_CLASSIC){ 3605 l2cap_acl_classic_handler(handle, packet, size); 3606 } else { 3607 l2cap_acl_le_handler(handle, packet, size); 3608 } 3609 3610 l2cap_run(); 3611 } 3612 3613 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 3614 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 3615 l2cap_fixed_channel_t * channel = l2cap_fixed_channel_for_channel_id(channel_id); 3616 if (!channel) return; 3617 channel->packet_handler = the_packet_handler; 3618 } 3619 3620 #ifdef ENABLE_CLASSIC 3621 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3622 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 3623 channel->state = L2CAP_STATE_CLOSED; 3624 l2cap_handle_channel_closed(channel); 3625 // discard channel 3626 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3627 l2cap_free_channel_entry(channel); 3628 } 3629 #endif 3630 3631 #ifdef L2CAP_USES_CHANNELS 3632 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 3633 btstack_linked_list_iterator_t it; 3634 btstack_linked_list_iterator_init(&it, services); 3635 while (btstack_linked_list_iterator_has_next(&it)){ 3636 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 3637 if ( service->psm == psm){ 3638 return service; 3639 }; 3640 } 3641 return NULL; 3642 } 3643 #endif 3644 3645 #ifdef ENABLE_CLASSIC 3646 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 3647 return l2cap_get_service_internal(&l2cap_services, psm); 3648 } 3649 3650 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 3651 3652 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 3653 3654 // check for alread registered psm 3655 l2cap_service_t *service = l2cap_get_service(psm); 3656 if (service) { 3657 log_error("l2cap_register_service: PSM %u already registered", psm); 3658 return L2CAP_SERVICE_ALREADY_REGISTERED; 3659 } 3660 3661 // alloc structure 3662 service = btstack_memory_l2cap_service_get(); 3663 if (!service) { 3664 log_error("l2cap_register_service: no memory for l2cap_service_t"); 3665 return BTSTACK_MEMORY_ALLOC_FAILED; 3666 } 3667 3668 // fill in 3669 service->psm = psm; 3670 service->mtu = mtu; 3671 service->packet_handler = service_packet_handler; 3672 service->required_security_level = security_level; 3673 3674 // add to services list 3675 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 3676 3677 // enable page scan 3678 gap_connectable_control(1); 3679 3680 return 0; 3681 } 3682 3683 uint8_t l2cap_unregister_service(uint16_t psm){ 3684 3685 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 3686 3687 l2cap_service_t *service = l2cap_get_service(psm); 3688 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3689 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 3690 btstack_memory_l2cap_service_free(service); 3691 3692 // disable page scan when no services registered 3693 if (btstack_linked_list_empty(&l2cap_services)) { 3694 gap_connectable_control(0); 3695 } 3696 return 0; 3697 } 3698 #endif 3699 3700 3701 #ifdef ENABLE_LE_DATA_CHANNELS 3702 3703 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 3704 if (!channel->waiting_for_can_send_now) return; 3705 if (channel->send_sdu_buffer) return; 3706 channel->waiting_for_can_send_now = 0; 3707 log_debug("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 3708 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 3709 } 3710 3711 // 1BH2222 3712 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 3713 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", 3714 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 3715 uint8_t event[19]; 3716 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 3717 event[1] = sizeof(event) - 2; 3718 event[2] = channel->address_type; 3719 reverse_bd_addr(channel->address, &event[3]); 3720 little_endian_store_16(event, 9, channel->con_handle); 3721 little_endian_store_16(event, 11, channel->psm); 3722 little_endian_store_16(event, 13, channel->local_cid); 3723 little_endian_store_16(event, 15, channel->remote_cid); 3724 little_endian_store_16(event, 17, channel->remote_mtu); 3725 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3726 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3727 } 3728 // 11BH22222 3729 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 3730 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", 3731 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 3732 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 3733 uint8_t event[23]; 3734 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 3735 event[1] = sizeof(event) - 2; 3736 event[2] = status; 3737 event[3] = channel->address_type; 3738 reverse_bd_addr(channel->address, &event[4]); 3739 little_endian_store_16(event, 10, channel->con_handle); 3740 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 3741 little_endian_store_16(event, 13, channel->psm); 3742 little_endian_store_16(event, 15, channel->local_cid); 3743 little_endian_store_16(event, 17, channel->remote_cid); 3744 little_endian_store_16(event, 19, channel->local_mtu); 3745 little_endian_store_16(event, 21, channel->remote_mtu); 3746 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3747 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3748 } 3749 // 2 3750 static void l2cap_emit_le_channel_closed(l2cap_channel_t * channel){ 3751 log_info("L2CAP_EVENT_LE_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 3752 uint8_t event[4]; 3753 event[0] = L2CAP_EVENT_LE_CHANNEL_CLOSED; 3754 event[1] = sizeof(event) - 2; 3755 little_endian_store_16(event, 2, channel->local_cid); 3756 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 3757 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 3758 } 3759 3760 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 3761 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 3762 channel->state = L2CAP_STATE_CLOSED; 3763 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 3764 // discard channel 3765 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 3766 l2cap_free_channel_entry(channel); 3767 } 3768 3769 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 3770 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 3771 } 3772 3773 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 3774 3775 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 3776 3777 // check for alread registered psm 3778 l2cap_service_t *service = l2cap_le_get_service(psm); 3779 if (service) { 3780 return L2CAP_SERVICE_ALREADY_REGISTERED; 3781 } 3782 3783 // alloc structure 3784 service = btstack_memory_l2cap_service_get(); 3785 if (!service) { 3786 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 3787 return BTSTACK_MEMORY_ALLOC_FAILED; 3788 } 3789 3790 // fill in 3791 service->psm = psm; 3792 service->mtu = 0; 3793 service->packet_handler = packet_handler; 3794 service->required_security_level = security_level; 3795 3796 // add to services list 3797 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 3798 3799 // done 3800 return 0; 3801 } 3802 3803 uint8_t l2cap_le_unregister_service(uint16_t psm) { 3804 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 3805 l2cap_service_t *service = l2cap_le_get_service(psm); 3806 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 3807 3808 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 3809 btstack_memory_l2cap_service_free(service); 3810 return 0; 3811 } 3812 3813 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 3814 // get channel 3815 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3816 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3817 3818 // validate state 3819 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3820 return ERROR_CODE_COMMAND_DISALLOWED; 3821 } 3822 3823 // set state accept connection 3824 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 3825 channel->receive_sdu_buffer = receive_sdu_buffer; 3826 channel->local_mtu = mtu; 3827 channel->new_credits_incoming = initial_credits; 3828 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3829 3830 // test 3831 // channel->new_credits_incoming = 1; 3832 3833 // go 3834 l2cap_run(); 3835 return 0; 3836 } 3837 3838 /** 3839 * @brief Deny incoming LE Data Channel connection due to resource constraints 3840 * @param local_cid L2CAP LE Data Channel Identifier 3841 */ 3842 3843 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 3844 // get channel 3845 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3846 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3847 3848 // validate state 3849 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 3850 return ERROR_CODE_COMMAND_DISALLOWED; 3851 } 3852 3853 // set state decline connection 3854 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 3855 channel->reason = 0x04; // no resources available 3856 l2cap_run(); 3857 return 0; 3858 } 3859 3860 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 3861 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 3862 uint16_t * out_local_cid) { 3863 3864 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 3865 3866 3867 hci_connection_t * connection = hci_connection_for_handle(con_handle); 3868 if (!connection) { 3869 log_error("no hci_connection for handle 0x%04x", con_handle); 3870 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 3871 } 3872 3873 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); 3874 if (!channel) { 3875 return BTSTACK_MEMORY_ALLOC_FAILED; 3876 } 3877 log_info("l2cap_le_create_channel %p", channel); 3878 3879 // store local_cid 3880 if (out_local_cid){ 3881 *out_local_cid = channel->local_cid; 3882 } 3883 3884 // provide buffer 3885 channel->con_handle = con_handle; 3886 channel->receive_sdu_buffer = receive_sdu_buffer; 3887 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 3888 channel->new_credits_incoming = initial_credits; 3889 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 3890 3891 // add to connections list 3892 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 3893 3894 // go 3895 l2cap_run(); 3896 return 0; 3897 } 3898 3899 /** 3900 * @brief Provide credtis for LE Data Channel 3901 * @param local_cid L2CAP LE Data Channel Identifier 3902 * @param credits Number additional credits for peer 3903 */ 3904 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 3905 3906 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3907 if (!channel) { 3908 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3909 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3910 } 3911 3912 // check state 3913 if (channel->state != L2CAP_STATE_OPEN){ 3914 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 3915 } 3916 3917 // assert incoming credits + credits <= 0xffff 3918 uint32_t total_credits = channel->credits_incoming; 3919 total_credits += channel->new_credits_incoming; 3920 total_credits += credits; 3921 if (total_credits > 0xffff){ 3922 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 3923 channel->new_credits_incoming, credits); 3924 } 3925 3926 // set credits_granted 3927 channel->new_credits_incoming += credits; 3928 3929 // go 3930 l2cap_run(); 3931 return 0; 3932 } 3933 3934 /** 3935 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 3936 * @param local_cid L2CAP LE Data Channel Identifier 3937 */ 3938 int l2cap_le_can_send_now(uint16_t local_cid){ 3939 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3940 if (!channel) { 3941 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 3942 return 0; 3943 } 3944 3945 // check state 3946 if (channel->state != L2CAP_STATE_OPEN) return 0; 3947 3948 // check queue 3949 if (channel->send_sdu_buffer) return 0; 3950 3951 // fine, go ahead 3952 return 1; 3953 } 3954 3955 /** 3956 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 3957 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 3958 * so packet handler should be ready to handle it 3959 * @param local_cid L2CAP LE Data Channel Identifier 3960 */ 3961 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 3962 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3963 if (!channel) { 3964 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 3965 return 0; 3966 } 3967 channel->waiting_for_can_send_now = 1; 3968 l2cap_le_notify_channel_can_send(channel); 3969 return 0; 3970 } 3971 3972 /** 3973 * @brief Send data via LE Data Channel 3974 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 3975 * @param local_cid L2CAP LE Data Channel Identifier 3976 * @param data data to send 3977 * @param size data size 3978 */ 3979 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 3980 3981 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 3982 if (!channel) { 3983 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3984 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3985 } 3986 3987 if (len > channel->remote_mtu){ 3988 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 3989 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 3990 } 3991 3992 if (channel->send_sdu_buffer){ 3993 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 3994 return BTSTACK_ACL_BUFFERS_FULL; 3995 } 3996 3997 channel->send_sdu_buffer = data; 3998 channel->send_sdu_len = len; 3999 channel->send_sdu_pos = 0; 4000 4001 l2cap_run(); 4002 return 0; 4003 } 4004 4005 /** 4006 * @brief Disconnect from LE Data Channel 4007 * @param local_cid L2CAP LE Data Channel Identifier 4008 */ 4009 uint8_t l2cap_le_disconnect(uint16_t local_cid) 4010 { 4011 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 4012 if (!channel) { 4013 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 4014 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 4015 } 4016 4017 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 4018 l2cap_run(); 4019 return 0; 4020 } 4021 4022 #endif 4023