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