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