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