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