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