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