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