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 #if 0 558 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){ 559 return (((uint16_t) sar) << 14) | (req_seq << 8) | (final << 7) | 1; 560 } 561 #endif 562 static int l2cap_next_ertm_seq_nr(int seq_nr){ 563 return (seq_nr + 1) & 0x3f; 564 } 565 #endif 566 567 // assumption - only on Classic connections 568 int l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){ 569 570 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 571 if (!channel) { 572 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 573 return -1; // TODO: define error 574 } 575 576 if (len > channel->remote_mtu){ 577 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 578 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 579 } 580 581 if (!hci_can_send_acl_packet_now(channel->con_handle)){ 582 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 583 return BTSTACK_ACL_BUFFERS_FULL; 584 } 585 586 hci_reserve_packet_buffer(); 587 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 588 589 int control_size = 0; 590 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 591 // hack to send i-frames 592 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 593 uint16_t control = l2cap_encanced_control_field_for_information_frame(channel->next_tx_seq, 0, 0, L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU); 594 log_info("I-Frame: control 0x%04x", control); 595 little_endian_store_16(acl_buffer, 8, control); 596 channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq); 597 control_size = 2; 598 } 599 #endif 600 601 memcpy(&acl_buffer[8+control_size], data, len); 602 603 return l2cap_send_prepared(local_cid, len+control_size); 604 } 605 606 int l2cap_send_echo_request(hci_con_handle_t con_handle, uint8_t *data, uint16_t len){ 607 return l2cap_send_signaling_packet(con_handle, ECHO_REQUEST, 0x77, len, data); 608 } 609 610 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 611 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var | flag); 612 } 613 614 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 615 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var & ~flag); 616 } 617 #endif 618 619 620 #ifdef ENABLE_BLE 621 static int l2cap_send_le_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 622 623 if (!hci_can_send_acl_packet_now(handle)){ 624 log_info("l2cap_send_le_signaling_packet, cannot send"); 625 return BTSTACK_ACL_BUFFERS_FULL; 626 } 627 628 // log_info("l2cap_send_le_signaling_packet type %u", cmd); 629 hci_reserve_packet_buffer(); 630 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 631 va_list argptr; 632 va_start(argptr, identifier); 633 uint16_t len = l2cap_create_signaling_le(acl_buffer, handle, cmd, identifier, argptr); 634 va_end(argptr); 635 // log_info("l2cap_send_le_signaling_packet con %u!", handle); 636 return hci_send_acl_packet_buffer(len); 637 } 638 #endif 639 640 uint16_t l2cap_max_mtu(void){ 641 return HCI_ACL_PAYLOAD_SIZE - L2CAP_HEADER_SIZE; 642 } 643 644 uint16_t l2cap_max_le_mtu(void){ 645 return l2cap_max_mtu(); 646 } 647 648 static uint16_t l2cap_setup_options_mtu(l2cap_channel_t * channel, uint8_t * config_options){ 649 config_options[0] = 1; // MTU 650 config_options[1] = 2; // len param 651 little_endian_store_16( (uint8_t*)&config_options, 2, channel->local_mtu); 652 return 4; 653 } 654 655 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 656 static uint16_t l2cap_setup_options_ertm(l2cap_channel_t * channel, uint8_t * config_options){ 657 config_options[0] = 0x04; // RETRANSMISSION AND FLOW CONTROL OPTION 658 config_options[1] = 9; // length 659 config_options[2] = (uint8_t) channel->mode; 660 config_options[3] = channel->num_rx_buffers; // == TxWindows size 661 config_options[4] = channel->local_max_transmit; 662 little_endian_store_16( config_options, 5, channel->local_retransmission_timeout_ms); 663 little_endian_store_16( config_options, 7, channel->local_monitor_timeout_ms); 664 little_endian_store_16( config_options, 9, channel->local_mtu); 665 return 11; 666 } 667 #endif 668 669 static uint16_t l2cap_setup_options(l2cap_channel_t * channel, uint8_t * config_options){ 670 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 671 // use ERTM options if supported 672 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 673 if ((connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_DONE) && (connection->l2cap_state.extended_feature_mask & 0x08)){ 674 return l2cap_setup_options_ertm(channel, config_options); 675 676 } 677 #endif 678 return l2cap_setup_options_mtu(channel, config_options); 679 } 680 681 static uint32_t l2cap_extended_features_mask(void){ 682 // extended features request supported, features: fixed channels, unicast connectionless data reception 683 uint32_t features = 0x280; 684 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 685 features |= 0x0008; 686 #endif 687 return features; 688 } 689 690 // MARK: L2CAP_RUN 691 // process outstanding signaling tasks 692 static void l2cap_run(void){ 693 694 // log_info("l2cap_run: entered"); 695 696 // check pending signaling responses 697 while (signaling_responses_pending){ 698 699 hci_con_handle_t handle = signaling_responses[0].handle; 700 701 if (!hci_can_send_acl_packet_now(handle)) break; 702 703 uint8_t sig_id = signaling_responses[0].sig_id; 704 uint8_t response_code = signaling_responses[0].code; 705 uint16_t infoType = signaling_responses[0].data; // INFORMATION_REQUEST 706 uint16_t result = signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT 707 #ifdef ENABLE_CLASSIC 708 uint16_t source_cid = signaling_responses[0].cid; // CONNECTION_REQUEST 709 #endif 710 UNUSED(infoType); 711 712 // remove first item before sending (to avoid sending response mutliple times) 713 signaling_responses_pending--; 714 int i; 715 for (i=0; i < signaling_responses_pending; i++){ 716 memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t)); 717 } 718 719 switch (response_code){ 720 #ifdef ENABLE_CLASSIC 721 case CONNECTION_REQUEST: 722 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 723 // also disconnect if result is 0x0003 - security blocked 724 if (result == 0x0003){ 725 hci_disconnect_security_block(handle); 726 } 727 break; 728 case ECHO_REQUEST: 729 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 730 break; 731 case INFORMATION_REQUEST: 732 switch (infoType){ 733 case 1: { // Connectionless MTU 734 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 735 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(connectionless_mtu), &connectionless_mtu); 736 } 737 break; 738 case 2: { // Extended Features Supported 739 uint32_t features = l2cap_extended_features_mask(); 740 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(features), &features); 741 } 742 break; 743 case 3: { // Fixed Channels Supported 744 uint8_t map[8]; 745 memset(map, 0, 8); 746 map[0] = 0x06; // L2CAP Signaling Channel (0x02) + Connectionless reception (0x04) 747 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(map), &map); 748 } 749 break; 750 default: 751 // all other types are not supported 752 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL); 753 break; 754 } 755 break; 756 case COMMAND_REJECT: 757 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 758 break; 759 #endif 760 #ifdef ENABLE_BLE 761 case LE_CREDIT_BASED_CONNECTION_REQUEST: 762 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 763 break; 764 case COMMAND_REJECT_LE: 765 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 766 break; 767 #endif 768 default: 769 // should not happen 770 break; 771 } 772 } 773 774 btstack_linked_list_iterator_t it; 775 UNUSED(it); 776 777 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 778 // send l2cap information request if neccessary 779 hci_connections_get_iterator(&it); 780 while(btstack_linked_list_iterator_has_next(&it)){ 781 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 782 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST){ 783 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W4_EXTENDED_FEATURE_RESPONSE; 784 // send information request for extended features 785 uint8_t sig_id = l2cap_next_sig_id(); 786 uint8_t info_type = 2; 787 l2cap_send_signaling_packet(connection->con_handle, INFORMATION_REQUEST, sig_id, info_type); 788 return; 789 } 790 } 791 #endif 792 793 #ifdef ENABLE_CLASSIC 794 uint8_t config_options[10]; 795 btstack_linked_list_iterator_init(&it, &l2cap_channels); 796 while (btstack_linked_list_iterator_has_next(&it)){ 797 798 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 799 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 800 switch (channel->state){ 801 802 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 803 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 804 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 805 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 806 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 807 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0); 808 } 809 break; 810 811 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 812 if (!hci_can_send_command_packet_now()) break; 813 // send connection request - set state first 814 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 815 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 816 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1); 817 break; 818 819 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 820 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 821 channel->state = L2CAP_STATE_INVALID; 822 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0); 823 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 824 l2cap_stop_rtx(channel); 825 btstack_linked_list_iterator_remove(&it); 826 btstack_memory_l2cap_channel_free(channel); 827 break; 828 829 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 830 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 831 channel->state = L2CAP_STATE_CONFIG; 832 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 833 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 834 break; 835 836 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 837 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 838 // success, start l2cap handshake 839 channel->local_sig_id = l2cap_next_sig_id(); 840 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 841 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 842 l2cap_start_rtx(channel); 843 break; 844 845 case L2CAP_STATE_CONFIG: 846 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 847 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 848 uint16_t flags = 0; 849 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 850 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 851 flags = 1; 852 } else { 853 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 854 } 855 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 856 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 857 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL); 858 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 859 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED){ 860 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 861 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 862 uint16_t options_size = l2cap_setup_options(channel, config_options); 863 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); 864 #endif 865 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 866 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 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_SUCCESS, options_size, &config_options); 869 } else { 870 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_SUCCESS, 0, NULL); 871 } 872 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 873 } 874 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 875 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 876 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 877 channel->local_sig_id = l2cap_next_sig_id(); 878 uint16_t options_size = l2cap_setup_options(channel, config_options); 879 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, options_size, &config_options); 880 l2cap_start_rtx(channel); 881 } 882 if (l2cap_channel_ready_for_open(channel)){ 883 channel->state = L2CAP_STATE_OPEN; 884 l2cap_emit_channel_opened(channel, 0); // success 885 } 886 break; 887 888 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 889 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 890 channel->state = L2CAP_STATE_INVALID; 891 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 892 // 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 :) 893 l2cap_finialize_channel_close(channel); // -- remove from list 894 break; 895 896 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 897 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 898 channel->local_sig_id = l2cap_next_sig_id(); 899 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 900 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 901 break; 902 default: 903 break; 904 } 905 } 906 #endif 907 908 #ifdef ENABLE_LE_DATA_CHANNELS 909 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 910 while (btstack_linked_list_iterator_has_next(&it)){ 911 uint8_t * acl_buffer; 912 uint8_t * l2cap_payload; 913 uint16_t pos; 914 uint16_t payload_size; 915 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 916 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 917 switch (channel->state){ 918 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 919 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 920 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 921 // le psm, source cid, mtu, mps, initial credits 922 channel->local_sig_id = l2cap_next_sig_id(); 923 channel->credits_incoming = channel->new_credits_incoming; 924 channel->new_credits_incoming = 0; 925 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); 926 break; 927 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 928 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 929 // TODO: support larger MPS 930 channel->state = L2CAP_STATE_OPEN; 931 channel->credits_incoming = channel->new_credits_incoming; 932 channel->new_credits_incoming = 0; 933 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); 934 // notify client 935 l2cap_emit_le_channel_opened(channel, 0); 936 break; 937 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 938 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 939 channel->state = L2CAP_STATE_INVALID; 940 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 941 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 942 l2cap_stop_rtx(channel); 943 btstack_linked_list_iterator_remove(&it); 944 btstack_memory_l2cap_channel_free(channel); 945 break; 946 case L2CAP_STATE_OPEN: 947 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 948 949 // send credits 950 if (channel->new_credits_incoming){ 951 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 952 channel->local_sig_id = l2cap_next_sig_id(); 953 uint16_t new_credits = channel->new_credits_incoming; 954 channel->new_credits_incoming = 0; 955 channel->credits_incoming += new_credits; 956 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 957 break; 958 } 959 960 // send data 961 if (!channel->send_sdu_buffer) break; 962 if (!channel->credits_outgoing) break; 963 964 // send part of SDU 965 hci_reserve_packet_buffer(); 966 acl_buffer = hci_get_outgoing_packet_buffer(); 967 l2cap_payload = acl_buffer + 8; 968 pos = 0; 969 if (!channel->send_sdu_pos){ 970 // store SDU len 971 channel->send_sdu_pos += 2; 972 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 973 pos += 2; 974 } 975 payload_size = btstack_min(channel->send_sdu_len + 2 - channel->send_sdu_pos, channel->remote_mps - pos); 976 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 977 memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len 978 pos += payload_size; 979 channel->send_sdu_pos += payload_size; 980 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 981 // done 982 983 channel->credits_outgoing--; 984 985 if (channel->send_sdu_pos >= channel->send_sdu_len + 2){ 986 channel->send_sdu_buffer = NULL; 987 // send done event 988 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 989 // inform about can send now 990 l2cap_le_notify_channel_can_send(channel); 991 } 992 hci_send_acl_packet_buffer(8 + pos); 993 break; 994 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 995 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 996 channel->local_sig_id = l2cap_next_sig_id(); 997 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 998 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 999 break; 1000 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1001 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1002 channel->state = L2CAP_STATE_INVALID; 1003 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1004 l2cap_le_finialize_channel_close(channel); // -- remove from list 1005 break; 1006 default: 1007 break; 1008 } 1009 } 1010 #endif 1011 1012 #ifdef ENABLE_BLE 1013 // send l2cap con paramter update if necessary 1014 hci_connections_get_iterator(&it); 1015 while(btstack_linked_list_iterator_has_next(&it)){ 1016 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1017 if (connection->address_type != BD_ADDR_TYPE_LE_PUBLIC && connection->address_type != BD_ADDR_TYPE_LE_RANDOM) continue; 1018 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1019 switch (connection->le_con_parameter_update_state){ 1020 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1021 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1022 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, connection->le_con_param_update_identifier, 1023 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1024 break; 1025 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1026 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1027 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1028 break; 1029 case CON_PARAMETER_UPDATE_DENY: 1030 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1031 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1032 break; 1033 default: 1034 break; 1035 } 1036 } 1037 #endif 1038 1039 // log_info("l2cap_run: exit"); 1040 } 1041 1042 #ifdef ENABLE_CLASSIC 1043 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1044 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 1045 log_info("l2cap_handle_connection_complete expected state"); 1046 // success, start l2cap handshake 1047 channel->con_handle = con_handle; 1048 // check remote SSP feature first 1049 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1050 } 1051 } 1052 1053 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1054 1055 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1056 // assumption: outgoing connection 1057 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1058 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1059 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1060 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1061 return; 1062 } 1063 #endif 1064 1065 // fine, go ahead 1066 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1067 } 1068 1069 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1070 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1071 1072 // we have been waiting for remote supported features, if both support SSP, 1073 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)); 1074 if (gap_ssp_supported_on_both_sides(channel->con_handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){ 1075 // request security level 2 1076 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 1077 gap_request_security_level(channel->con_handle, LEVEL_2); 1078 return; 1079 } 1080 1081 l2cap_ready_to_connect(channel); 1082 } 1083 #endif 1084 1085 #ifdef L2CAP_USES_CHANNELS 1086 static l2cap_channel_t * l2cap_create_channel_entry(btstack_packet_handler_t packet_handler, bd_addr_t address, bd_addr_type_t address_type, 1087 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 1088 1089 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 1090 if (!channel) { 1091 return NULL; 1092 } 1093 1094 // Init memory (make valgrind happy) 1095 memset(channel, 0, sizeof(l2cap_channel_t)); 1096 1097 // fill in 1098 channel->packet_handler = packet_handler; 1099 bd_addr_copy(channel->address, address); 1100 channel->address_type = address_type; 1101 channel->psm = psm; 1102 channel->local_mtu = local_mtu; 1103 channel->remote_mtu = L2CAP_MINIMAL_MTU; 1104 channel->required_security_level = security_level; 1105 1106 // 1107 channel->local_cid = l2cap_next_local_cid(); 1108 channel->con_handle = 0; 1109 1110 // set initial state 1111 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 1112 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 1113 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 1114 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 1115 return channel; 1116 } 1117 #endif 1118 1119 #ifdef ENABLE_CLASSIC 1120 1121 /** 1122 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 1123 * @param packet_handler 1124 * @param address 1125 * @param psm 1126 * @param mtu 1127 * @param local_cid 1128 */ 1129 1130 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){ 1131 // limit MTU to the size of our outtgoing HCI buffer 1132 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 1133 1134 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x mtu %u -> local mtu %u", bd_addr_to_str(address), psm, mtu, local_mtu); 1135 1136 l2cap_channel_t * channel = l2cap_create_channel_entry(channel_packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1137 if (!channel) { 1138 return BTSTACK_MEMORY_ALLOC_FAILED; 1139 } 1140 1141 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1142 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1143 #endif 1144 1145 // add to connections list 1146 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1147 1148 // store local_cid 1149 if (out_local_cid){ 1150 *out_local_cid = channel->local_cid; 1151 } 1152 1153 // check if hci connection is already usable 1154 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1155 if (conn){ 1156 log_info("l2cap_create_channel, hci connection already exists"); 1157 l2cap_handle_connection_complete(conn->con_handle, channel); 1158 // check if remote supported fearures are already received 1159 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1160 l2cap_handle_remote_supported_features_received(channel); 1161 } 1162 } 1163 1164 l2cap_run(); 1165 1166 return 0; 1167 } 1168 1169 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1170 1171 static uint8_t l2cap_ertm_validate_local_config(uint8_t max_transmit, uint16_t retransmission_timeout_ms, 1172 uint16_t monitor_timeout_ms, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){ 1173 UNUSED(buffer); 1174 UNUSED(size); 1175 1176 uint8_t result = ERROR_CODE_SUCCESS; 1177 if (max_transmit < 1){ 1178 log_error("max_transmit must be >= 1"); 1179 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1180 } 1181 if (retransmission_timeout_ms < 2000){ 1182 log_error("retransmission_timeout_ms must be >= 2000 ms"); 1183 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1184 } 1185 if (monitor_timeout_ms < 12000){ 1186 log_error("monitor_timeout_ms must be >= 12000 ms"); 1187 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1188 } 1189 if (num_rx_buffers < 1){ 1190 log_error("num_rx_buffers must be >= 1"); 1191 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1192 } 1193 if (num_tx_buffers < 1){ 1194 log_error("num_rx_buffers must be >= 1"); 1195 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1196 } 1197 return result; 1198 } 1199 1200 static void l2cap_ertm_configure_channel(l2cap_channel_t * channel, int ertm_mandatory, uint8_t max_transmit, 1201 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){ 1202 1203 channel->mode = L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION; 1204 channel->ertm_mandatory = ertm_mandatory; 1205 channel->local_max_transmit = max_transmit; 1206 channel->local_retransmission_timeout_ms = retransmission_timeout_ms; 1207 channel->local_monitor_timeout_ms = monitor_timeout_ms; 1208 channel->num_rx_buffers = num_rx_buffers; 1209 channel->num_tx_buffers = num_tx_buffers; 1210 1211 // TODO: align buffer pointer 1212 uint32_t pos = 0; 1213 channel->rx_packets_state = (l2cap_ertm_rx_packet_state_t *) &buffer[pos]; 1214 pos += num_rx_buffers * sizeof(l2cap_ertm_rx_packet_state_t); 1215 channel->tx_packets_state = (l2cap_ertm_tx_packet_state_t *) &buffer[pos]; 1216 pos += num_tx_buffers * sizeof(l2cap_ertm_tx_packet_state_t); 1217 // calculate MTU 1218 channel->local_mtu = (size - pos) / (num_rx_buffers + num_tx_buffers); 1219 log_info("Local ERTM MTU: %u", channel->local_mtu); 1220 channel->rx_packets_data = &buffer[pos]; 1221 pos += num_rx_buffers * channel->local_mtu; 1222 channel->tx_packets_data = &buffer[pos]; 1223 log_info("RX packets %p, TX packets %p", channel->rx_packets_data, channel->tx_packets_data); 1224 } 1225 1226 uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, 1227 int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, 1228 uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size, uint16_t * out_local_cid){ 1229 1230 // limit MTU to the size of our outtgoing HCI buffer 1231 uint16_t local_mtu = l2cap_max_mtu(); 1232 1233 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x -> local mtu %u", bd_addr_to_str(address), psm, local_mtu); 1234 1235 // validate local config 1236 uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1237 if (result) return result; 1238 1239 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1240 if (!channel) { 1241 return BTSTACK_MEMORY_ALLOC_FAILED; 1242 } 1243 1244 // configure ERTM 1245 l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, 1246 monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1247 1248 // add to connections list 1249 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1250 1251 // store local_cid 1252 if (out_local_cid){ 1253 *out_local_cid = channel->local_cid; 1254 } 1255 1256 // check if hci connection is already usable 1257 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1258 if (conn){ 1259 log_info("l2cap_create_channel, hci connection already exists"); 1260 l2cap_handle_connection_complete(conn->con_handle, channel); 1261 // check if remote supported fearures are already received 1262 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1263 l2cap_handle_remote_supported_features_received(channel); 1264 } 1265 } 1266 1267 l2cap_run(); 1268 1269 return 0; 1270 } 1271 #endif 1272 1273 void 1274 l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 1275 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 1276 // find channel for local_cid 1277 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1278 if (channel) { 1279 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1280 } 1281 // process 1282 l2cap_run(); 1283 } 1284 1285 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 1286 btstack_linked_list_iterator_t it; 1287 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1288 while (btstack_linked_list_iterator_has_next(&it)){ 1289 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1290 if ( bd_addr_cmp( channel->address, address) != 0) continue; 1291 // channel for this address found 1292 switch (channel->state){ 1293 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 1294 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1295 // failure, forward error code 1296 l2cap_emit_channel_opened(channel, status); 1297 // discard channel 1298 l2cap_stop_rtx(channel); 1299 btstack_linked_list_iterator_remove(&it); 1300 btstack_memory_l2cap_channel_free(channel); 1301 break; 1302 default: 1303 break; 1304 } 1305 } 1306 } 1307 1308 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 1309 btstack_linked_list_iterator_t it; 1310 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1311 while (btstack_linked_list_iterator_has_next(&it)){ 1312 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1313 if ( ! bd_addr_cmp( channel->address, address) ){ 1314 l2cap_handle_connection_complete(handle, channel); 1315 } 1316 } 1317 // process 1318 l2cap_run(); 1319 } 1320 #endif 1321 1322 static void l2cap_notify_channel_can_send(void){ 1323 1324 #ifdef ENABLE_CLASSIC 1325 btstack_linked_list_iterator_t it; 1326 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1327 while (btstack_linked_list_iterator_has_next(&it)){ 1328 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1329 if (!channel->waiting_for_can_send_now) continue; 1330 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1331 channel->waiting_for_can_send_now = 0; 1332 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 1333 } 1334 #endif 1335 1336 int i; 1337 for (i=0;i<L2CAP_FIXED_CHANNEL_TABLE_SIZE;i++){ 1338 if (!fixed_channels[i].callback) continue; 1339 if (!fixed_channels[i].waiting_for_can_send_now) continue; 1340 int can_send = 0; 1341 if (l2cap_fixed_channel_table_index_is_le(i)){ 1342 #ifdef ENABLE_BLE 1343 can_send = hci_can_send_acl_le_packet_now(); 1344 #endif 1345 } else { 1346 #ifdef ENABLE_CLASSIC 1347 can_send = hci_can_send_acl_classic_packet_now(); 1348 #endif 1349 } 1350 if (!can_send) continue; 1351 fixed_channels[i].waiting_for_can_send_now = 0; 1352 l2cap_emit_can_send_now(fixed_channels[i].callback, l2cap_fixed_channel_table_channel_id_for_index(i)); 1353 } 1354 } 1355 1356 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 1357 1358 UNUSED(packet_type); 1359 UNUSED(cid); 1360 UNUSED(size); 1361 1362 bd_addr_t address; 1363 hci_con_handle_t handle; 1364 int hci_con_used; 1365 btstack_linked_list_iterator_t it; 1366 1367 // avoid unused warnings 1368 UNUSED(address); 1369 UNUSED(hci_con_used); 1370 UNUSED(it); 1371 UNUSED(handle); 1372 1373 switch(hci_event_packet_get_type(packet)){ 1374 1375 // Notify channel packet handler if they can send now 1376 case HCI_EVENT_TRANSPORT_PACKET_SENT: 1377 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 1378 l2cap_run(); // try sending signaling packets first 1379 l2cap_notify_channel_can_send(); 1380 break; 1381 1382 case HCI_EVENT_COMMAND_STATUS: 1383 l2cap_run(); // try sending signaling packets first 1384 break; 1385 1386 #ifdef ENABLE_CLASSIC 1387 // handle connection complete events 1388 case HCI_EVENT_CONNECTION_COMPLETE: 1389 reverse_bd_addr(&packet[5], address); 1390 if (packet[2] == 0){ 1391 handle = little_endian_read_16(packet, 3); 1392 l2cap_handle_connection_success_for_addr(address, handle); 1393 } else { 1394 l2cap_handle_connection_failed_for_addr(address, packet[2]); 1395 } 1396 break; 1397 1398 // handle successful create connection cancel command 1399 case HCI_EVENT_COMMAND_COMPLETE: 1400 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 1401 if (packet[5] == 0){ 1402 reverse_bd_addr(&packet[6], address); 1403 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 1404 l2cap_handle_connection_failed_for_addr(address, 0x16); 1405 } 1406 } 1407 l2cap_run(); // try sending signaling packets first 1408 break; 1409 #endif 1410 1411 // handle disconnection complete events 1412 case HCI_EVENT_DISCONNECTION_COMPLETE: 1413 // send l2cap disconnect events for all channels on this handle and free them 1414 #ifdef ENABLE_CLASSIC 1415 handle = little_endian_read_16(packet, 3); 1416 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1417 while (btstack_linked_list_iterator_has_next(&it)){ 1418 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1419 if (channel->con_handle != handle) continue; 1420 l2cap_emit_channel_closed(channel); 1421 l2cap_stop_rtx(channel); 1422 btstack_linked_list_iterator_remove(&it); 1423 btstack_memory_l2cap_channel_free(channel); 1424 } 1425 #endif 1426 #ifdef ENABLE_LE_DATA_CHANNELS 1427 handle = little_endian_read_16(packet, 3); 1428 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1429 while (btstack_linked_list_iterator_has_next(&it)){ 1430 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1431 if (channel->con_handle != handle) continue; 1432 l2cap_emit_channel_closed(channel); 1433 btstack_linked_list_iterator_remove(&it); 1434 btstack_memory_l2cap_channel_free(channel); 1435 } 1436 #endif 1437 break; 1438 1439 // HCI Connection Timeouts 1440 #ifdef ENABLE_CLASSIC 1441 case L2CAP_EVENT_TIMEOUT_CHECK: 1442 handle = little_endian_read_16(packet, 2); 1443 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 1444 if (hci_authentication_active_for_handle(handle)) break; 1445 hci_con_used = 0; 1446 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1447 while (btstack_linked_list_iterator_has_next(&it)){ 1448 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1449 if (channel->con_handle != handle) continue; 1450 hci_con_used = 1; 1451 break; 1452 } 1453 if (hci_con_used) break; 1454 if (!hci_can_send_command_packet_now()) break; 1455 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 1456 break; 1457 1458 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 1459 handle = little_endian_read_16(packet, 3); 1460 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1461 while (btstack_linked_list_iterator_has_next(&it)){ 1462 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1463 if (channel->con_handle != handle) continue; 1464 l2cap_handle_remote_supported_features_received(channel); 1465 break; 1466 } 1467 break; 1468 1469 case GAP_EVENT_SECURITY_LEVEL: 1470 handle = little_endian_read_16(packet, 2); 1471 log_info("l2cap - security level update"); 1472 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1473 while (btstack_linked_list_iterator_has_next(&it)){ 1474 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1475 if (channel->con_handle != handle) continue; 1476 1477 log_info("l2cap - state %u", channel->state); 1478 1479 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 1480 gap_security_level_t required_level = channel->required_security_level; 1481 1482 switch (channel->state){ 1483 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1484 if (actual_level >= required_level){ 1485 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1486 // we need to know if ERTM is supported before sending a config response 1487 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1488 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1489 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 1490 #else 1491 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 1492 l2cap_emit_incoming_connection(channel); 1493 #endif 1494 } else { 1495 channel->reason = 0x0003; // security block 1496 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1497 } 1498 break; 1499 1500 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 1501 if (actual_level >= required_level){ 1502 l2cap_ready_to_connect(channel); 1503 } else { 1504 // disconnnect, authentication not good enough 1505 hci_disconnect_security_block(handle); 1506 } 1507 break; 1508 1509 default: 1510 break; 1511 } 1512 } 1513 break; 1514 #endif 1515 1516 default: 1517 break; 1518 } 1519 1520 l2cap_run(); 1521 } 1522 1523 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 1524 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 1525 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 1526 signaling_responses[signaling_responses_pending].handle = handle; 1527 signaling_responses[signaling_responses_pending].code = code; 1528 signaling_responses[signaling_responses_pending].sig_id = sig_id; 1529 signaling_responses[signaling_responses_pending].cid = cid; 1530 signaling_responses[signaling_responses_pending].data = data; 1531 signaling_responses_pending++; 1532 l2cap_run(); 1533 } 1534 } 1535 1536 #ifdef ENABLE_CLASSIC 1537 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 1538 channel->remote_sig_id = identifier; 1539 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 1540 l2cap_run(); 1541 } 1542 1543 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 1544 1545 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 1546 l2cap_service_t *service = l2cap_get_service(psm); 1547 if (!service) { 1548 // 0x0002 PSM not supported 1549 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 1550 return; 1551 } 1552 1553 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 1554 if (!hci_connection) { 1555 // 1556 log_error("no hci_connection for handle %u", handle); 1557 return; 1558 } 1559 1560 // alloc structure 1561 // log_info("l2cap_handle_connection_request register channel"); 1562 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, hci_connection->address, BD_ADDR_TYPE_CLASSIC, 1563 psm, service->mtu, service->required_security_level); 1564 if (!channel){ 1565 // 0x0004 No resources available 1566 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 1567 return; 1568 } 1569 1570 channel->con_handle = handle; 1571 channel->remote_cid = source_cid; 1572 channel->remote_sig_id = sig_id; 1573 1574 // limit local mtu to max acl packet length - l2cap header 1575 if (channel->local_mtu > l2cap_max_mtu()) { 1576 channel->local_mtu = l2cap_max_mtu(); 1577 } 1578 1579 // set initial state 1580 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 1581 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 1582 1583 // add to connections list 1584 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1585 1586 // assert security requirements 1587 gap_request_security_level(handle, channel->required_security_level); 1588 } 1589 1590 void l2cap_accept_connection(uint16_t local_cid){ 1591 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 1592 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1593 if (!channel) { 1594 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1595 return; 1596 } 1597 1598 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1599 // configure L2CAP Basic mode 1600 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1601 #endif 1602 1603 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1604 1605 // process 1606 l2cap_run(); 1607 } 1608 1609 1610 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1611 uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, int ertm_mandatory, uint8_t max_transmit, 1612 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){ 1613 1614 log_info("L2CAP_ACCEPT_ERTM_CONNECTION local_cid 0x%x", local_cid); 1615 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1616 if (!channel) { 1617 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1618 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1619 } 1620 1621 // validate local config 1622 uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1623 if (result) return result; 1624 1625 // configure L2CAP ERTM 1626 l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1627 1628 // continue 1629 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1630 1631 // process 1632 l2cap_run(); 1633 1634 return ERROR_CODE_SUCCESS; 1635 } 1636 #endif 1637 1638 1639 void l2cap_decline_connection(uint16_t local_cid){ 1640 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 1641 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1642 if (!channel) { 1643 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1644 return; 1645 } 1646 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1647 channel->reason = 0x04; // no resources available 1648 l2cap_run(); 1649 } 1650 1651 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 1652 1653 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1654 1655 uint16_t flags = little_endian_read_16(command, 6); 1656 if (flags & 1) { 1657 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1658 } 1659 1660 // accept the other's configuration options 1661 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 1662 uint16_t pos = 8; 1663 while (pos < end_pos){ 1664 uint8_t option_hint = command[pos] >> 7; 1665 uint8_t option_type = command[pos] & 0x7f; 1666 log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 1667 pos++; 1668 uint8_t length = command[pos++]; 1669 // MTU { type(8): 1, len(8):2, MTU(16) } 1670 if (option_type == 1 && length == 2){ 1671 channel->remote_mtu = little_endian_read_16(command, pos); 1672 if (channel->remote_mtu > l2cap_max_mtu()){ 1673 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 1674 channel->remote_mtu = l2cap_max_mtu(); 1675 } 1676 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1677 } 1678 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 1679 if (option_type == 2 && length == 2){ 1680 channel->flush_timeout = little_endian_read_16(command, pos); 1681 } 1682 1683 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1684 // Retransmission and Flow Control Option 1685 if (option_type == 4 && length == 9){ 1686 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 1687 switch(channel->mode){ 1688 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 1689 if (channel->ertm_mandatory){ 1690 // ERTM mandatory, but remote doens't offer ERTM -> disconnect 1691 if (mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1692 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1693 } else { 1694 // Both sides selected ERTM 1695 channel->remote_tx_window_size = command[pos+1]; 1696 channel->remote_max_transmit = command[pos+2]; 1697 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 1698 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 1699 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u", 1700 channel->remote_tx_window_size, 1701 channel->remote_max_transmit, 1702 channel->remote_retransmission_timeout_ms, 1703 channel->remote_monitor_timeout_ms); 1704 // we store remote MPS in remote_mtu, might need to get re-evaluated 1705 channel->remote_mtu = little_endian_read_16(command, pos + 7); 1706 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1707 } 1708 } else { 1709 // ERTM is optional 1710 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1711 } 1712 break; 1713 case L2CAP_CHANNEL_MODE_BASIC: 1714 switch (mode){ 1715 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 1716 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 1717 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 1718 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1719 } 1720 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 1721 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1722 break; 1723 default: // case L2CAP_CHANNEL_MODE_BASIC: 1724 // TODO store and evaluate configuration 1725 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1726 break; 1727 } 1728 break; 1729 default: 1730 break; 1731 } 1732 } 1733 #endif 1734 // check for unknown options 1735 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 1736 log_info("l2cap cid %u, unknown options", channel->local_cid); 1737 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 1738 } 1739 pos += length; 1740 } 1741 } 1742 1743 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 1744 log_info("l2cap_signaling_handle_configure_response"); 1745 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1746 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 1747 uint16_t pos = 10; 1748 while (pos < end_pos){ 1749 uint8_t option_hint = command[pos] >> 7; 1750 uint8_t option_type = command[pos] & 0x7f; 1751 log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 1752 pos++; 1753 uint8_t length = command[pos++]; 1754 1755 // Retransmission and Flow Control Option 1756 if (option_type == 4 && length == 9){ 1757 switch (channel->mode){ 1758 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 1759 if (channel->ertm_mandatory){ 1760 // ?? 1761 } else { 1762 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 1763 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 1764 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1765 } 1766 } 1767 break; 1768 case L2CAP_CHANNEL_MODE_BASIC: 1769 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 1770 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 1771 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1772 } 1773 break; 1774 default: 1775 break; 1776 } 1777 } 1778 1779 // check for unknown options 1780 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 1781 log_info("l2cap cid %u, unknown options", channel->local_cid); 1782 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 1783 } 1784 1785 pos += length; 1786 } 1787 #endif 1788 } 1789 1790 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 1791 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 1792 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 1793 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 1794 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 1795 if (channel->state == L2CAP_STATE_OPEN) return 0; 1796 return 1; 1797 } 1798 1799 1800 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 1801 1802 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 1803 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1804 uint16_t result = 0; 1805 1806 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 1807 1808 // handle DISCONNECT REQUESTS seperately 1809 if (code == DISCONNECTION_REQUEST){ 1810 switch (channel->state){ 1811 case L2CAP_STATE_CONFIG: 1812 case L2CAP_STATE_OPEN: 1813 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1814 case L2CAP_STATE_WAIT_DISCONNECT: 1815 l2cap_handle_disconnect_request(channel, identifier); 1816 break; 1817 1818 default: 1819 // ignore in other states 1820 break; 1821 } 1822 return; 1823 } 1824 1825 // @STATEMACHINE(l2cap) 1826 switch (channel->state) { 1827 1828 case L2CAP_STATE_WAIT_CONNECT_RSP: 1829 switch (code){ 1830 case CONNECTION_RESPONSE: 1831 l2cap_stop_rtx(channel); 1832 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 1833 switch (result) { 1834 case 0: 1835 // successful connection 1836 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 1837 channel->state = L2CAP_STATE_CONFIG; 1838 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1839 break; 1840 case 1: 1841 // connection pending. get some coffee, but start the ERTX 1842 l2cap_start_ertx(channel); 1843 break; 1844 default: 1845 // channel closed 1846 channel->state = L2CAP_STATE_CLOSED; 1847 // map l2cap connection response result to BTstack status enumeration 1848 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 1849 1850 // drop link key if security block 1851 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 1852 gap_drop_link_key_for_bd_addr(channel->address); 1853 } 1854 1855 // discard channel 1856 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1857 btstack_memory_l2cap_channel_free(channel); 1858 break; 1859 } 1860 break; 1861 1862 default: 1863 //@TODO: implement other signaling packets 1864 break; 1865 } 1866 break; 1867 1868 case L2CAP_STATE_CONFIG: 1869 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 1870 switch (code) { 1871 case CONFIGURE_REQUEST: 1872 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1873 l2cap_signaling_handle_configure_request(channel, command); 1874 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 1875 // only done if continuation not set 1876 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 1877 } 1878 break; 1879 case CONFIGURE_RESPONSE: 1880 l2cap_stop_rtx(channel); 1881 l2cap_signaling_handle_configure_response(channel, result, command); 1882 switch (result){ 1883 case 0: // success 1884 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 1885 break; 1886 case 4: // pending 1887 l2cap_start_ertx(channel); 1888 break; 1889 default: 1890 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1891 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 1892 // remote does not offer ertm but it's required 1893 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1894 break; 1895 } 1896 #endif 1897 // retry on negative result 1898 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1899 break; 1900 } 1901 break; 1902 default: 1903 break; 1904 } 1905 if (l2cap_channel_ready_for_open(channel)){ 1906 // for open: 1907 channel->state = L2CAP_STATE_OPEN; 1908 l2cap_emit_channel_opened(channel, 0); 1909 } 1910 break; 1911 1912 case L2CAP_STATE_WAIT_DISCONNECT: 1913 switch (code) { 1914 case DISCONNECTION_RESPONSE: 1915 l2cap_finialize_channel_close(channel); 1916 break; 1917 default: 1918 //@TODO: implement other signaling packets 1919 break; 1920 } 1921 break; 1922 1923 case L2CAP_STATE_CLOSED: 1924 // @TODO handle incoming requests 1925 break; 1926 1927 case L2CAP_STATE_OPEN: 1928 //@TODO: implement other signaling packets, e.g. re-configure 1929 break; 1930 default: 1931 break; 1932 } 1933 // log_info("new state %u", channel->state); 1934 } 1935 1936 1937 static void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){ 1938 1939 btstack_linked_list_iterator_t it; 1940 1941 // get code, signalind identifier and command len 1942 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 1943 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1944 1945 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 1946 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_RESPONSE){ 1947 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 1948 return; 1949 } 1950 1951 // general commands without an assigned channel 1952 switch(code) { 1953 1954 case CONNECTION_REQUEST: { 1955 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 1956 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 1957 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 1958 return; 1959 } 1960 1961 case ECHO_REQUEST: 1962 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 1963 return; 1964 1965 case INFORMATION_REQUEST: { 1966 uint16_t infoType = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 1967 l2cap_register_signaling_response(handle, code, sig_id, 0, infoType); 1968 return; 1969 } 1970 1971 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1972 case INFORMATION_RESPONSE: { 1973 hci_connection_t * connection = hci_connection_for_handle(handle); 1974 if (!connection) return; 1975 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 1976 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 1977 if (result != 0) return; 1978 if (info_type != 0x02) return; 1979 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 1980 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 1981 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 1982 // trigger connection request 1983 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1984 while (btstack_linked_list_iterator_has_next(&it)){ 1985 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1986 if (channel->con_handle != handle) continue; 1987 // bail if ERTM was requested but is not supported 1988 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 1989 if (channel->ertm_mandatory){ 1990 // channel closed 1991 channel->state = L2CAP_STATE_CLOSED; 1992 // map l2cap connection response result to BTstack status enumeration 1993 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTD); 1994 // discard channel 1995 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1996 btstack_memory_l2cap_channel_free(channel); 1997 continue; 1998 } else { 1999 // fallback to Basic mode 2000 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2001 } 2002 } 2003 // start connecting 2004 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2005 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 2006 } 2007 // respond to connection request 2008 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2009 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2010 l2cap_emit_incoming_connection(channel); 2011 } 2012 } 2013 return; 2014 } 2015 #endif 2016 2017 default: 2018 break; 2019 } 2020 2021 2022 // Get potential destination CID 2023 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2024 2025 // Find channel for this sig_id and connection handle 2026 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2027 while (btstack_linked_list_iterator_has_next(&it)){ 2028 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2029 if (channel->con_handle != handle) continue; 2030 if (code & 1) { 2031 // match odd commands (responses) by previous signaling identifier 2032 if (channel->local_sig_id == sig_id) { 2033 l2cap_signaling_handler_channel(channel, command); 2034 break; 2035 } 2036 } else { 2037 // match even commands (requests) by local channel id 2038 if (channel->local_cid == dest_cid) { 2039 l2cap_signaling_handler_channel(channel, command); 2040 break; 2041 } 2042 } 2043 } 2044 } 2045 #endif 2046 2047 #ifdef ENABLE_BLE 2048 2049 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 2050 uint8_t event[6]; 2051 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 2052 event[1] = 4; 2053 little_endian_store_16(event, 2, con_handle); 2054 little_endian_store_16(event, 4, result); 2055 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2056 if (!l2cap_event_packet_handler) return; 2057 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 2058 } 2059 2060 // @returns valid 2061 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 2062 hci_connection_t * connection; 2063 uint16_t result; 2064 uint8_t event[10]; 2065 2066 #ifdef ENABLE_LE_DATA_CHANNELS 2067 btstack_linked_list_iterator_t it; 2068 l2cap_channel_t * channel; 2069 uint16_t local_cid; 2070 uint16_t le_psm; 2071 uint16_t new_credits; 2072 uint16_t credits_before; 2073 l2cap_service_t * service; 2074 uint16_t source_cid; 2075 #endif 2076 2077 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2078 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u", code, sig_id); 2079 2080 switch (code){ 2081 2082 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 2083 result = little_endian_read_16(command, 4); 2084 l2cap_emit_connection_parameter_update_response(handle, result); 2085 break; 2086 2087 case CONNECTION_PARAMETER_UPDATE_REQUEST: 2088 connection = hci_connection_for_handle(handle); 2089 if (connection){ 2090 if (connection->role != HCI_ROLE_MASTER){ 2091 // reject command without notifying upper layer when not in master role 2092 return 0; 2093 } 2094 int update_parameter = 1; 2095 le_connection_parameter_range_t existing_range; 2096 gap_get_connection_parameter_range(&existing_range); 2097 uint16_t le_conn_interval_min = little_endian_read_16(command,8); 2098 uint16_t le_conn_interval_max = little_endian_read_16(command,10); 2099 uint16_t le_conn_latency = little_endian_read_16(command,12); 2100 uint16_t le_supervision_timeout = little_endian_read_16(command,14); 2101 2102 if (le_conn_interval_min < existing_range.le_conn_interval_min) update_parameter = 0; 2103 if (le_conn_interval_max > existing_range.le_conn_interval_max) update_parameter = 0; 2104 2105 if (le_conn_latency < existing_range.le_conn_latency_min) update_parameter = 0; 2106 if (le_conn_latency > existing_range.le_conn_latency_max) update_parameter = 0; 2107 2108 if (le_supervision_timeout < existing_range.le_supervision_timeout_min) update_parameter = 0; 2109 if (le_supervision_timeout > existing_range.le_supervision_timeout_max) update_parameter = 0; 2110 2111 if (update_parameter){ 2112 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 2113 connection->le_conn_interval_min = le_conn_interval_min; 2114 connection->le_conn_interval_max = le_conn_interval_max; 2115 connection->le_conn_latency = le_conn_latency; 2116 connection->le_supervision_timeout = le_supervision_timeout; 2117 } else { 2118 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 2119 } 2120 connection->le_con_param_update_identifier = sig_id; 2121 } 2122 2123 if (!l2cap_event_packet_handler) break; 2124 2125 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 2126 event[1] = 8; 2127 memcpy(&event[2], &command[4], 8); 2128 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2129 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2130 break; 2131 2132 #ifdef ENABLE_LE_DATA_CHANNELS 2133 2134 case COMMAND_REJECT: 2135 // Find channel for this sig_id and connection handle 2136 channel = NULL; 2137 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2138 while (btstack_linked_list_iterator_has_next(&it)){ 2139 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2140 if (a_channel->con_handle != handle) continue; 2141 if (a_channel->local_sig_id != sig_id) continue; 2142 channel = a_channel; 2143 break; 2144 } 2145 if (!channel) break; 2146 2147 // if received while waiting for le connection response, assume legacy device 2148 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 2149 channel->state = L2CAP_STATE_CLOSED; 2150 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 2151 l2cap_emit_le_channel_opened(channel, 0x0002); 2152 2153 // discard channel 2154 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2155 btstack_memory_l2cap_channel_free(channel); 2156 break; 2157 } 2158 break; 2159 2160 case LE_CREDIT_BASED_CONNECTION_REQUEST: 2161 2162 // get hci connection, bail if not found (must not happen) 2163 connection = hci_connection_for_handle(handle); 2164 if (!connection) return 0; 2165 2166 // check if service registered 2167 le_psm = little_endian_read_16(command, 4); 2168 service = l2cap_le_get_service(le_psm); 2169 source_cid = little_endian_read_16(command, 6); 2170 2171 if (service){ 2172 if (source_cid < 0x40){ 2173 // 0x0009 Connection refused - Invalid Source CID 2174 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 2175 return 1; 2176 } 2177 2178 // go through list of channels for this ACL connection and check if we get a match 2179 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2180 while (btstack_linked_list_iterator_has_next(&it)){ 2181 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2182 if (a_channel->con_handle != handle) continue; 2183 if (a_channel->remote_cid != source_cid) continue; 2184 // 0x000a Connection refused - Source CID already allocated 2185 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 2186 return 1; 2187 } 2188 2189 // security: check encryption 2190 if (service->required_security_level >= LEVEL_2){ 2191 if (sm_encryption_key_size(handle) == 0){ 2192 // 0x0008 Connection refused - insufficient encryption 2193 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 2194 return 1; 2195 } 2196 // anything less than 16 byte key size is insufficient 2197 if (sm_encryption_key_size(handle) < 16){ 2198 // 0x0007 Connection refused – insufficient encryption key size 2199 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 2200 return 1; 2201 } 2202 } 2203 2204 // security: check authencation 2205 if (service->required_security_level >= LEVEL_3){ 2206 if (!sm_authenticated(handle)){ 2207 // 0x0005 Connection refused – insufficient authentication 2208 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 2209 return 1; 2210 } 2211 } 2212 2213 // security: check authorization 2214 if (service->required_security_level >= LEVEL_4){ 2215 if (sm_authorization_state(handle) != AUTHORIZATION_GRANTED){ 2216 // 0x0006 Connection refused – insufficient authorization 2217 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 2218 return 1; 2219 } 2220 } 2221 2222 // allocate channel 2223 channel = l2cap_create_channel_entry(service->packet_handler, connection->address, 2224 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 2225 if (!channel){ 2226 // 0x0004 Connection refused – no resources available 2227 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2228 return 1; 2229 } 2230 2231 channel->con_handle = handle; 2232 channel->remote_cid = source_cid; 2233 channel->remote_sig_id = sig_id; 2234 channel->remote_mtu = little_endian_read_16(command, 8); 2235 channel->remote_mps = little_endian_read_16(command, 10); 2236 channel->credits_outgoing = little_endian_read_16(command, 12); 2237 2238 // set initial state 2239 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2240 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 2241 2242 // add to connections list 2243 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2244 2245 // post connection request event 2246 l2cap_emit_le_incoming_connection(channel); 2247 2248 } else { 2249 // Connection refused – LE_PSM not supported 2250 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2251 } 2252 break; 2253 2254 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 2255 // Find channel for this sig_id and connection handle 2256 channel = NULL; 2257 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2258 while (btstack_linked_list_iterator_has_next(&it)){ 2259 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2260 if (a_channel->con_handle != handle) continue; 2261 if (a_channel->local_sig_id != sig_id) continue; 2262 channel = a_channel; 2263 break; 2264 } 2265 if (!channel) break; 2266 2267 // cid + 0 2268 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 2269 if (result){ 2270 channel->state = L2CAP_STATE_CLOSED; 2271 // map l2cap connection response result to BTstack status enumeration 2272 l2cap_emit_le_channel_opened(channel, result); 2273 2274 // discard channel 2275 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2276 btstack_memory_l2cap_channel_free(channel); 2277 break; 2278 } 2279 2280 // success 2281 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2282 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2283 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 2284 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 2285 channel->state = L2CAP_STATE_OPEN; 2286 l2cap_emit_le_channel_opened(channel, result); 2287 break; 2288 2289 case LE_FLOW_CONTROL_CREDIT: 2290 // find channel 2291 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2292 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2293 if (!channel) { 2294 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2295 break; 2296 } 2297 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2298 credits_before = channel->credits_outgoing; 2299 channel->credits_outgoing += new_credits; 2300 // check for credit overrun 2301 if (credits_before > channel->credits_outgoing){ 2302 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 2303 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2304 break; 2305 } 2306 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 2307 break; 2308 2309 case DISCONNECTION_REQUEST: 2310 // find channel 2311 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2312 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2313 if (!channel) { 2314 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2315 break; 2316 } 2317 channel->remote_sig_id = sig_id; 2318 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2319 break; 2320 2321 #endif 2322 2323 case DISCONNECTION_RESPONSE: 2324 break; 2325 2326 default: 2327 // command unknown -> reject command 2328 return 0; 2329 } 2330 return 1; 2331 } 2332 #endif 2333 2334 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size ){ 2335 UNUSED(packet_type); 2336 UNUSED(channel); 2337 2338 l2cap_channel_t * l2cap_channel; 2339 UNUSED(l2cap_channel); 2340 2341 // Get Channel ID 2342 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 2343 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 2344 2345 switch (channel_id) { 2346 2347 #ifdef ENABLE_CLASSIC 2348 case L2CAP_CID_SIGNALING: { 2349 uint16_t command_offset = 8; 2350 while (command_offset < size) { 2351 // handle signaling commands 2352 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 2353 2354 // increment command_offset 2355 command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2356 } 2357 break; 2358 } 2359 #endif 2360 2361 #ifdef ENABLE_BLE 2362 case L2CAP_CID_SIGNALING_LE: { 2363 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 2364 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 2365 if (!valid){ 2366 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2367 } 2368 break; 2369 } 2370 #endif 2371 2372 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 2373 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback) { 2374 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2375 } 2376 break; 2377 2378 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 2379 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback) { 2380 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2381 } 2382 break; 2383 2384 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 2385 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback) { 2386 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2387 } 2388 break; 2389 2390 default: 2391 #ifdef ENABLE_CLASSIC 2392 // Find channel for this channel_id and connection handle 2393 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 2394 if (l2cap_channel) { 2395 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2396 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2397 2398 // verify FCS 2399 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 2400 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 2401 log_info("Packet FCS 0x%04x, calculated FCS 0x%04x", fcs_packet, fcs_calculated); 2402 if (fcs_calculated != fcs_packet){ 2403 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 2404 // TODO: trigger retransmission or something like that 2405 break; 2406 } 2407 2408 // switch on packet type 2409 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2410 if (control & 1){ 2411 log_info("S-Frame not not implemented yet"); 2412 // S-Frame 2413 break; 2414 } else { 2415 // I-Frame 2416 // get control 2417 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 2418 log_info("Control: 0x%04x, SAR type %u, pos %u", control, (int) sar, l2cap_channel->rx_packets_state->pos); 2419 uint16_t sdu_length; 2420 uint16_t segment_length; 2421 uint16_t payload_offset; 2422 switch (sar){ 2423 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 2424 payload_offset = COMPLETE_L2CAP_HEADER+2; 2425 segment_length = payload_offset-2; 2426 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER+2], segment_length); 2427 break; 2428 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 2429 // TODO: use current packet 2430 // TODO: check if reassembly started 2431 // TODO: check len against local mtu 2432 sdu_length = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER+2); 2433 payload_offset = COMPLETE_L2CAP_HEADER+4; 2434 segment_length = size - payload_offset-2; 2435 memcpy(&l2cap_channel->rx_packets_data[0], &packet[payload_offset], segment_length); 2436 l2cap_channel->rx_packets_state->sdu_length = sdu_length; 2437 l2cap_channel->rx_packets_state->pos = segment_length; 2438 break; 2439 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 2440 payload_offset = COMPLETE_L2CAP_HEADER+2; 2441 segment_length = size - payload_offset-2; 2442 memcpy(&l2cap_channel->rx_packets_data[l2cap_channel->rx_packets_state->pos], &packet[payload_offset], segment_length); 2443 l2cap_channel->rx_packets_state->pos += segment_length; 2444 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->rx_packets_data, l2cap_channel->rx_packets_state[0].pos); 2445 break; 2446 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 2447 payload_offset = COMPLETE_L2CAP_HEADER+2; 2448 segment_length = size - payload_offset-2; 2449 memcpy(&l2cap_channel->rx_packets_data[l2cap_channel->rx_packets_state->pos], &packet[payload_offset], segment_length); 2450 l2cap_channel->rx_packets_state->pos += segment_length; 2451 break; 2452 } 2453 } 2454 break; 2455 } 2456 #endif 2457 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2458 } 2459 #endif 2460 #ifdef ENABLE_LE_DATA_CHANNELS 2461 l2cap_channel = l2cap_le_get_channel_for_local_cid(channel_id); 2462 if (l2cap_channel) { 2463 // credit counting 2464 if (l2cap_channel->credits_incoming == 0){ 2465 log_error("LE Data Channel packet received but no incoming credits"); 2466 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2467 break; 2468 } 2469 l2cap_channel->credits_incoming--; 2470 2471 // automatic credits 2472 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 2473 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 2474 } 2475 2476 // first fragment 2477 uint16_t pos = 0; 2478 if (!l2cap_channel->receive_sdu_len){ 2479 l2cap_channel->receive_sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2480 l2cap_channel->receive_sdu_pos = 0; 2481 pos += 2; 2482 size -= 2; 2483 } 2484 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], size-COMPLETE_L2CAP_HEADER); 2485 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 2486 // done? 2487 log_info("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 2488 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 2489 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 2490 l2cap_channel->receive_sdu_len = 0; 2491 } 2492 } else { 2493 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 2494 } 2495 #endif 2496 break; 2497 } 2498 2499 l2cap_run(); 2500 } 2501 2502 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 2503 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 2504 int index = l2cap_fixed_channel_table_index_for_channel_id(channel_id); 2505 if (index < 0) return; 2506 fixed_channels[index].callback = the_packet_handler; 2507 } 2508 2509 #ifdef ENABLE_CLASSIC 2510 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 2511 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 2512 channel->state = L2CAP_STATE_CLOSED; 2513 l2cap_emit_channel_closed(channel); 2514 // discard channel 2515 l2cap_stop_rtx(channel); 2516 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2517 btstack_memory_l2cap_channel_free(channel); 2518 } 2519 2520 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 2521 btstack_linked_list_iterator_t it; 2522 btstack_linked_list_iterator_init(&it, services); 2523 while (btstack_linked_list_iterator_has_next(&it)){ 2524 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 2525 if ( service->psm == psm){ 2526 return service; 2527 }; 2528 } 2529 return NULL; 2530 } 2531 2532 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 2533 return l2cap_get_service_internal(&l2cap_services, psm); 2534 } 2535 2536 2537 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 2538 2539 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 2540 2541 // check for alread registered psm 2542 l2cap_service_t *service = l2cap_get_service(psm); 2543 if (service) { 2544 log_error("l2cap_register_service: PSM %u already registered", psm); 2545 return L2CAP_SERVICE_ALREADY_REGISTERED; 2546 } 2547 2548 // alloc structure 2549 service = btstack_memory_l2cap_service_get(); 2550 if (!service) { 2551 log_error("l2cap_register_service: no memory for l2cap_service_t"); 2552 return BTSTACK_MEMORY_ALLOC_FAILED; 2553 } 2554 2555 // fill in 2556 service->psm = psm; 2557 service->mtu = mtu; 2558 service->packet_handler = service_packet_handler; 2559 service->required_security_level = security_level; 2560 2561 // add to services list 2562 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 2563 2564 // enable page scan 2565 gap_connectable_control(1); 2566 2567 return 0; 2568 } 2569 2570 uint8_t l2cap_unregister_service(uint16_t psm){ 2571 2572 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 2573 2574 l2cap_service_t *service = l2cap_get_service(psm); 2575 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 2576 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 2577 btstack_memory_l2cap_service_free(service); 2578 2579 // disable page scan when no services registered 2580 if (btstack_linked_list_empty(&l2cap_services)) { 2581 gap_connectable_control(0); 2582 } 2583 return 0; 2584 } 2585 #endif 2586 2587 2588 #ifdef ENABLE_LE_DATA_CHANNELS 2589 2590 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 2591 if (!channel->waiting_for_can_send_now) return; 2592 if (channel->send_sdu_buffer) return; 2593 channel->waiting_for_can_send_now = 0; 2594 log_info("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 2595 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 2596 } 2597 2598 // 1BH2222 2599 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 2600 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", 2601 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 2602 uint8_t event[19]; 2603 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 2604 event[1] = sizeof(event) - 2; 2605 event[2] = channel->address_type; 2606 reverse_bd_addr(channel->address, &event[3]); 2607 little_endian_store_16(event, 9, channel->con_handle); 2608 little_endian_store_16(event, 11, channel->psm); 2609 little_endian_store_16(event, 13, channel->local_cid); 2610 little_endian_store_16(event, 15, channel->remote_cid); 2611 little_endian_store_16(event, 17, channel->remote_mtu); 2612 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2613 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 2614 } 2615 // 11BH22222 2616 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 2617 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", 2618 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 2619 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 2620 uint8_t event[23]; 2621 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 2622 event[1] = sizeof(event) - 2; 2623 event[2] = status; 2624 event[3] = channel->address_type; 2625 reverse_bd_addr(channel->address, &event[4]); 2626 little_endian_store_16(event, 10, channel->con_handle); 2627 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 2628 little_endian_store_16(event, 13, channel->psm); 2629 little_endian_store_16(event, 15, channel->local_cid); 2630 little_endian_store_16(event, 17, channel->remote_cid); 2631 little_endian_store_16(event, 19, channel->local_mtu); 2632 little_endian_store_16(event, 21, channel->remote_mtu); 2633 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2634 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 2635 } 2636 2637 static l2cap_channel_t * l2cap_le_get_channel_for_local_cid(uint16_t local_cid){ 2638 btstack_linked_list_iterator_t it; 2639 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2640 while (btstack_linked_list_iterator_has_next(&it)){ 2641 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2642 if ( channel->local_cid == local_cid) { 2643 return channel; 2644 } 2645 } 2646 return NULL; 2647 } 2648 2649 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 2650 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 2651 channel->state = L2CAP_STATE_CLOSED; 2652 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 2653 // discard channel 2654 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2655 btstack_memory_l2cap_channel_free(channel); 2656 } 2657 2658 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 2659 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 2660 } 2661 2662 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 2663 2664 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 2665 2666 // check for alread registered psm 2667 l2cap_service_t *service = l2cap_le_get_service(psm); 2668 if (service) { 2669 return L2CAP_SERVICE_ALREADY_REGISTERED; 2670 } 2671 2672 // alloc structure 2673 service = btstack_memory_l2cap_service_get(); 2674 if (!service) { 2675 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 2676 return BTSTACK_MEMORY_ALLOC_FAILED; 2677 } 2678 2679 // fill in 2680 service->psm = psm; 2681 service->mtu = 0; 2682 service->packet_handler = packet_handler; 2683 service->required_security_level = security_level; 2684 2685 // add to services list 2686 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 2687 2688 // done 2689 return 0; 2690 } 2691 2692 uint8_t l2cap_le_unregister_service(uint16_t psm) { 2693 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 2694 l2cap_service_t *service = l2cap_le_get_service(psm); 2695 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 2696 2697 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 2698 btstack_memory_l2cap_service_free(service); 2699 return 0; 2700 } 2701 2702 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 2703 // get channel 2704 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2705 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2706 2707 // validate state 2708 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 2709 return ERROR_CODE_COMMAND_DISALLOWED; 2710 } 2711 2712 // set state accept connection 2713 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 2714 channel->receive_sdu_buffer = receive_sdu_buffer; 2715 channel->local_mtu = mtu; 2716 channel->new_credits_incoming = initial_credits; 2717 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 2718 2719 // test 2720 // channel->new_credits_incoming = 1; 2721 2722 // go 2723 l2cap_run(); 2724 return 0; 2725 } 2726 2727 /** 2728 * @brief Deny incoming LE Data Channel connection due to resource constraints 2729 * @param local_cid L2CAP LE Data Channel Identifier 2730 */ 2731 2732 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 2733 // get channel 2734 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2735 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2736 2737 // validate state 2738 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 2739 return ERROR_CODE_COMMAND_DISALLOWED; 2740 } 2741 2742 // set state decline connection 2743 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 2744 channel->reason = 0x04; // no resources available 2745 l2cap_run(); 2746 return 0; 2747 } 2748 2749 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 2750 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 2751 uint16_t * out_local_cid) { 2752 2753 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 2754 2755 2756 hci_connection_t * connection = hci_connection_for_handle(con_handle); 2757 if (!connection) { 2758 log_error("no hci_connection for handle 0x%04x", con_handle); 2759 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 2760 } 2761 2762 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, connection->address, connection->address_type, psm, mtu, security_level); 2763 if (!channel) { 2764 return BTSTACK_MEMORY_ALLOC_FAILED; 2765 } 2766 log_info("l2cap_le_create_channel %p", channel); 2767 2768 // store local_cid 2769 if (out_local_cid){ 2770 *out_local_cid = channel->local_cid; 2771 } 2772 2773 // provide buffer 2774 channel->con_handle = con_handle; 2775 channel->receive_sdu_buffer = receive_sdu_buffer; 2776 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 2777 channel->new_credits_incoming = initial_credits; 2778 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 2779 2780 // add to connections list 2781 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2782 2783 // go 2784 l2cap_run(); 2785 return 0; 2786 } 2787 2788 /** 2789 * @brief Provide credtis for LE Data Channel 2790 * @param local_cid L2CAP LE Data Channel Identifier 2791 * @param credits Number additional credits for peer 2792 */ 2793 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 2794 2795 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2796 if (!channel) { 2797 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 2798 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2799 } 2800 2801 // check state 2802 if (channel->state != L2CAP_STATE_OPEN){ 2803 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 2804 } 2805 2806 // assert incoming credits + credits <= 0xffff 2807 uint32_t total_credits = channel->credits_incoming; 2808 total_credits += channel->new_credits_incoming; 2809 total_credits += credits; 2810 if (total_credits > 0xffff){ 2811 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 2812 channel->new_credits_incoming, credits); 2813 } 2814 2815 // set credits_granted 2816 channel->new_credits_incoming += credits; 2817 2818 // go 2819 l2cap_run(); 2820 return 0; 2821 } 2822 2823 /** 2824 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 2825 * @param local_cid L2CAP LE Data Channel Identifier 2826 */ 2827 int l2cap_le_can_send_now(uint16_t local_cid){ 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 0; 2832 } 2833 2834 // check state 2835 if (channel->state != L2CAP_STATE_OPEN) return 0; 2836 2837 // check queue 2838 if (channel->send_sdu_buffer) return 0; 2839 2840 // fine, go ahead 2841 return 1; 2842 } 2843 2844 /** 2845 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 2846 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 2847 * so packet handler should be ready to handle it 2848 * @param local_cid L2CAP LE Data Channel Identifier 2849 */ 2850 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 2851 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2852 if (!channel) { 2853 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 2854 return 0; 2855 } 2856 channel->waiting_for_can_send_now = 1; 2857 l2cap_le_notify_channel_can_send(channel); 2858 return 0; 2859 } 2860 2861 /** 2862 * @brief Send data via LE Data Channel 2863 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 2864 * @param local_cid L2CAP LE Data Channel Identifier 2865 * @param data data to send 2866 * @param size data size 2867 */ 2868 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 2869 2870 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2871 if (!channel) { 2872 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 2873 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2874 } 2875 2876 if (len > channel->remote_mtu){ 2877 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 2878 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 2879 } 2880 2881 if (channel->send_sdu_buffer){ 2882 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 2883 return BTSTACK_ACL_BUFFERS_FULL; 2884 } 2885 2886 channel->send_sdu_buffer = data; 2887 channel->send_sdu_len = len; 2888 channel->send_sdu_pos = 0; 2889 2890 l2cap_run(); 2891 return 0; 2892 } 2893 2894 /** 2895 * @brief Disconnect from LE Data Channel 2896 * @param local_cid L2CAP LE Data Channel Identifier 2897 */ 2898 uint8_t l2cap_le_disconnect(uint16_t local_cid) 2899 { 2900 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2901 if (!channel) { 2902 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 2903 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2904 } 2905 2906 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2907 l2cap_run(); 2908 return 0; 2909 } 2910 2911 #endif 2912