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