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