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