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