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