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 if (channel->send_supervisor_frame_receiver_not_ready){ 976 channel->send_supervisor_frame_receiver_not_ready = 0;; 977 log_info("Send S-Frame: RNR %u", channel->req_seq); 978 uint16_t control = l2cap_encanced_control_field_for_supevisor_frame( L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY, 0, 0, channel->req_seq); 979 l2cap_ertm_send_supervisor_frame(channel, control); 980 continue; 981 } 982 #endif 983 984 } 985 #endif 986 987 #ifdef ENABLE_LE_DATA_CHANNELS 988 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 989 while (btstack_linked_list_iterator_has_next(&it)){ 990 uint8_t * acl_buffer; 991 uint8_t * l2cap_payload; 992 uint16_t pos; 993 uint16_t payload_size; 994 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 995 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 996 switch (channel->state){ 997 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 998 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 999 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 1000 // le psm, source cid, mtu, mps, initial credits 1001 channel->local_sig_id = l2cap_next_sig_id(); 1002 channel->credits_incoming = channel->new_credits_incoming; 1003 channel->new_credits_incoming = 0; 1004 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); 1005 break; 1006 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 1007 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1008 // TODO: support larger MPS 1009 channel->state = L2CAP_STATE_OPEN; 1010 channel->credits_incoming = channel->new_credits_incoming; 1011 channel->new_credits_incoming = 0; 1012 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); 1013 // notify client 1014 l2cap_emit_le_channel_opened(channel, 0); 1015 break; 1016 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 1017 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1018 channel->state = L2CAP_STATE_INVALID; 1019 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 1020 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 1021 l2cap_stop_rtx(channel); 1022 btstack_linked_list_iterator_remove(&it); 1023 btstack_memory_l2cap_channel_free(channel); 1024 break; 1025 case L2CAP_STATE_OPEN: 1026 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1027 1028 // send credits 1029 if (channel->new_credits_incoming){ 1030 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 1031 channel->local_sig_id = l2cap_next_sig_id(); 1032 uint16_t new_credits = channel->new_credits_incoming; 1033 channel->new_credits_incoming = 0; 1034 channel->credits_incoming += new_credits; 1035 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 1036 break; 1037 } 1038 1039 // send data 1040 if (!channel->send_sdu_buffer) break; 1041 if (!channel->credits_outgoing) break; 1042 1043 // send part of SDU 1044 hci_reserve_packet_buffer(); 1045 acl_buffer = hci_get_outgoing_packet_buffer(); 1046 l2cap_payload = acl_buffer + 8; 1047 pos = 0; 1048 if (!channel->send_sdu_pos){ 1049 // store SDU len 1050 channel->send_sdu_pos += 2; 1051 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 1052 pos += 2; 1053 } 1054 payload_size = btstack_min(channel->send_sdu_len + 2 - channel->send_sdu_pos, channel->remote_mps - pos); 1055 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 1056 memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len 1057 pos += payload_size; 1058 channel->send_sdu_pos += payload_size; 1059 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 1060 // done 1061 1062 channel->credits_outgoing--; 1063 1064 if (channel->send_sdu_pos >= channel->send_sdu_len + 2){ 1065 channel->send_sdu_buffer = NULL; 1066 // send done event 1067 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 1068 // inform about can send now 1069 l2cap_le_notify_channel_can_send(channel); 1070 } 1071 hci_send_acl_packet_buffer(8 + pos); 1072 break; 1073 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1074 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1075 channel->local_sig_id = l2cap_next_sig_id(); 1076 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 1077 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 1078 break; 1079 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 1080 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 1081 channel->state = L2CAP_STATE_INVALID; 1082 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 1083 l2cap_le_finialize_channel_close(channel); // -- remove from list 1084 break; 1085 default: 1086 break; 1087 } 1088 } 1089 #endif 1090 1091 #ifdef ENABLE_BLE 1092 // send l2cap con paramter update if necessary 1093 hci_connections_get_iterator(&it); 1094 while(btstack_linked_list_iterator_has_next(&it)){ 1095 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 1096 if (connection->address_type != BD_ADDR_TYPE_LE_PUBLIC && connection->address_type != BD_ADDR_TYPE_LE_RANDOM) continue; 1097 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 1098 switch (connection->le_con_parameter_update_state){ 1099 case CON_PARAMETER_UPDATE_SEND_REQUEST: 1100 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1101 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, connection->le_con_param_update_identifier, 1102 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 1103 break; 1104 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 1105 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 1106 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 1107 break; 1108 case CON_PARAMETER_UPDATE_DENY: 1109 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 1110 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 1111 break; 1112 default: 1113 break; 1114 } 1115 } 1116 #endif 1117 1118 // log_info("l2cap_run: exit"); 1119 } 1120 1121 #ifdef ENABLE_CLASSIC 1122 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 1123 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 1124 log_info("l2cap_handle_connection_complete expected state"); 1125 // success, start l2cap handshake 1126 channel->con_handle = con_handle; 1127 // check remote SSP feature first 1128 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 1129 } 1130 } 1131 1132 static void l2cap_ready_to_connect(l2cap_channel_t * channel){ 1133 1134 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1135 // assumption: outgoing connection 1136 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1137 if (connection->l2cap_state.information_state == L2CAP_INFORMATION_STATE_IDLE){ 1138 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1139 channel->state = L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES; 1140 return; 1141 } 1142 #endif 1143 1144 // fine, go ahead 1145 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1146 } 1147 1148 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 1149 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 1150 1151 // we have been waiting for remote supported features, if both support SSP, 1152 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)); 1153 if (gap_ssp_supported_on_both_sides(channel->con_handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){ 1154 // request security level 2 1155 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 1156 gap_request_security_level(channel->con_handle, LEVEL_2); 1157 return; 1158 } 1159 1160 l2cap_ready_to_connect(channel); 1161 } 1162 #endif 1163 1164 #ifdef L2CAP_USES_CHANNELS 1165 static l2cap_channel_t * l2cap_create_channel_entry(btstack_packet_handler_t packet_handler, bd_addr_t address, bd_addr_type_t address_type, 1166 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 1167 1168 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 1169 if (!channel) { 1170 return NULL; 1171 } 1172 1173 // Init memory (make valgrind happy) 1174 memset(channel, 0, sizeof(l2cap_channel_t)); 1175 1176 // fill in 1177 channel->packet_handler = packet_handler; 1178 bd_addr_copy(channel->address, address); 1179 channel->address_type = address_type; 1180 channel->psm = psm; 1181 channel->local_mtu = local_mtu; 1182 channel->remote_mtu = L2CAP_MINIMAL_MTU; 1183 channel->required_security_level = security_level; 1184 1185 // 1186 channel->local_cid = l2cap_next_local_cid(); 1187 channel->con_handle = 0; 1188 1189 // set initial state 1190 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 1191 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 1192 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 1193 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 1194 1195 return channel; 1196 } 1197 #endif 1198 1199 #ifdef ENABLE_CLASSIC 1200 1201 /** 1202 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 1203 * @param packet_handler 1204 * @param address 1205 * @param psm 1206 * @param mtu 1207 * @param local_cid 1208 */ 1209 1210 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){ 1211 // limit MTU to the size of our outtgoing HCI buffer 1212 uint16_t local_mtu = btstack_min(mtu, l2cap_max_mtu()); 1213 1214 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x mtu %u -> local mtu %u", bd_addr_to_str(address), psm, mtu, local_mtu); 1215 1216 l2cap_channel_t * channel = l2cap_create_channel_entry(channel_packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1217 if (!channel) { 1218 return BTSTACK_MEMORY_ALLOC_FAILED; 1219 } 1220 1221 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1222 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1223 #endif 1224 1225 // add to connections list 1226 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1227 1228 // store local_cid 1229 if (out_local_cid){ 1230 *out_local_cid = channel->local_cid; 1231 } 1232 1233 // check if hci connection is already usable 1234 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1235 if (conn){ 1236 log_info("l2cap_create_channel, hci connection already exists"); 1237 l2cap_handle_connection_complete(conn->con_handle, channel); 1238 // check if remote supported fearures are already received 1239 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1240 l2cap_handle_remote_supported_features_received(channel); 1241 } 1242 } 1243 1244 l2cap_run(); 1245 1246 return 0; 1247 } 1248 1249 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1250 1251 static uint8_t l2cap_ertm_validate_local_config(uint8_t max_transmit, uint16_t retransmission_timeout_ms, 1252 uint16_t monitor_timeout_ms, uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size){ 1253 UNUSED(buffer); 1254 UNUSED(size); 1255 1256 uint8_t result = ERROR_CODE_SUCCESS; 1257 if (max_transmit < 1){ 1258 log_error("max_transmit must be >= 1"); 1259 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1260 } 1261 if (retransmission_timeout_ms < 2000){ 1262 log_error("retransmission_timeout_ms must be >= 2000 ms"); 1263 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1264 } 1265 if (monitor_timeout_ms < 12000){ 1266 log_error("monitor_timeout_ms must be >= 12000 ms"); 1267 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1268 } 1269 if (num_rx_buffers < 1){ 1270 log_error("num_rx_buffers must be >= 1"); 1271 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1272 } 1273 if (num_tx_buffers < 1){ 1274 log_error("num_rx_buffers must be >= 1"); 1275 result = ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS; 1276 } 1277 return result; 1278 } 1279 1280 static void l2cap_ertm_configure_channel(l2cap_channel_t * channel, int ertm_mandatory, uint8_t max_transmit, 1281 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){ 1282 1283 channel->mode = L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION; 1284 channel->ertm_mandatory = ertm_mandatory; 1285 channel->local_max_transmit = max_transmit; 1286 channel->local_retransmission_timeout_ms = retransmission_timeout_ms; 1287 channel->local_monitor_timeout_ms = monitor_timeout_ms; 1288 channel->num_rx_buffers = num_rx_buffers; 1289 channel->num_tx_buffers = num_tx_buffers; 1290 1291 // TODO: align buffer pointer 1292 uint32_t pos = 0; 1293 channel->rx_packets_state = (l2cap_ertm_rx_packet_state_t *) &buffer[pos]; 1294 pos += num_rx_buffers * sizeof(l2cap_ertm_rx_packet_state_t); 1295 channel->tx_packets_state = (l2cap_ertm_tx_packet_state_t *) &buffer[pos]; 1296 pos += num_tx_buffers * sizeof(l2cap_ertm_tx_packet_state_t); 1297 // calculate MTU 1298 channel->local_mtu = (size - pos) / (num_rx_buffers + num_tx_buffers); 1299 log_info("Local ERTM MTU: %u", channel->local_mtu); 1300 channel->rx_packets_data = &buffer[pos]; 1301 pos += num_rx_buffers * channel->local_mtu; 1302 channel->tx_packets_data = &buffer[pos]; 1303 log_info("RX packets %p, TX packets %p", channel->rx_packets_data, channel->tx_packets_data); 1304 } 1305 1306 uint8_t l2cap_create_ertm_channel(btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, 1307 int ertm_mandatory, uint8_t max_transmit, uint16_t retransmission_timeout_ms, uint16_t monitor_timeout_ms, 1308 uint8_t num_tx_buffers, uint8_t num_rx_buffers, uint8_t * buffer, uint32_t size, uint16_t * out_local_cid){ 1309 1310 // limit MTU to the size of our outtgoing HCI buffer 1311 uint16_t local_mtu = l2cap_max_mtu(); 1312 1313 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x -> local mtu %u", bd_addr_to_str(address), psm, local_mtu); 1314 1315 // validate local config 1316 uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1317 if (result) return result; 1318 1319 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 1320 if (!channel) { 1321 return BTSTACK_MEMORY_ALLOC_FAILED; 1322 } 1323 1324 // configure ERTM 1325 l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, 1326 monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1327 1328 // add to connections list 1329 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1330 1331 // store local_cid 1332 if (out_local_cid){ 1333 *out_local_cid = channel->local_cid; 1334 } 1335 1336 // check if hci connection is already usable 1337 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 1338 if (conn){ 1339 log_info("l2cap_create_channel, hci connection already exists"); 1340 l2cap_handle_connection_complete(conn->con_handle, channel); 1341 // check if remote supported fearures are already received 1342 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 1343 l2cap_handle_remote_supported_features_received(channel); 1344 } 1345 } 1346 1347 l2cap_run(); 1348 1349 return 0; 1350 } 1351 #endif 1352 1353 void 1354 l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 1355 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 1356 // find channel for local_cid 1357 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1358 if (channel) { 1359 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1360 } 1361 // process 1362 l2cap_run(); 1363 } 1364 1365 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 1366 btstack_linked_list_iterator_t it; 1367 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1368 while (btstack_linked_list_iterator_has_next(&it)){ 1369 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1370 if ( bd_addr_cmp( channel->address, address) != 0) continue; 1371 // channel for this address found 1372 switch (channel->state){ 1373 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 1374 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1375 // failure, forward error code 1376 l2cap_emit_channel_opened(channel, status); 1377 // discard channel 1378 l2cap_stop_rtx(channel); 1379 btstack_linked_list_iterator_remove(&it); 1380 btstack_memory_l2cap_channel_free(channel); 1381 break; 1382 default: 1383 break; 1384 } 1385 } 1386 } 1387 1388 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 1389 btstack_linked_list_iterator_t it; 1390 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1391 while (btstack_linked_list_iterator_has_next(&it)){ 1392 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1393 if ( ! bd_addr_cmp( channel->address, address) ){ 1394 l2cap_handle_connection_complete(handle, channel); 1395 } 1396 } 1397 // process 1398 l2cap_run(); 1399 } 1400 #endif 1401 1402 static void l2cap_notify_channel_can_send(void){ 1403 1404 #ifdef ENABLE_CLASSIC 1405 btstack_linked_list_iterator_t it; 1406 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1407 while (btstack_linked_list_iterator_has_next(&it)){ 1408 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1409 if (!channel->waiting_for_can_send_now) continue; 1410 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1411 channel->waiting_for_can_send_now = 0; 1412 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 1413 } 1414 #endif 1415 1416 int i; 1417 for (i=0;i<L2CAP_FIXED_CHANNEL_TABLE_SIZE;i++){ 1418 if (!fixed_channels[i].callback) continue; 1419 if (!fixed_channels[i].waiting_for_can_send_now) continue; 1420 int can_send = 0; 1421 if (l2cap_fixed_channel_table_index_is_le(i)){ 1422 #ifdef ENABLE_BLE 1423 can_send = hci_can_send_acl_le_packet_now(); 1424 #endif 1425 } else { 1426 #ifdef ENABLE_CLASSIC 1427 can_send = hci_can_send_acl_classic_packet_now(); 1428 #endif 1429 } 1430 if (!can_send) continue; 1431 fixed_channels[i].waiting_for_can_send_now = 0; 1432 l2cap_emit_can_send_now(fixed_channels[i].callback, l2cap_fixed_channel_table_channel_id_for_index(i)); 1433 } 1434 } 1435 1436 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 1437 1438 UNUSED(packet_type); 1439 UNUSED(cid); 1440 UNUSED(size); 1441 1442 bd_addr_t address; 1443 hci_con_handle_t handle; 1444 int hci_con_used; 1445 btstack_linked_list_iterator_t it; 1446 1447 // avoid unused warnings 1448 UNUSED(address); 1449 UNUSED(hci_con_used); 1450 UNUSED(it); 1451 UNUSED(handle); 1452 1453 switch(hci_event_packet_get_type(packet)){ 1454 1455 // Notify channel packet handler if they can send now 1456 case HCI_EVENT_TRANSPORT_PACKET_SENT: 1457 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 1458 l2cap_run(); // try sending signaling packets first 1459 l2cap_notify_channel_can_send(); 1460 break; 1461 1462 case HCI_EVENT_COMMAND_STATUS: 1463 l2cap_run(); // try sending signaling packets first 1464 break; 1465 1466 #ifdef ENABLE_CLASSIC 1467 // handle connection complete events 1468 case HCI_EVENT_CONNECTION_COMPLETE: 1469 reverse_bd_addr(&packet[5], address); 1470 if (packet[2] == 0){ 1471 handle = little_endian_read_16(packet, 3); 1472 l2cap_handle_connection_success_for_addr(address, handle); 1473 } else { 1474 l2cap_handle_connection_failed_for_addr(address, packet[2]); 1475 } 1476 break; 1477 1478 // handle successful create connection cancel command 1479 case HCI_EVENT_COMMAND_COMPLETE: 1480 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 1481 if (packet[5] == 0){ 1482 reverse_bd_addr(&packet[6], address); 1483 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 1484 l2cap_handle_connection_failed_for_addr(address, 0x16); 1485 } 1486 } 1487 l2cap_run(); // try sending signaling packets first 1488 break; 1489 #endif 1490 1491 // handle disconnection complete events 1492 case HCI_EVENT_DISCONNECTION_COMPLETE: 1493 // send l2cap disconnect events for all channels on this handle and free them 1494 #ifdef ENABLE_CLASSIC 1495 handle = little_endian_read_16(packet, 3); 1496 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1497 while (btstack_linked_list_iterator_has_next(&it)){ 1498 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1499 if (channel->con_handle != handle) continue; 1500 l2cap_emit_channel_closed(channel); 1501 l2cap_stop_rtx(channel); 1502 btstack_linked_list_iterator_remove(&it); 1503 btstack_memory_l2cap_channel_free(channel); 1504 } 1505 #endif 1506 #ifdef ENABLE_LE_DATA_CHANNELS 1507 handle = little_endian_read_16(packet, 3); 1508 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1509 while (btstack_linked_list_iterator_has_next(&it)){ 1510 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1511 if (channel->con_handle != handle) continue; 1512 l2cap_emit_channel_closed(channel); 1513 btstack_linked_list_iterator_remove(&it); 1514 btstack_memory_l2cap_channel_free(channel); 1515 } 1516 #endif 1517 break; 1518 1519 // HCI Connection Timeouts 1520 #ifdef ENABLE_CLASSIC 1521 case L2CAP_EVENT_TIMEOUT_CHECK: 1522 handle = little_endian_read_16(packet, 2); 1523 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 1524 if (hci_authentication_active_for_handle(handle)) break; 1525 hci_con_used = 0; 1526 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1527 while (btstack_linked_list_iterator_has_next(&it)){ 1528 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1529 if (channel->con_handle != handle) continue; 1530 hci_con_used = 1; 1531 break; 1532 } 1533 if (hci_con_used) break; 1534 if (!hci_can_send_command_packet_now()) break; 1535 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 1536 break; 1537 1538 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 1539 handle = little_endian_read_16(packet, 3); 1540 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1541 while (btstack_linked_list_iterator_has_next(&it)){ 1542 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1543 if (channel->con_handle != handle) continue; 1544 l2cap_handle_remote_supported_features_received(channel); 1545 break; 1546 } 1547 break; 1548 1549 case GAP_EVENT_SECURITY_LEVEL: 1550 handle = little_endian_read_16(packet, 2); 1551 log_info("l2cap - security level update"); 1552 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1553 while (btstack_linked_list_iterator_has_next(&it)){ 1554 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1555 if (channel->con_handle != handle) continue; 1556 1557 log_info("l2cap - state %u", channel->state); 1558 1559 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 1560 gap_security_level_t required_level = channel->required_security_level; 1561 1562 switch (channel->state){ 1563 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1564 if (actual_level >= required_level){ 1565 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1566 // we need to know if ERTM is supported before sending a config response 1567 hci_connection_t * connection = hci_connection_for_handle(channel->con_handle); 1568 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_W2_SEND_EXTENDED_FEATURE_REQUEST; 1569 channel->state = L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES; 1570 #else 1571 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 1572 l2cap_emit_incoming_connection(channel); 1573 #endif 1574 } else { 1575 channel->reason = 0x0003; // security block 1576 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1577 } 1578 break; 1579 1580 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 1581 if (actual_level >= required_level){ 1582 l2cap_ready_to_connect(channel); 1583 } else { 1584 // disconnnect, authentication not good enough 1585 hci_disconnect_security_block(handle); 1586 } 1587 break; 1588 1589 default: 1590 break; 1591 } 1592 } 1593 break; 1594 #endif 1595 1596 default: 1597 break; 1598 } 1599 1600 l2cap_run(); 1601 } 1602 1603 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 1604 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 1605 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 1606 signaling_responses[signaling_responses_pending].handle = handle; 1607 signaling_responses[signaling_responses_pending].code = code; 1608 signaling_responses[signaling_responses_pending].sig_id = sig_id; 1609 signaling_responses[signaling_responses_pending].cid = cid; 1610 signaling_responses[signaling_responses_pending].data = data; 1611 signaling_responses_pending++; 1612 l2cap_run(); 1613 } 1614 } 1615 1616 #ifdef ENABLE_CLASSIC 1617 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 1618 channel->remote_sig_id = identifier; 1619 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 1620 l2cap_run(); 1621 } 1622 1623 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 1624 1625 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 1626 l2cap_service_t *service = l2cap_get_service(psm); 1627 if (!service) { 1628 // 0x0002 PSM not supported 1629 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 1630 return; 1631 } 1632 1633 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 1634 if (!hci_connection) { 1635 // 1636 log_error("no hci_connection for handle %u", handle); 1637 return; 1638 } 1639 1640 // alloc structure 1641 // log_info("l2cap_handle_connection_request register channel"); 1642 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, hci_connection->address, BD_ADDR_TYPE_CLASSIC, 1643 psm, service->mtu, service->required_security_level); 1644 if (!channel){ 1645 // 0x0004 No resources available 1646 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 1647 return; 1648 } 1649 1650 channel->con_handle = handle; 1651 channel->remote_cid = source_cid; 1652 channel->remote_sig_id = sig_id; 1653 1654 // limit local mtu to max acl packet length - l2cap header 1655 if (channel->local_mtu > l2cap_max_mtu()) { 1656 channel->local_mtu = l2cap_max_mtu(); 1657 } 1658 1659 // set initial state 1660 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 1661 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 1662 1663 // add to connections list 1664 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1665 1666 // assert security requirements 1667 gap_request_security_level(handle, channel->required_security_level); 1668 } 1669 1670 void l2cap_accept_connection(uint16_t local_cid){ 1671 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 1672 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1673 if (!channel) { 1674 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1675 return; 1676 } 1677 1678 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1679 // configure L2CAP Basic mode 1680 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1681 #endif 1682 1683 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1684 1685 // process 1686 l2cap_run(); 1687 } 1688 1689 1690 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1691 uint8_t l2cap_accept_ertm_connection(uint16_t local_cid, int ertm_mandatory, uint8_t max_transmit, 1692 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){ 1693 1694 log_info("L2CAP_ACCEPT_ERTM_CONNECTION local_cid 0x%x", local_cid); 1695 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1696 if (!channel) { 1697 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1698 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1699 } 1700 1701 // validate local config 1702 uint8_t result = l2cap_ertm_validate_local_config(max_transmit, retransmission_timeout_ms, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1703 if (result) return result; 1704 1705 // configure L2CAP ERTM 1706 l2cap_ertm_configure_channel(channel, ertm_mandatory, max_transmit, retransmission_timeout_ms, monitor_timeout_ms, num_tx_buffers, num_rx_buffers, buffer, size); 1707 1708 // continue 1709 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1710 1711 // process 1712 l2cap_run(); 1713 1714 return ERROR_CODE_SUCCESS; 1715 } 1716 1717 uint8_t l2cap_ertm_set_busy(uint16_t local_cid){ 1718 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1719 if (!channel) { 1720 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1721 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 1722 } 1723 channel->send_supervisor_frame_receiver_not_ready = 1; 1724 l2cap_run(); 1725 return ERROR_CODE_SUCCESS; 1726 } 1727 1728 uint8_t l2cap_ertm_set_ready(uint16_t local_cid){ 1729 return ERROR_CODE_SUCCESS; 1730 UNUSED(local_cid); 1731 } 1732 1733 static void l2cap_ertm_handle_req_seq(l2cap_channel_t * l2cap_channel, uint8_t req_seq){ 1734 l2cap_ertm_tx_packet_state_t * tx_state; 1735 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index]; 1736 if ( ((req_seq - 1) & 0x3f) == tx_state->tx_seq){ 1737 log_info("RR seq %u == seq of oldest tx packet -> packet done", req_seq); 1738 l2cap_channel->tx_read_index++; 1739 if (l2cap_channel->tx_read_index >= l2cap_channel->num_rx_buffers){ 1740 l2cap_channel->tx_read_index = 0; 1741 } 1742 } else { 1743 log_info("RR seq %u != seq of oldest tx packet %u ???", req_seq, tx_state->tx_seq); 1744 } 1745 } 1746 1747 #endif 1748 1749 1750 void l2cap_decline_connection(uint16_t local_cid){ 1751 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 1752 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1753 if (!channel) { 1754 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1755 return; 1756 } 1757 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1758 channel->reason = 0x04; // no resources available 1759 l2cap_run(); 1760 } 1761 1762 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 1763 1764 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1765 1766 uint16_t flags = little_endian_read_16(command, 6); 1767 if (flags & 1) { 1768 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1769 } 1770 1771 // accept the other's configuration options 1772 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 1773 uint16_t pos = 8; 1774 while (pos < end_pos){ 1775 uint8_t option_hint = command[pos] >> 7; 1776 uint8_t option_type = command[pos] & 0x7f; 1777 log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 1778 pos++; 1779 uint8_t length = command[pos++]; 1780 // MTU { type(8): 1, len(8):2, MTU(16) } 1781 if (option_type == 1 && length == 2){ 1782 channel->remote_mtu = little_endian_read_16(command, pos); 1783 if (channel->remote_mtu > l2cap_max_mtu()){ 1784 log_info("Remote MTU %u larger than outgoing buffer, only using MTU = %u", channel->remote_mtu, l2cap_max_mtu()); 1785 channel->remote_mtu = l2cap_max_mtu(); 1786 } 1787 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1788 } 1789 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 1790 if (option_type == 2 && length == 2){ 1791 channel->flush_timeout = little_endian_read_16(command, pos); 1792 } 1793 1794 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1795 // Retransmission and Flow Control Option 1796 if (option_type == 4 && length == 9){ 1797 l2cap_channel_mode_t mode = (l2cap_channel_mode_t) command[pos]; 1798 switch(channel->mode){ 1799 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 1800 // Store remote config 1801 channel->remote_tx_window_size = command[pos+1]; 1802 channel->remote_max_transmit = command[pos+2]; 1803 channel->remote_retransmission_timeout_ms = little_endian_read_16(command, pos + 3); 1804 channel->remote_monitor_timeout_ms = little_endian_read_16(command, pos + 5); 1805 log_info("FC&C config: tx window: %u, max transmit %u, retrans timeout %u, monitor timeout %u", 1806 channel->remote_tx_window_size, 1807 channel->remote_max_transmit, 1808 channel->remote_retransmission_timeout_ms, 1809 channel->remote_monitor_timeout_ms); 1810 // we store remote MPS in remote_mtu, might need to get re-evaluated 1811 channel->remote_mtu = little_endian_read_16(command, pos + 7); 1812 // If ERTM mandatory, but remote doens't offer ERTM -> disconnect 1813 if (channel->ertm_mandatory && mode != L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 1814 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1815 } else { 1816 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1817 } 1818 break; 1819 case L2CAP_CHANNEL_MODE_BASIC: 1820 switch (mode){ 1821 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 1822 // remote asks for ERTM, but we want basic mode. disconnect if this happens a second time 1823 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED){ 1824 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1825 } 1826 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_BASIC_FALLBACK_TRIED); 1827 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_REJECTED); 1828 break; 1829 default: // case L2CAP_CHANNEL_MODE_BASIC: 1830 // TODO store and evaluate configuration 1831 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1832 break; 1833 } 1834 break; 1835 default: 1836 break; 1837 } 1838 } 1839 #endif 1840 // check for unknown options 1841 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 1842 log_info("l2cap cid %u, unknown options", channel->local_cid); 1843 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 1844 } 1845 pos += length; 1846 } 1847 } 1848 1849 static void l2cap_signaling_handle_configure_response(l2cap_channel_t *channel, uint8_t result, uint8_t *command){ 1850 log_info("l2cap_signaling_handle_configure_response"); 1851 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1852 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 1853 uint16_t pos = 10; 1854 while (pos < end_pos){ 1855 uint8_t option_hint = command[pos] >> 7; 1856 uint8_t option_type = command[pos] & 0x7f; 1857 log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 1858 pos++; 1859 uint8_t length = command[pos++]; 1860 1861 // Retransmission and Flow Control Option 1862 if (option_type == 4 && length == 9){ 1863 switch (channel->mode){ 1864 case L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION: 1865 if (channel->ertm_mandatory){ 1866 // ?? 1867 } else { 1868 // On 'Reject - Unacceptable Parameters' to our optional ERTM request, fall back to BASIC mode 1869 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 1870 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 1871 } 1872 } 1873 break; 1874 case L2CAP_CHANNEL_MODE_BASIC: 1875 if (result == L2CAP_CONF_RESULT_UNACCEPTABLE_PARAMETERS){ 1876 // On 'Reject - Unacceptable Parameters' to our Basic mode request, disconnect 1877 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1878 } 1879 break; 1880 default: 1881 break; 1882 } 1883 } 1884 1885 // check for unknown options 1886 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 1887 log_info("l2cap cid %u, unknown options", channel->local_cid); 1888 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 1889 } 1890 1891 pos += length; 1892 } 1893 #endif 1894 } 1895 1896 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 1897 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 1898 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 1899 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 1900 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 1901 if (channel->state == L2CAP_STATE_OPEN) return 0; 1902 return 1; 1903 } 1904 1905 1906 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 1907 1908 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 1909 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1910 uint16_t result = 0; 1911 1912 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 1913 1914 // handle DISCONNECT REQUESTS seperately 1915 if (code == DISCONNECTION_REQUEST){ 1916 switch (channel->state){ 1917 case L2CAP_STATE_CONFIG: 1918 case L2CAP_STATE_OPEN: 1919 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1920 case L2CAP_STATE_WAIT_DISCONNECT: 1921 l2cap_handle_disconnect_request(channel, identifier); 1922 break; 1923 1924 default: 1925 // ignore in other states 1926 break; 1927 } 1928 return; 1929 } 1930 1931 // @STATEMACHINE(l2cap) 1932 switch (channel->state) { 1933 1934 case L2CAP_STATE_WAIT_CONNECT_RSP: 1935 switch (code){ 1936 case CONNECTION_RESPONSE: 1937 l2cap_stop_rtx(channel); 1938 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 1939 switch (result) { 1940 case 0: 1941 // successful connection 1942 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 1943 channel->state = L2CAP_STATE_CONFIG; 1944 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1945 break; 1946 case 1: 1947 // connection pending. get some coffee, but start the ERTX 1948 l2cap_start_ertx(channel); 1949 break; 1950 default: 1951 // channel closed 1952 channel->state = L2CAP_STATE_CLOSED; 1953 // map l2cap connection response result to BTstack status enumeration 1954 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 1955 1956 // drop link key if security block 1957 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 1958 gap_drop_link_key_for_bd_addr(channel->address); 1959 } 1960 1961 // discard channel 1962 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1963 btstack_memory_l2cap_channel_free(channel); 1964 break; 1965 } 1966 break; 1967 1968 default: 1969 //@TODO: implement other signaling packets 1970 break; 1971 } 1972 break; 1973 1974 case L2CAP_STATE_CONFIG: 1975 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 1976 switch (code) { 1977 case CONFIGURE_REQUEST: 1978 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1979 l2cap_signaling_handle_configure_request(channel, command); 1980 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 1981 // only done if continuation not set 1982 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 1983 } 1984 break; 1985 case CONFIGURE_RESPONSE: 1986 l2cap_stop_rtx(channel); 1987 l2cap_signaling_handle_configure_response(channel, result, command); 1988 switch (result){ 1989 case 0: // success 1990 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 1991 break; 1992 case 4: // pending 1993 l2cap_start_ertx(channel); 1994 break; 1995 default: 1996 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 1997 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION && channel->ertm_mandatory){ 1998 // remote does not offer ertm but it's required 1999 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2000 break; 2001 } 2002 #endif 2003 // retry on negative result 2004 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 2005 break; 2006 } 2007 break; 2008 default: 2009 break; 2010 } 2011 if (l2cap_channel_ready_for_open(channel)){ 2012 // for open: 2013 channel->state = L2CAP_STATE_OPEN; 2014 l2cap_emit_channel_opened(channel, 0); 2015 } 2016 break; 2017 2018 case L2CAP_STATE_WAIT_DISCONNECT: 2019 switch (code) { 2020 case DISCONNECTION_RESPONSE: 2021 l2cap_finialize_channel_close(channel); 2022 break; 2023 default: 2024 //@TODO: implement other signaling packets 2025 break; 2026 } 2027 break; 2028 2029 case L2CAP_STATE_CLOSED: 2030 // @TODO handle incoming requests 2031 break; 2032 2033 case L2CAP_STATE_OPEN: 2034 //@TODO: implement other signaling packets, e.g. re-configure 2035 break; 2036 default: 2037 break; 2038 } 2039 // log_info("new state %u", channel->state); 2040 } 2041 2042 2043 static void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){ 2044 2045 btstack_linked_list_iterator_t it; 2046 2047 // get code, signalind identifier and command len 2048 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2049 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 2050 2051 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_RESPONSE 2052 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_RESPONSE){ 2053 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2054 return; 2055 } 2056 2057 // general commands without an assigned channel 2058 switch(code) { 2059 2060 case CONNECTION_REQUEST: { 2061 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2062 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2063 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 2064 return; 2065 } 2066 2067 case ECHO_REQUEST: 2068 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 2069 return; 2070 2071 case INFORMATION_REQUEST: { 2072 uint16_t infoType = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2073 l2cap_register_signaling_response(handle, code, sig_id, 0, infoType); 2074 return; 2075 } 2076 2077 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2078 case INFORMATION_RESPONSE: { 2079 hci_connection_t * connection = hci_connection_for_handle(handle); 2080 if (!connection) return; 2081 uint16_t info_type = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2082 uint16_t result = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 2083 if (result != 0) return; 2084 if (info_type != 0x02) return; 2085 connection->l2cap_state.information_state = L2CAP_INFORMATION_STATE_DONE; 2086 connection->l2cap_state.extended_feature_mask = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 2087 log_info("extended features mask 0x%02x", connection->l2cap_state.extended_feature_mask); 2088 // trigger connection request 2089 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2090 while (btstack_linked_list_iterator_has_next(&it)){ 2091 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2092 if (channel->con_handle != handle) continue; 2093 // bail if ERTM was requested but is not supported 2094 if ((channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION) && ((connection->l2cap_state.extended_feature_mask & 0x08) == 0)){ 2095 if (channel->ertm_mandatory){ 2096 // channel closed 2097 channel->state = L2CAP_STATE_CLOSED; 2098 // map l2cap connection response result to BTstack status enumeration 2099 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_ERTM_NOT_SUPPORTD); 2100 // discard channel 2101 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2102 btstack_memory_l2cap_channel_free(channel); 2103 continue; 2104 } else { 2105 // fallback to Basic mode 2106 channel->mode = L2CAP_CHANNEL_MODE_BASIC; 2107 } 2108 } 2109 // start connecting 2110 if (channel->state == L2CAP_STATE_WAIT_OUTGOING_EXTENDED_FEATURES){ 2111 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 2112 } 2113 // respond to connection request 2114 if (channel->state == L2CAP_STATE_WAIT_INCOMING_EXTENDED_FEATURES){ 2115 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2116 l2cap_emit_incoming_connection(channel); 2117 } 2118 } 2119 return; 2120 } 2121 #endif 2122 2123 default: 2124 break; 2125 } 2126 2127 2128 // Get potential destination CID 2129 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 2130 2131 // Find channel for this sig_id and connection handle 2132 btstack_linked_list_iterator_init(&it, &l2cap_channels); 2133 while (btstack_linked_list_iterator_has_next(&it)){ 2134 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2135 if (channel->con_handle != handle) continue; 2136 if (code & 1) { 2137 // match odd commands (responses) by previous signaling identifier 2138 if (channel->local_sig_id == sig_id) { 2139 l2cap_signaling_handler_channel(channel, command); 2140 break; 2141 } 2142 } else { 2143 // match even commands (requests) by local channel id 2144 if (channel->local_cid == dest_cid) { 2145 l2cap_signaling_handler_channel(channel, command); 2146 break; 2147 } 2148 } 2149 } 2150 } 2151 #endif 2152 2153 #ifdef ENABLE_BLE 2154 2155 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 2156 uint8_t event[6]; 2157 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 2158 event[1] = 4; 2159 little_endian_store_16(event, 2, con_handle); 2160 little_endian_store_16(event, 4, result); 2161 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2162 if (!l2cap_event_packet_handler) return; 2163 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 2164 } 2165 2166 // @returns valid 2167 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 2168 hci_connection_t * connection; 2169 uint16_t result; 2170 uint8_t event[10]; 2171 2172 #ifdef ENABLE_LE_DATA_CHANNELS 2173 btstack_linked_list_iterator_t it; 2174 l2cap_channel_t * channel; 2175 uint16_t local_cid; 2176 uint16_t le_psm; 2177 uint16_t new_credits; 2178 uint16_t credits_before; 2179 l2cap_service_t * service; 2180 uint16_t source_cid; 2181 #endif 2182 2183 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 2184 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u", code, sig_id); 2185 2186 switch (code){ 2187 2188 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 2189 result = little_endian_read_16(command, 4); 2190 l2cap_emit_connection_parameter_update_response(handle, result); 2191 break; 2192 2193 case CONNECTION_PARAMETER_UPDATE_REQUEST: 2194 connection = hci_connection_for_handle(handle); 2195 if (connection){ 2196 if (connection->role != HCI_ROLE_MASTER){ 2197 // reject command without notifying upper layer when not in master role 2198 return 0; 2199 } 2200 int update_parameter = 1; 2201 le_connection_parameter_range_t existing_range; 2202 gap_get_connection_parameter_range(&existing_range); 2203 uint16_t le_conn_interval_min = little_endian_read_16(command,8); 2204 uint16_t le_conn_interval_max = little_endian_read_16(command,10); 2205 uint16_t le_conn_latency = little_endian_read_16(command,12); 2206 uint16_t le_supervision_timeout = little_endian_read_16(command,14); 2207 2208 if (le_conn_interval_min < existing_range.le_conn_interval_min) update_parameter = 0; 2209 if (le_conn_interval_max > existing_range.le_conn_interval_max) update_parameter = 0; 2210 2211 if (le_conn_latency < existing_range.le_conn_latency_min) update_parameter = 0; 2212 if (le_conn_latency > existing_range.le_conn_latency_max) update_parameter = 0; 2213 2214 if (le_supervision_timeout < existing_range.le_supervision_timeout_min) update_parameter = 0; 2215 if (le_supervision_timeout > existing_range.le_supervision_timeout_max) update_parameter = 0; 2216 2217 if (update_parameter){ 2218 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 2219 connection->le_conn_interval_min = le_conn_interval_min; 2220 connection->le_conn_interval_max = le_conn_interval_max; 2221 connection->le_conn_latency = le_conn_latency; 2222 connection->le_supervision_timeout = le_supervision_timeout; 2223 } else { 2224 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 2225 } 2226 connection->le_con_param_update_identifier = sig_id; 2227 } 2228 2229 if (!l2cap_event_packet_handler) break; 2230 2231 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 2232 event[1] = 8; 2233 memcpy(&event[2], &command[4], 8); 2234 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2235 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2236 break; 2237 2238 #ifdef ENABLE_LE_DATA_CHANNELS 2239 2240 case COMMAND_REJECT: 2241 // Find channel for this sig_id and connection handle 2242 channel = NULL; 2243 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2244 while (btstack_linked_list_iterator_has_next(&it)){ 2245 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2246 if (a_channel->con_handle != handle) continue; 2247 if (a_channel->local_sig_id != sig_id) continue; 2248 channel = a_channel; 2249 break; 2250 } 2251 if (!channel) break; 2252 2253 // if received while waiting for le connection response, assume legacy device 2254 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 2255 channel->state = L2CAP_STATE_CLOSED; 2256 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 2257 l2cap_emit_le_channel_opened(channel, 0x0002); 2258 2259 // discard channel 2260 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2261 btstack_memory_l2cap_channel_free(channel); 2262 break; 2263 } 2264 break; 2265 2266 case LE_CREDIT_BASED_CONNECTION_REQUEST: 2267 2268 // get hci connection, bail if not found (must not happen) 2269 connection = hci_connection_for_handle(handle); 2270 if (!connection) return 0; 2271 2272 // check if service registered 2273 le_psm = little_endian_read_16(command, 4); 2274 service = l2cap_le_get_service(le_psm); 2275 source_cid = little_endian_read_16(command, 6); 2276 2277 if (service){ 2278 if (source_cid < 0x40){ 2279 // 0x0009 Connection refused - Invalid Source CID 2280 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 2281 return 1; 2282 } 2283 2284 // go through list of channels for this ACL connection and check if we get a match 2285 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2286 while (btstack_linked_list_iterator_has_next(&it)){ 2287 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2288 if (a_channel->con_handle != handle) continue; 2289 if (a_channel->remote_cid != source_cid) continue; 2290 // 0x000a Connection refused - Source CID already allocated 2291 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 2292 return 1; 2293 } 2294 2295 // security: check encryption 2296 if (service->required_security_level >= LEVEL_2){ 2297 if (sm_encryption_key_size(handle) == 0){ 2298 // 0x0008 Connection refused - insufficient encryption 2299 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 2300 return 1; 2301 } 2302 // anything less than 16 byte key size is insufficient 2303 if (sm_encryption_key_size(handle) < 16){ 2304 // 0x0007 Connection refused – insufficient encryption key size 2305 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 2306 return 1; 2307 } 2308 } 2309 2310 // security: check authencation 2311 if (service->required_security_level >= LEVEL_3){ 2312 if (!sm_authenticated(handle)){ 2313 // 0x0005 Connection refused – insufficient authentication 2314 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 2315 return 1; 2316 } 2317 } 2318 2319 // security: check authorization 2320 if (service->required_security_level >= LEVEL_4){ 2321 if (sm_authorization_state(handle) != AUTHORIZATION_GRANTED){ 2322 // 0x0006 Connection refused – insufficient authorization 2323 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 2324 return 1; 2325 } 2326 } 2327 2328 // allocate channel 2329 channel = l2cap_create_channel_entry(service->packet_handler, connection->address, 2330 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 2331 if (!channel){ 2332 // 0x0004 Connection refused – no resources available 2333 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 2334 return 1; 2335 } 2336 2337 channel->con_handle = handle; 2338 channel->remote_cid = source_cid; 2339 channel->remote_sig_id = sig_id; 2340 channel->remote_mtu = little_endian_read_16(command, 8); 2341 channel->remote_mps = little_endian_read_16(command, 10); 2342 channel->credits_outgoing = little_endian_read_16(command, 12); 2343 2344 // set initial state 2345 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 2346 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 2347 2348 // add to connections list 2349 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2350 2351 // post connection request event 2352 l2cap_emit_le_incoming_connection(channel); 2353 2354 } else { 2355 // Connection refused – LE_PSM not supported 2356 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 2357 } 2358 break; 2359 2360 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 2361 // Find channel for this sig_id and connection handle 2362 channel = NULL; 2363 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2364 while (btstack_linked_list_iterator_has_next(&it)){ 2365 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2366 if (a_channel->con_handle != handle) continue; 2367 if (a_channel->local_sig_id != sig_id) continue; 2368 channel = a_channel; 2369 break; 2370 } 2371 if (!channel) break; 2372 2373 // cid + 0 2374 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 2375 if (result){ 2376 channel->state = L2CAP_STATE_CLOSED; 2377 // map l2cap connection response result to BTstack status enumeration 2378 l2cap_emit_le_channel_opened(channel, result); 2379 2380 // discard channel 2381 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2382 btstack_memory_l2cap_channel_free(channel); 2383 break; 2384 } 2385 2386 // success 2387 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2388 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2389 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 2390 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 2391 channel->state = L2CAP_STATE_OPEN; 2392 l2cap_emit_le_channel_opened(channel, result); 2393 break; 2394 2395 case LE_FLOW_CONTROL_CREDIT: 2396 // find channel 2397 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2398 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2399 if (!channel) { 2400 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2401 break; 2402 } 2403 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 2404 credits_before = channel->credits_outgoing; 2405 channel->credits_outgoing += new_credits; 2406 // check for credit overrun 2407 if (credits_before > channel->credits_outgoing){ 2408 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 2409 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2410 break; 2411 } 2412 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 2413 break; 2414 2415 case DISCONNECTION_REQUEST: 2416 // find channel 2417 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 2418 channel = l2cap_le_get_channel_for_local_cid(local_cid); 2419 if (!channel) { 2420 log_error("l2cap: no channel for cid 0x%02x", local_cid); 2421 break; 2422 } 2423 channel->remote_sig_id = sig_id; 2424 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 2425 break; 2426 2427 #endif 2428 2429 case DISCONNECTION_RESPONSE: 2430 break; 2431 2432 default: 2433 // command unknown -> reject command 2434 return 0; 2435 } 2436 return 1; 2437 } 2438 #endif 2439 2440 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size ){ 2441 UNUSED(packet_type); 2442 UNUSED(channel); 2443 2444 l2cap_channel_t * l2cap_channel; 2445 UNUSED(l2cap_channel); 2446 2447 // Get Channel ID 2448 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 2449 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 2450 2451 switch (channel_id) { 2452 2453 #ifdef ENABLE_CLASSIC 2454 case L2CAP_CID_SIGNALING: { 2455 uint16_t command_offset = 8; 2456 while (command_offset < size) { 2457 // handle signaling commands 2458 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 2459 2460 // increment command_offset 2461 command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 2462 } 2463 break; 2464 } 2465 #endif 2466 2467 #ifdef ENABLE_BLE 2468 case L2CAP_CID_SIGNALING_LE: { 2469 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 2470 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 2471 if (!valid){ 2472 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 2473 } 2474 break; 2475 } 2476 #endif 2477 2478 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 2479 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback) { 2480 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2481 } 2482 break; 2483 2484 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 2485 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback) { 2486 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2487 } 2488 break; 2489 2490 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 2491 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback) { 2492 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2493 } 2494 break; 2495 2496 default: 2497 #ifdef ENABLE_CLASSIC 2498 // Find channel for this channel_id and connection handle 2499 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 2500 if (l2cap_channel) { 2501 #ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE 2502 if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){ 2503 2504 // verify FCS 2505 uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2)); 2506 uint16_t fcs_packet = little_endian_read_16(packet, size-2); 2507 log_info("Packet FCS 0x%04x, calculated FCS 0x%04x", fcs_packet, fcs_calculated); 2508 if (fcs_calculated != fcs_packet){ 2509 log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated); 2510 // TODO: trigger retransmission or something like that 2511 break; 2512 } 2513 2514 // switch on packet type 2515 uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2516 uint8_t req_seq = (control >> 8) & 0x3f; 2517 if (control & 1){ 2518 // S-Frame 2519 // int poll = (control >> 7) & 0x01; 2520 l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03); 2521 switch (s){ 2522 case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY: 2523 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2524 break; 2525 default: 2526 break; 2527 } 2528 break; 2529 } else { 2530 // I-Frame 2531 // get control 2532 l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14); 2533 uint8_t tx_seq = (control >> 1) & 0x3f; 2534 log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq); 2535 log_info("SAR: pos %u", l2cap_channel->rx_packets_state->pos); 2536 log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq); 2537 l2cap_ertm_handle_req_seq(l2cap_channel, req_seq); 2538 // check ordering 2539 if (l2cap_channel->expected_tx_seq == tx_seq){ 2540 log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq); 2541 l2cap_channel->req_seq = tx_seq; 2542 l2cap_channel->send_supervisor_frame_receiver_ready = 1; 2543 l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq); 2544 uint16_t sdu_length; 2545 uint16_t segment_length; 2546 uint16_t payload_offset; 2547 switch (sar){ 2548 case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU: 2549 payload_offset = COMPLETE_L2CAP_HEADER+2; 2550 segment_length = payload_offset-2; 2551 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER+2], segment_length); 2552 break; 2553 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU: 2554 // TODO: use current packet 2555 // TODO: check if reassembly started 2556 // TODO: check len against local mtu 2557 sdu_length = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER+2); 2558 payload_offset = COMPLETE_L2CAP_HEADER+4; 2559 segment_length = size - payload_offset-2; 2560 memcpy(&l2cap_channel->rx_packets_data[0], &packet[payload_offset], segment_length); 2561 l2cap_channel->rx_packets_state->sdu_length = sdu_length; 2562 l2cap_channel->rx_packets_state->pos = segment_length; 2563 break; 2564 case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU: 2565 payload_offset = COMPLETE_L2CAP_HEADER+2; 2566 segment_length = size - payload_offset-2; 2567 memcpy(&l2cap_channel->rx_packets_data[l2cap_channel->rx_packets_state->pos], &packet[payload_offset], segment_length); 2568 l2cap_channel->rx_packets_state->pos += segment_length; 2569 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->rx_packets_data, l2cap_channel->rx_packets_state[0].pos); 2570 break; 2571 case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU: 2572 payload_offset = COMPLETE_L2CAP_HEADER+2; 2573 segment_length = size - payload_offset-2; 2574 memcpy(&l2cap_channel->rx_packets_data[l2cap_channel->rx_packets_state->pos], &packet[payload_offset], segment_length); 2575 l2cap_channel->rx_packets_state->pos += segment_length; 2576 break; 2577 } 2578 } 2579 } 2580 break; 2581 } 2582 #endif 2583 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 2584 } 2585 #endif 2586 #ifdef ENABLE_LE_DATA_CHANNELS 2587 l2cap_channel = l2cap_le_get_channel_for_local_cid(channel_id); 2588 if (l2cap_channel) { 2589 // credit counting 2590 if (l2cap_channel->credits_incoming == 0){ 2591 log_error("LE Data Channel packet received but no incoming credits"); 2592 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2593 break; 2594 } 2595 l2cap_channel->credits_incoming--; 2596 2597 // automatic credits 2598 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 2599 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 2600 } 2601 2602 // first fragment 2603 uint16_t pos = 0; 2604 if (!l2cap_channel->receive_sdu_len){ 2605 l2cap_channel->receive_sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 2606 l2cap_channel->receive_sdu_pos = 0; 2607 pos += 2; 2608 size -= 2; 2609 } 2610 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], size-COMPLETE_L2CAP_HEADER); 2611 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 2612 // done? 2613 log_info("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 2614 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 2615 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 2616 l2cap_channel->receive_sdu_len = 0; 2617 } 2618 } else { 2619 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 2620 } 2621 #endif 2622 break; 2623 } 2624 2625 l2cap_run(); 2626 } 2627 2628 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 2629 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 2630 int index = l2cap_fixed_channel_table_index_for_channel_id(channel_id); 2631 if (index < 0) return; 2632 fixed_channels[index].callback = the_packet_handler; 2633 } 2634 2635 #ifdef ENABLE_CLASSIC 2636 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 2637 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 2638 channel->state = L2CAP_STATE_CLOSED; 2639 l2cap_emit_channel_closed(channel); 2640 // discard channel 2641 l2cap_stop_rtx(channel); 2642 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 2643 btstack_memory_l2cap_channel_free(channel); 2644 } 2645 2646 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 2647 btstack_linked_list_iterator_t it; 2648 btstack_linked_list_iterator_init(&it, services); 2649 while (btstack_linked_list_iterator_has_next(&it)){ 2650 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 2651 if ( service->psm == psm){ 2652 return service; 2653 }; 2654 } 2655 return NULL; 2656 } 2657 2658 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 2659 return l2cap_get_service_internal(&l2cap_services, psm); 2660 } 2661 2662 2663 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 2664 2665 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 2666 2667 // check for alread registered psm 2668 l2cap_service_t *service = l2cap_get_service(psm); 2669 if (service) { 2670 log_error("l2cap_register_service: PSM %u already registered", psm); 2671 return L2CAP_SERVICE_ALREADY_REGISTERED; 2672 } 2673 2674 // alloc structure 2675 service = btstack_memory_l2cap_service_get(); 2676 if (!service) { 2677 log_error("l2cap_register_service: no memory for l2cap_service_t"); 2678 return BTSTACK_MEMORY_ALLOC_FAILED; 2679 } 2680 2681 // fill in 2682 service->psm = psm; 2683 service->mtu = mtu; 2684 service->packet_handler = service_packet_handler; 2685 service->required_security_level = security_level; 2686 2687 // add to services list 2688 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 2689 2690 // enable page scan 2691 gap_connectable_control(1); 2692 2693 return 0; 2694 } 2695 2696 uint8_t l2cap_unregister_service(uint16_t psm){ 2697 2698 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 2699 2700 l2cap_service_t *service = l2cap_get_service(psm); 2701 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 2702 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 2703 btstack_memory_l2cap_service_free(service); 2704 2705 // disable page scan when no services registered 2706 if (btstack_linked_list_empty(&l2cap_services)) { 2707 gap_connectable_control(0); 2708 } 2709 return 0; 2710 } 2711 #endif 2712 2713 2714 #ifdef ENABLE_LE_DATA_CHANNELS 2715 2716 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 2717 if (!channel->waiting_for_can_send_now) return; 2718 if (channel->send_sdu_buffer) return; 2719 channel->waiting_for_can_send_now = 0; 2720 log_info("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 2721 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 2722 } 2723 2724 // 1BH2222 2725 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 2726 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", 2727 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 2728 uint8_t event[19]; 2729 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 2730 event[1] = sizeof(event) - 2; 2731 event[2] = channel->address_type; 2732 reverse_bd_addr(channel->address, &event[3]); 2733 little_endian_store_16(event, 9, channel->con_handle); 2734 little_endian_store_16(event, 11, channel->psm); 2735 little_endian_store_16(event, 13, channel->local_cid); 2736 little_endian_store_16(event, 15, channel->remote_cid); 2737 little_endian_store_16(event, 17, channel->remote_mtu); 2738 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2739 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 2740 } 2741 // 11BH22222 2742 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 2743 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", 2744 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 2745 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 2746 uint8_t event[23]; 2747 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 2748 event[1] = sizeof(event) - 2; 2749 event[2] = status; 2750 event[3] = channel->address_type; 2751 reverse_bd_addr(channel->address, &event[4]); 2752 little_endian_store_16(event, 10, channel->con_handle); 2753 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 2754 little_endian_store_16(event, 13, channel->psm); 2755 little_endian_store_16(event, 15, channel->local_cid); 2756 little_endian_store_16(event, 17, channel->remote_cid); 2757 little_endian_store_16(event, 19, channel->local_mtu); 2758 little_endian_store_16(event, 21, channel->remote_mtu); 2759 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2760 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 2761 } 2762 2763 static l2cap_channel_t * l2cap_le_get_channel_for_local_cid(uint16_t local_cid){ 2764 btstack_linked_list_iterator_t it; 2765 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2766 while (btstack_linked_list_iterator_has_next(&it)){ 2767 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2768 if ( channel->local_cid == local_cid) { 2769 return channel; 2770 } 2771 } 2772 return NULL; 2773 } 2774 2775 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 2776 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 2777 channel->state = L2CAP_STATE_CLOSED; 2778 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 2779 // discard channel 2780 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2781 btstack_memory_l2cap_channel_free(channel); 2782 } 2783 2784 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 2785 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 2786 } 2787 2788 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 2789 2790 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 2791 2792 // check for alread registered psm 2793 l2cap_service_t *service = l2cap_le_get_service(psm); 2794 if (service) { 2795 return L2CAP_SERVICE_ALREADY_REGISTERED; 2796 } 2797 2798 // alloc structure 2799 service = btstack_memory_l2cap_service_get(); 2800 if (!service) { 2801 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 2802 return BTSTACK_MEMORY_ALLOC_FAILED; 2803 } 2804 2805 // fill in 2806 service->psm = psm; 2807 service->mtu = 0; 2808 service->packet_handler = packet_handler; 2809 service->required_security_level = security_level; 2810 2811 // add to services list 2812 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 2813 2814 // done 2815 return 0; 2816 } 2817 2818 uint8_t l2cap_le_unregister_service(uint16_t psm) { 2819 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 2820 l2cap_service_t *service = l2cap_le_get_service(psm); 2821 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 2822 2823 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 2824 btstack_memory_l2cap_service_free(service); 2825 return 0; 2826 } 2827 2828 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 2829 // get channel 2830 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2831 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2832 2833 // validate state 2834 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 2835 return ERROR_CODE_COMMAND_DISALLOWED; 2836 } 2837 2838 // set state accept connection 2839 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 2840 channel->receive_sdu_buffer = receive_sdu_buffer; 2841 channel->local_mtu = mtu; 2842 channel->new_credits_incoming = initial_credits; 2843 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 2844 2845 // test 2846 // channel->new_credits_incoming = 1; 2847 2848 // go 2849 l2cap_run(); 2850 return 0; 2851 } 2852 2853 /** 2854 * @brief Deny incoming LE Data Channel connection due to resource constraints 2855 * @param local_cid L2CAP LE Data Channel Identifier 2856 */ 2857 2858 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 2859 // get channel 2860 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2861 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2862 2863 // validate state 2864 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 2865 return ERROR_CODE_COMMAND_DISALLOWED; 2866 } 2867 2868 // set state decline connection 2869 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 2870 channel->reason = 0x04; // no resources available 2871 l2cap_run(); 2872 return 0; 2873 } 2874 2875 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 2876 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 2877 uint16_t * out_local_cid) { 2878 2879 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 2880 2881 2882 hci_connection_t * connection = hci_connection_for_handle(con_handle); 2883 if (!connection) { 2884 log_error("no hci_connection for handle 0x%04x", con_handle); 2885 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 2886 } 2887 2888 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, connection->address, connection->address_type, psm, mtu, security_level); 2889 if (!channel) { 2890 return BTSTACK_MEMORY_ALLOC_FAILED; 2891 } 2892 log_info("l2cap_le_create_channel %p", channel); 2893 2894 // store local_cid 2895 if (out_local_cid){ 2896 *out_local_cid = channel->local_cid; 2897 } 2898 2899 // provide buffer 2900 channel->con_handle = con_handle; 2901 channel->receive_sdu_buffer = receive_sdu_buffer; 2902 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 2903 channel->new_credits_incoming = initial_credits; 2904 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 2905 2906 // add to connections list 2907 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2908 2909 // go 2910 l2cap_run(); 2911 return 0; 2912 } 2913 2914 /** 2915 * @brief Provide credtis for LE Data Channel 2916 * @param local_cid L2CAP LE Data Channel Identifier 2917 * @param credits Number additional credits for peer 2918 */ 2919 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 2920 2921 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2922 if (!channel) { 2923 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 2924 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2925 } 2926 2927 // check state 2928 if (channel->state != L2CAP_STATE_OPEN){ 2929 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 2930 } 2931 2932 // assert incoming credits + credits <= 0xffff 2933 uint32_t total_credits = channel->credits_incoming; 2934 total_credits += channel->new_credits_incoming; 2935 total_credits += credits; 2936 if (total_credits > 0xffff){ 2937 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 2938 channel->new_credits_incoming, credits); 2939 } 2940 2941 // set credits_granted 2942 channel->new_credits_incoming += credits; 2943 2944 // go 2945 l2cap_run(); 2946 return 0; 2947 } 2948 2949 /** 2950 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 2951 * @param local_cid L2CAP LE Data Channel Identifier 2952 */ 2953 int l2cap_le_can_send_now(uint16_t local_cid){ 2954 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2955 if (!channel) { 2956 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 2957 return 0; 2958 } 2959 2960 // check state 2961 if (channel->state != L2CAP_STATE_OPEN) return 0; 2962 2963 // check queue 2964 if (channel->send_sdu_buffer) return 0; 2965 2966 // fine, go ahead 2967 return 1; 2968 } 2969 2970 /** 2971 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 2972 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 2973 * so packet handler should be ready to handle it 2974 * @param local_cid L2CAP LE Data Channel Identifier 2975 */ 2976 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 2977 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2978 if (!channel) { 2979 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 2980 return 0; 2981 } 2982 channel->waiting_for_can_send_now = 1; 2983 l2cap_le_notify_channel_can_send(channel); 2984 return 0; 2985 } 2986 2987 /** 2988 * @brief Send data via LE Data Channel 2989 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 2990 * @param local_cid L2CAP LE Data Channel Identifier 2991 * @param data data to send 2992 * @param size data size 2993 */ 2994 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 2995 2996 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2997 if (!channel) { 2998 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 2999 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3000 } 3001 3002 if (len > channel->remote_mtu){ 3003 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 3004 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 3005 } 3006 3007 if (channel->send_sdu_buffer){ 3008 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 3009 return BTSTACK_ACL_BUFFERS_FULL; 3010 } 3011 3012 channel->send_sdu_buffer = data; 3013 channel->send_sdu_len = len; 3014 channel->send_sdu_pos = 0; 3015 3016 l2cap_run(); 3017 return 0; 3018 } 3019 3020 /** 3021 * @brief Disconnect from LE Data Channel 3022 * @param local_cid L2CAP LE Data Channel Identifier 3023 */ 3024 uint8_t l2cap_le_disconnect(uint16_t local_cid) 3025 { 3026 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 3027 if (!channel) { 3028 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 3029 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 3030 } 3031 3032 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 3033 l2cap_run(); 3034 return 0; 3035 } 3036 3037 #endif 3038