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