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