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