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