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