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