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