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