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