1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define __BTSTACK_FILE__ "l2cap.c" 39 40 /* 41 * l2cap.c 42 * 43 * Logical Link Control and Adaption Protocl (L2CAP) 44 * 45 * Created by Matthias Ringwald on 5/16/09. 46 */ 47 48 #include "l2cap.h" 49 #include "hci.h" 50 #include "hci_dump.h" 51 #include "bluetooth_sdp.h" 52 #include "btstack_debug.h" 53 #include "btstack_event.h" 54 #include "btstack_memory.h" 55 56 #ifdef ENABLE_LE_DATA_CHANNELS 57 #include "ble/sm.h" 58 #endif 59 60 #include <stdarg.h> 61 #include <string.h> 62 63 #include <stdio.h> 64 65 // nr of buffered acl packets in outgoing queue to get max performance 66 #define NR_BUFFERED_ACL_PACKETS 3 67 68 // used to cache l2cap rejects, echo, and informational requests 69 #define NR_PENDING_SIGNALING_RESPONSES 3 70 71 // nr of credits provided to remote if credits fall below watermark 72 #define L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK 5 73 #define L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT 5 74 75 // offsets for L2CAP SIGNALING COMMANDS 76 #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET 0 77 #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET 1 78 #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2 79 #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET 4 80 81 // internal table 82 #define L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL 0 83 #define L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL 1 84 #define L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL 2 85 #define L2CAP_FIXED_CHANNEL_TABLE_SIZE (L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL+1) 86 87 #if defined(ENABLE_LE_DATA_CHANNELS) || defined(ENABLE_CLASSIC) 88 #define L2CAP_USES_CHANNELS 89 #endif 90 91 // prototypes 92 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 93 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size ); 94 static void l2cap_notify_channel_can_send(void); 95 static void l2cap_emit_can_send_now(btstack_packet_handler_t packet_handler, uint16_t channel); 96 #ifdef ENABLE_CLASSIC 97 static void l2cap_finialize_channel_close(l2cap_channel_t *channel); 98 static inline l2cap_service_t * l2cap_get_service(uint16_t psm); 99 static void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status); 100 static void l2cap_emit_channel_closed(l2cap_channel_t *channel); 101 static void l2cap_emit_incoming_connection(l2cap_channel_t *channel); 102 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel); 103 #endif 104 #ifdef ENABLE_LE_DATA_CHANNELS 105 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status); 106 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel); 107 static l2cap_channel_t * l2cap_le_get_channel_for_local_cid(uint16_t local_cid); 108 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel); 109 static void l2cap_le_finialize_channel_close(l2cap_channel_t *channel); 110 static inline l2cap_service_t * l2cap_le_get_service(uint16_t psm); 111 #endif 112 113 typedef struct l2cap_fixed_channel { 114 btstack_packet_handler_t callback; 115 uint8_t waiting_for_can_send_now; 116 } l2cap_fixed_channel_t; 117 118 #ifdef ENABLE_CLASSIC 119 static btstack_linked_list_t l2cap_channels; 120 static btstack_linked_list_t l2cap_services; 121 static uint8_t require_security_level2_for_outgoing_sdp; 122 #endif 123 124 #ifdef ENABLE_LE_DATA_CHANNELS 125 static btstack_linked_list_t l2cap_le_channels; 126 static btstack_linked_list_t l2cap_le_services; 127 #endif 128 129 // used to cache l2cap rejects, echo, and informational requests 130 static l2cap_signaling_response_t signaling_responses[NR_PENDING_SIGNALING_RESPONSES]; 131 static int signaling_responses_pending; 132 133 static btstack_packet_callback_registration_t hci_event_callback_registration; 134 135 static btstack_packet_handler_t l2cap_event_packet_handler; 136 static l2cap_fixed_channel_t fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_SIZE]; 137 138 static uint16_t l2cap_fixed_channel_table_channel_id_for_index(int index){ 139 switch (index){ 140 case L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL: 141 return L2CAP_CID_ATTRIBUTE_PROTOCOL; 142 case L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL: 143 return L2CAP_CID_SECURITY_MANAGER_PROTOCOL; 144 case L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL: 145 return L2CAP_CID_CONNECTIONLESS_CHANNEL; 146 default: 147 return 0; 148 } 149 } 150 static int l2cap_fixed_channel_table_index_for_channel_id(uint16_t channel_id){ 151 switch (channel_id){ 152 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 153 return L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL; 154 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 155 return L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL; 156 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 157 return L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL; 158 default: 159 return -1; 160 } 161 } 162 163 static int l2cap_fixed_channel_table_index_is_le(int index){ 164 if (index == L2CAP_CID_CONNECTIONLESS_CHANNEL) return 0; 165 return 1; 166 } 167 168 void l2cap_init(void){ 169 signaling_responses_pending = 0; 170 171 #ifdef ENABLE_CLASSIC 172 l2cap_channels = NULL; 173 l2cap_services = NULL; 174 require_security_level2_for_outgoing_sdp = 0; 175 #endif 176 177 #ifdef ENABLE_LE_DATA_CHANNELS 178 l2cap_le_services = NULL; 179 l2cap_le_channels = NULL; 180 #endif 181 182 l2cap_event_packet_handler = NULL; 183 memset(fixed_channels, 0, sizeof(fixed_channels)); 184 185 // 186 // register callback with HCI 187 // 188 hci_event_callback_registration.callback = &l2cap_hci_event_handler; 189 hci_add_event_handler(&hci_event_callback_registration); 190 191 hci_register_acl_packet_handler(&l2cap_acl_handler); 192 193 #ifdef ENABLE_CLASSIC 194 gap_connectable_control(0); // no services yet 195 #endif 196 } 197 198 void l2cap_register_packet_handler(void (*handler)(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 199 l2cap_event_packet_handler = handler; 200 } 201 202 void l2cap_request_can_send_fix_channel_now_event(hci_con_handle_t con_handle, uint16_t channel_id){ 203 UNUSED(con_handle); 204 205 int index = l2cap_fixed_channel_table_index_for_channel_id(channel_id); 206 if (index < 0) return; 207 fixed_channels[index].waiting_for_can_send_now = 1; 208 l2cap_notify_channel_can_send(); 209 } 210 211 int l2cap_can_send_fixed_channel_packet_now(hci_con_handle_t con_handle, uint16_t channel_id){ 212 UNUSED(channel_id); 213 214 return hci_can_send_acl_packet_now(con_handle); 215 } 216 217 uint8_t *l2cap_get_outgoing_buffer(void){ 218 return hci_get_outgoing_packet_buffer() + COMPLETE_L2CAP_HEADER; // 8 bytes 219 } 220 221 int l2cap_reserve_packet_buffer(void){ 222 return hci_reserve_packet_buffer(); 223 } 224 225 void l2cap_release_packet_buffer(void){ 226 hci_release_packet_buffer(); 227 } 228 229 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){ 230 // 0 - Connection handle : PB=pb : BC=00 231 little_endian_store_16(acl_buffer, 0, con_handle | (packet_boundary << 12) | (0 << 14)); 232 // 2 - ACL length 233 little_endian_store_16(acl_buffer, 2, len + 4); 234 // 4 - L2CAP packet length 235 little_endian_store_16(acl_buffer, 4, len + 0); 236 // 6 - L2CAP channel DEST 237 little_endian_store_16(acl_buffer, 6, remote_cid); 238 } 239 240 // assumption - only on LE connections 241 int l2cap_send_prepared_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint16_t len){ 242 243 if (!hci_is_packet_buffer_reserved()){ 244 log_error("l2cap_send_prepared_connectionless called without reserving packet first"); 245 return BTSTACK_ACL_BUFFERS_FULL; 246 } 247 248 if (!hci_can_send_prepared_acl_packet_now(con_handle)){ 249 log_info("l2cap_send_prepared_connectionless handle 0x%02x, cid 0x%02x, cannot send", con_handle, cid); 250 return BTSTACK_ACL_BUFFERS_FULL; 251 } 252 253 log_debug("l2cap_send_prepared_connectionless handle %u, cid 0x%02x", con_handle, cid); 254 255 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 256 l2cap_setup_header(acl_buffer, con_handle, 0, cid, len); 257 // send 258 return hci_send_acl_packet_buffer(len+8); 259 } 260 261 // assumption - only on LE connections 262 int l2cap_send_connectionless(hci_con_handle_t con_handle, uint16_t cid, uint8_t *data, uint16_t len){ 263 264 if (!hci_can_send_acl_packet_now(con_handle)){ 265 log_info("l2cap_send cid 0x%02x, cannot send", cid); 266 return BTSTACK_ACL_BUFFERS_FULL; 267 } 268 269 hci_reserve_packet_buffer(); 270 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 271 272 memcpy(&acl_buffer[8], data, len); 273 274 return l2cap_send_prepared_connectionless(con_handle, cid, len); 275 } 276 277 static void l2cap_emit_can_send_now(btstack_packet_handler_t packet_handler, uint16_t channel) { 278 log_debug("L2CAP_EVENT_CHANNEL_CAN_SEND_NOW local_cid 0x%x", channel); 279 uint8_t event[4]; 280 event[0] = L2CAP_EVENT_CAN_SEND_NOW; 281 event[1] = sizeof(event) - 2; 282 little_endian_store_16(event, 2, channel); 283 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 284 packet_handler(HCI_EVENT_PACKET, channel, event, sizeof(event)); 285 } 286 287 #ifdef L2CAP_USES_CHANNELS 288 static void l2cap_dispatch_to_channel(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){ 289 (* (channel->packet_handler))(type, channel->local_cid, data, size); 290 } 291 292 static void l2cap_emit_simple_event_with_cid(l2cap_channel_t * channel, uint8_t event_code){ 293 uint8_t event[4]; 294 event[0] = event_code; 295 event[1] = sizeof(event) - 2; 296 little_endian_store_16(event, 2, channel->local_cid); 297 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 298 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 299 } 300 #endif 301 302 #ifdef ENABLE_CLASSIC 303 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 304 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", 305 status, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 306 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu, channel->flush_timeout); 307 uint8_t event[24]; 308 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 309 event[1] = sizeof(event) - 2; 310 event[2] = status; 311 reverse_bd_addr(channel->address, &event[3]); 312 little_endian_store_16(event, 9, channel->con_handle); 313 little_endian_store_16(event, 11, channel->psm); 314 little_endian_store_16(event, 13, channel->local_cid); 315 little_endian_store_16(event, 15, channel->remote_cid); 316 little_endian_store_16(event, 17, channel->local_mtu); 317 little_endian_store_16(event, 19, channel->remote_mtu); 318 little_endian_store_16(event, 21, channel->flush_timeout); 319 event[23] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 320 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 321 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 322 } 323 324 static void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 325 log_info("L2CAP_EVENT_CHANNEL_CLOSED local_cid 0x%x", channel->local_cid); 326 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 327 } 328 329 static void l2cap_emit_incoming_connection(l2cap_channel_t *channel) { 330 log_info("L2CAP_EVENT_INCOMING_CONNECTION addr %s handle 0x%x psm 0x%x local_cid 0x%x remote_cid 0x%x", 331 bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid); 332 uint8_t event[16]; 333 event[0] = L2CAP_EVENT_INCOMING_CONNECTION; 334 event[1] = sizeof(event) - 2; 335 reverse_bd_addr(channel->address, &event[2]); 336 little_endian_store_16(event, 8, channel->con_handle); 337 little_endian_store_16(event, 10, channel->psm); 338 little_endian_store_16(event, 12, channel->local_cid); 339 little_endian_store_16(event, 14, channel->remote_cid); 340 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 341 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 342 } 343 344 static l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){ 345 btstack_linked_list_iterator_t it; 346 btstack_linked_list_iterator_init(&it, &l2cap_channels); 347 while (btstack_linked_list_iterator_has_next(&it)){ 348 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 349 if ( channel->local_cid == local_cid) { 350 return channel; 351 } 352 } 353 return NULL; 354 } 355 356 /// 357 358 void l2cap_request_can_send_now_event(uint16_t local_cid){ 359 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 360 if (!channel) return; 361 channel->waiting_for_can_send_now = 1; 362 l2cap_notify_channel_can_send(); 363 } 364 365 int l2cap_can_send_packet_now(uint16_t local_cid){ 366 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 367 if (!channel) return 0; 368 return hci_can_send_acl_packet_now(channel->con_handle); 369 } 370 371 int l2cap_can_send_prepared_packet_now(uint16_t local_cid){ 372 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid); 373 if (!channel) return 0; 374 return hci_can_send_prepared_acl_packet_now(channel->con_handle); 375 } 376 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){ 377 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 378 if (channel) { 379 return channel->remote_mtu; 380 } 381 return 0; 382 } 383 384 static l2cap_channel_t * l2cap_channel_for_rtx_timer(btstack_timer_source_t * ts){ 385 btstack_linked_list_iterator_t it; 386 btstack_linked_list_iterator_init(&it, &l2cap_channels); 387 while (btstack_linked_list_iterator_has_next(&it)){ 388 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 389 if ( &channel->rtx == ts) { 390 return channel; 391 } 392 } 393 return NULL; 394 } 395 396 static void l2cap_rtx_timeout(btstack_timer_source_t * ts){ 397 l2cap_channel_t * channel = l2cap_channel_for_rtx_timer(ts); 398 if (!channel) return; 399 400 log_info("l2cap_rtx_timeout for local cid 0x%02x", channel->local_cid); 401 402 // "When terminating the channel, it is not necessary to send a L2CAP_DisconnectReq 403 // and enter WAIT_DISCONNECT state. Channels can be transitioned directly to the CLOSED state." 404 // notify client 405 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_RTX_TIMEOUT); 406 407 // discard channel 408 // no need to stop timer here, it is removed from list during timer callback 409 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 410 btstack_memory_l2cap_channel_free(channel); 411 } 412 413 static void l2cap_stop_rtx(l2cap_channel_t * channel){ 414 log_info("l2cap_stop_rtx for local cid 0x%02x", channel->local_cid); 415 btstack_run_loop_remove_timer(&channel->rtx); 416 } 417 418 static void l2cap_start_rtx(l2cap_channel_t * channel){ 419 l2cap_stop_rtx(channel); 420 log_info("l2cap_start_rtx for local cid 0x%02x", channel->local_cid); 421 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 422 btstack_run_loop_set_timer(&channel->rtx, L2CAP_RTX_TIMEOUT_MS); 423 btstack_run_loop_add_timer(&channel->rtx); 424 } 425 426 static void l2cap_start_ertx(l2cap_channel_t * channel){ 427 log_info("l2cap_start_ertx for local cid 0x%02x", channel->local_cid); 428 l2cap_stop_rtx(channel); 429 btstack_run_loop_set_timer_handler(&channel->rtx, l2cap_rtx_timeout); 430 btstack_run_loop_set_timer(&channel->rtx, L2CAP_ERTX_TIMEOUT_MS); 431 btstack_run_loop_add_timer(&channel->rtx); 432 } 433 434 void l2cap_require_security_level_2_for_outgoing_sdp(void){ 435 require_security_level2_for_outgoing_sdp = 1; 436 } 437 438 static int l2cap_security_level_0_allowed_for_PSM(uint16_t psm){ 439 return (psm == BLUETOOTH_PROTOCOL_SDP) && (!require_security_level2_for_outgoing_sdp); 440 } 441 442 static int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 443 if (!hci_can_send_acl_packet_now(handle)){ 444 log_info("l2cap_send_signaling_packet, cannot send"); 445 return BTSTACK_ACL_BUFFERS_FULL; 446 } 447 448 // log_info("l2cap_send_signaling_packet type %u", cmd); 449 hci_reserve_packet_buffer(); 450 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 451 va_list argptr; 452 va_start(argptr, identifier); 453 uint16_t len = l2cap_create_signaling_classic(acl_buffer, handle, cmd, identifier, argptr); 454 va_end(argptr); 455 // log_info("l2cap_send_signaling_packet con %u!", handle); 456 return hci_send_acl_packet_buffer(len); 457 } 458 459 // assumption - only on Classic connections 460 int l2cap_send_prepared(uint16_t local_cid, uint16_t len){ 461 462 if (!hci_is_packet_buffer_reserved()){ 463 log_error("l2cap_send_prepared called without reserving packet first"); 464 return BTSTACK_ACL_BUFFERS_FULL; 465 } 466 467 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 468 if (!channel) { 469 log_error("l2cap_send_prepared no channel for cid 0x%02x", local_cid); 470 return -1; // TODO: define error 471 } 472 473 if (!hci_can_send_prepared_acl_packet_now(channel->con_handle)){ 474 log_info("l2cap_send_prepared cid 0x%02x, cannot send", local_cid); 475 return BTSTACK_ACL_BUFFERS_FULL; 476 } 477 478 log_debug("l2cap_send_prepared cid 0x%02x, handle %u, 1 credit used", local_cid, channel->con_handle); 479 480 // set non-flushable packet boundary flag if supported on Controller 481 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 482 uint8_t packet_boundary_flag = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 483 l2cap_setup_header(acl_buffer, channel->con_handle, packet_boundary_flag, channel->remote_cid, len); 484 // send 485 return hci_send_acl_packet_buffer(len+8); 486 } 487 488 // assumption - only on Classic connections 489 int l2cap_send(uint16_t local_cid, uint8_t *data, uint16_t len){ 490 491 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 492 if (!channel) { 493 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 494 return -1; // TODO: define error 495 } 496 497 if (len > channel->remote_mtu){ 498 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 499 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 500 } 501 502 if (!hci_can_send_acl_packet_now(channel->con_handle)){ 503 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 504 return BTSTACK_ACL_BUFFERS_FULL; 505 } 506 507 hci_reserve_packet_buffer(); 508 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 509 510 memcpy(&acl_buffer[8], data, len); 511 512 return l2cap_send_prepared(local_cid, len); 513 } 514 515 int l2cap_send_echo_request(hci_con_handle_t con_handle, uint8_t *data, uint16_t len){ 516 return l2cap_send_signaling_packet(con_handle, ECHO_REQUEST, 0x77, len, data); 517 } 518 519 static inline void channelStateVarSetFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 520 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var | flag); 521 } 522 523 static inline void channelStateVarClearFlag(l2cap_channel_t *channel, L2CAP_CHANNEL_STATE_VAR flag){ 524 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (channel->state_var & ~flag); 525 } 526 #endif 527 528 529 #ifdef ENABLE_BLE 530 static int l2cap_send_le_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 531 532 if (!hci_can_send_acl_packet_now(handle)){ 533 log_info("l2cap_send_le_signaling_packet, cannot send"); 534 return BTSTACK_ACL_BUFFERS_FULL; 535 } 536 537 // log_info("l2cap_send_le_signaling_packet type %u", cmd); 538 hci_reserve_packet_buffer(); 539 uint8_t *acl_buffer = hci_get_outgoing_packet_buffer(); 540 va_list argptr; 541 va_start(argptr, identifier); 542 uint16_t len = l2cap_create_signaling_le(acl_buffer, handle, cmd, identifier, argptr); 543 va_end(argptr); 544 // log_info("l2cap_send_le_signaling_packet con %u!", handle); 545 return hci_send_acl_packet_buffer(len); 546 } 547 #endif 548 549 uint16_t l2cap_max_mtu(void){ 550 return HCI_ACL_PAYLOAD_SIZE - L2CAP_HEADER_SIZE; 551 } 552 553 uint16_t l2cap_max_le_mtu(void){ 554 return l2cap_max_mtu(); 555 } 556 557 // MARK: L2CAP_RUN 558 // process outstanding signaling tasks 559 static void l2cap_run(void){ 560 561 // log_info("l2cap_run: entered"); 562 563 // check pending signaling responses 564 while (signaling_responses_pending){ 565 566 hci_con_handle_t handle = signaling_responses[0].handle; 567 568 if (!hci_can_send_acl_packet_now(handle)) break; 569 570 uint8_t sig_id = signaling_responses[0].sig_id; 571 uint8_t response_code = signaling_responses[0].code; 572 uint16_t source_cid = signaling_responses[0].cid; // CONNECTION_REQUEST 573 uint16_t infoType = signaling_responses[0].data; // INFORMATION_REQUEST 574 uint16_t result = signaling_responses[0].data; // CONNECTION_REQUEST, COMMAND_REJECT 575 UNUSED(infoType); 576 577 // remove first item before sending (to avoid sending response mutliple times) 578 signaling_responses_pending--; 579 int i; 580 for (i=0; i < signaling_responses_pending; i++){ 581 memcpy(&signaling_responses[i], &signaling_responses[i+1], sizeof(l2cap_signaling_response_t)); 582 } 583 584 switch (response_code){ 585 #ifdef ENABLE_CLASSIC 586 case CONNECTION_REQUEST: 587 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, source_cid, 0, result, 0); 588 // also disconnect if result is 0x0003 - security blocked 589 if (result == 0x0003){ 590 hci_disconnect_security_block(handle); 591 } 592 break; 593 case ECHO_REQUEST: 594 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, 0, NULL); 595 break; 596 case INFORMATION_REQUEST: 597 switch (infoType){ 598 case 1: { // Connectionless MTU 599 uint16_t connectionless_mtu = hci_max_acl_data_packet_length(); 600 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(connectionless_mtu), &connectionless_mtu); 601 } 602 break; 603 case 2: { // Extended Features Supported 604 // extended features request supported, features: fixed channels, unicast connectionless data reception 605 uint32_t features = 0x280; 606 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(features), &features); 607 } 608 break; 609 case 3: { // Fixed Channels Supported 610 uint8_t map[8]; 611 memset(map, 0, 8); 612 map[0] = 0x06; // L2CAP Signaling Channel (0x02) + Connectionless reception (0x04) 613 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, sizeof(map), &map); 614 } 615 break; 616 default: 617 // all other types are not supported 618 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL); 619 break; 620 } 621 break; 622 case COMMAND_REJECT: 623 l2cap_send_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 624 break; 625 #endif 626 #ifdef ENABLE_BLE 627 case LE_CREDIT_BASED_CONNECTION_REQUEST: 628 l2cap_send_le_signaling_packet(handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, sig_id, 0, 0, 0, 0, result); 629 break; 630 case COMMAND_REJECT_LE: 631 l2cap_send_le_signaling_packet(handle, COMMAND_REJECT, sig_id, result, 0, NULL); 632 break; 633 #endif 634 default: 635 // should not happen 636 break; 637 } 638 } 639 640 btstack_linked_list_iterator_t it; 641 UNUSED(it); 642 643 #ifdef ENABLE_CLASSIC 644 uint8_t config_options[4]; 645 btstack_linked_list_iterator_init(&it, &l2cap_channels); 646 while (btstack_linked_list_iterator_has_next(&it)){ 647 648 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 649 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 650 switch (channel->state){ 651 652 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 653 case L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT: 654 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 655 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND) { 656 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND); 657 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 1, 0); 658 } 659 break; 660 661 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 662 if (!hci_can_send_command_packet_now()) break; 663 // send connection request - set state first 664 channel->state = L2CAP_STATE_WAIT_CONNECTION_COMPLETE; 665 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 666 hci_send_cmd(&hci_create_connection, channel->address, hci_usable_acl_packet_types(), 0, 0, 0, 1); 667 break; 668 669 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE: 670 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 671 channel->state = L2CAP_STATE_INVALID; 672 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, channel->reason, 0); 673 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 674 l2cap_stop_rtx(channel); 675 btstack_linked_list_iterator_remove(&it); 676 btstack_memory_l2cap_channel_free(channel); 677 break; 678 679 case L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT: 680 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 681 channel->state = L2CAP_STATE_CONFIG; 682 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 683 l2cap_send_signaling_packet(channel->con_handle, CONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid, 0, 0); 684 break; 685 686 case L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST: 687 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 688 // success, start l2cap handshake 689 channel->local_sig_id = l2cap_next_sig_id(); 690 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 691 l2cap_send_signaling_packet( channel->con_handle, CONNECTION_REQUEST, channel->local_sig_id, channel->psm, channel->local_cid); 692 l2cap_start_rtx(channel); 693 break; 694 695 case L2CAP_STATE_CONFIG: 696 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 697 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP){ 698 uint16_t flags = 0; 699 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 700 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT) { 701 flags = 1; 702 } else { 703 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP); 704 } 705 if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID){ 706 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, L2CAP_CONF_RESULT_UNKNOWN_OPTIONS, 0, NULL); 707 } else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU){ 708 config_options[0] = 1; // MTU 709 config_options[1] = 2; // len param 710 little_endian_store_16( (uint8_t*)&config_options, 2, channel->remote_mtu); 711 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, 0, 4, &config_options); 712 channelStateVarClearFlag(channel,L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 713 } else { 714 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_RESPONSE, channel->remote_sig_id, channel->remote_cid, flags, 0, 0, NULL); 715 } 716 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 717 } 718 else if (channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ){ 719 channelStateVarClearFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 720 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SENT_CONF_REQ); 721 channel->local_sig_id = l2cap_next_sig_id(); 722 config_options[0] = 1; // MTU 723 config_options[1] = 2; // len param 724 little_endian_store_16( (uint8_t*)&config_options, 2, channel->local_mtu); 725 l2cap_send_signaling_packet(channel->con_handle, CONFIGURE_REQUEST, channel->local_sig_id, channel->remote_cid, 0, 4, &config_options); 726 l2cap_start_rtx(channel); 727 } 728 if (l2cap_channel_ready_for_open(channel)){ 729 channel->state = L2CAP_STATE_OPEN; 730 l2cap_emit_channel_opened(channel, 0); // success 731 } 732 break; 733 734 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 735 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 736 channel->state = L2CAP_STATE_INVALID; 737 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 738 // 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 :) 739 l2cap_finialize_channel_close(channel); // -- remove from list 740 break; 741 742 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 743 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 744 channel->local_sig_id = l2cap_next_sig_id(); 745 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 746 l2cap_send_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 747 break; 748 default: 749 break; 750 } 751 } 752 #endif 753 754 #ifdef ENABLE_LE_DATA_CHANNELS 755 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 756 while (btstack_linked_list_iterator_has_next(&it)){ 757 uint8_t * acl_buffer; 758 uint8_t * l2cap_payload; 759 uint16_t pos; 760 uint16_t payload_size; 761 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 762 // log_info("l2cap_run: channel %p, state %u, var 0x%02x", channel, channel->state, channel->state_var); 763 switch (channel->state){ 764 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST: 765 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 766 channel->state = L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE; 767 // le psm, source cid, mtu, mps, initial credits 768 channel->local_sig_id = l2cap_next_sig_id(); 769 channel->credits_incoming = channel->new_credits_incoming; 770 channel->new_credits_incoming = 0; 771 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); 772 break; 773 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT: 774 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 775 // TODO: support larger MPS 776 channel->state = L2CAP_STATE_OPEN; 777 channel->credits_incoming = channel->new_credits_incoming; 778 channel->new_credits_incoming = 0; 779 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); 780 // notify client 781 l2cap_emit_le_channel_opened(channel, 0); 782 break; 783 case L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE: 784 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 785 channel->state = L2CAP_STATE_INVALID; 786 l2cap_send_le_signaling_packet(channel->con_handle, LE_CREDIT_BASED_CONNECTION_RESPONSE, channel->remote_sig_id, 0, 0, 0, 0, channel->reason); 787 // discard channel - l2cap_finialize_channel_close without sending l2cap close event 788 l2cap_stop_rtx(channel); 789 btstack_linked_list_iterator_remove(&it); 790 btstack_memory_l2cap_channel_free(channel); 791 break; 792 case L2CAP_STATE_OPEN: 793 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 794 795 // send credits 796 if (channel->new_credits_incoming){ 797 log_info("l2cap: sending %u credits", channel->new_credits_incoming); 798 channel->local_sig_id = l2cap_next_sig_id(); 799 uint16_t new_credits = channel->new_credits_incoming; 800 channel->new_credits_incoming = 0; 801 channel->credits_incoming += new_credits; 802 l2cap_send_le_signaling_packet(channel->con_handle, LE_FLOW_CONTROL_CREDIT, channel->local_sig_id, channel->remote_cid, new_credits); 803 break; 804 } 805 806 // send data 807 if (!channel->send_sdu_buffer) break; 808 if (!channel->credits_outgoing) break; 809 810 // send part of SDU 811 hci_reserve_packet_buffer(); 812 acl_buffer = hci_get_outgoing_packet_buffer(); 813 l2cap_payload = acl_buffer + 8; 814 pos = 0; 815 if (!channel->send_sdu_pos){ 816 // store SDU len 817 channel->send_sdu_pos += 2; 818 little_endian_store_16(l2cap_payload, pos, channel->send_sdu_len); 819 pos += 2; 820 } 821 payload_size = btstack_min(channel->send_sdu_len + 2 - channel->send_sdu_pos, channel->remote_mps - pos); 822 log_info("len %u, pos %u => payload %u, credits %u", channel->send_sdu_len, channel->send_sdu_pos, payload_size, channel->credits_outgoing); 823 memcpy(&l2cap_payload[pos], &channel->send_sdu_buffer[channel->send_sdu_pos-2], payload_size); // -2 for virtual SDU len 824 pos += payload_size; 825 channel->send_sdu_pos += payload_size; 826 l2cap_setup_header(acl_buffer, channel->con_handle, 0, channel->remote_cid, pos); 827 // done 828 829 channel->credits_outgoing--; 830 831 if (channel->send_sdu_pos >= channel->send_sdu_len + 2){ 832 channel->send_sdu_buffer = NULL; 833 // send done event 834 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_PACKET_SENT); 835 // inform about can send now 836 l2cap_le_notify_channel_can_send(channel); 837 } 838 hci_send_acl_packet_buffer(8 + pos); 839 break; 840 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 841 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 842 channel->local_sig_id = l2cap_next_sig_id(); 843 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 844 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_REQUEST, channel->local_sig_id, channel->remote_cid, channel->local_cid); 845 break; 846 case L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE: 847 if (!hci_can_send_acl_packet_now(channel->con_handle)) break; 848 channel->state = L2CAP_STATE_INVALID; 849 l2cap_send_le_signaling_packet( channel->con_handle, DISCONNECTION_RESPONSE, channel->remote_sig_id, channel->local_cid, channel->remote_cid); 850 l2cap_le_finialize_channel_close(channel); // -- remove from list 851 break; 852 default: 853 break; 854 } 855 } 856 #endif 857 858 #ifdef ENABLE_BLE 859 // send l2cap con paramter update if necessary 860 hci_connections_get_iterator(&it); 861 while(btstack_linked_list_iterator_has_next(&it)){ 862 hci_connection_t * connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it); 863 if (connection->address_type != BD_ADDR_TYPE_LE_PUBLIC && connection->address_type != BD_ADDR_TYPE_LE_RANDOM) continue; 864 if (!hci_can_send_acl_packet_now(connection->con_handle)) continue; 865 switch (connection->le_con_parameter_update_state){ 866 case CON_PARAMETER_UPDATE_SEND_REQUEST: 867 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 868 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_REQUEST, connection->le_con_param_update_identifier, 869 connection->le_conn_interval_min, connection->le_conn_interval_max, connection->le_conn_latency, connection->le_supervision_timeout); 870 break; 871 case CON_PARAMETER_UPDATE_SEND_RESPONSE: 872 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_CHANGE_HCI_CON_PARAMETERS; 873 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 0); 874 break; 875 case CON_PARAMETER_UPDATE_DENY: 876 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_NONE; 877 l2cap_send_le_signaling_packet(connection->con_handle, CONNECTION_PARAMETER_UPDATE_RESPONSE, connection->le_con_param_update_identifier, 1); 878 break; 879 default: 880 break; 881 } 882 } 883 #endif 884 885 // log_info("l2cap_run: exit"); 886 } 887 888 #ifdef ENABLE_CLASSIC 889 static void l2cap_handle_connection_complete(hci_con_handle_t con_handle, l2cap_channel_t * channel){ 890 if (channel->state == L2CAP_STATE_WAIT_CONNECTION_COMPLETE || channel->state == L2CAP_STATE_WILL_SEND_CREATE_CONNECTION) { 891 log_info("l2cap_handle_connection_complete expected state"); 892 // success, start l2cap handshake 893 channel->con_handle = con_handle; 894 // check remote SSP feature first 895 channel->state = L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES; 896 } 897 } 898 899 static void l2cap_handle_remote_supported_features_received(l2cap_channel_t * channel){ 900 if (channel->state != L2CAP_STATE_WAIT_REMOTE_SUPPORTED_FEATURES) return; 901 902 // we have been waiting for remote supported features, if both support SSP, 903 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)); 904 if (gap_ssp_supported_on_both_sides(channel->con_handle) && !l2cap_security_level_0_allowed_for_PSM(channel->psm)){ 905 // request security level 2 906 channel->state = L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE; 907 gap_request_security_level(channel->con_handle, LEVEL_2); 908 return; 909 } 910 // fine, go ahead 911 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 912 } 913 #endif 914 915 #ifdef L2CAP_USES_CHANNELS 916 static l2cap_channel_t * l2cap_create_channel_entry(btstack_packet_handler_t packet_handler, bd_addr_t address, bd_addr_type_t address_type, 917 uint16_t psm, uint16_t local_mtu, gap_security_level_t security_level){ 918 919 l2cap_channel_t * channel = btstack_memory_l2cap_channel_get(); 920 if (!channel) { 921 return NULL; 922 } 923 924 // Init memory (make valgrind happy) 925 memset(channel, 0, sizeof(l2cap_channel_t)); 926 927 // fill in 928 channel->packet_handler = packet_handler; 929 bd_addr_copy(channel->address, address); 930 channel->address_type = address_type; 931 channel->psm = psm; 932 channel->local_mtu = local_mtu; 933 channel->remote_mtu = L2CAP_MINIMAL_MTU; 934 channel->required_security_level = security_level; 935 936 // 937 channel->local_cid = l2cap_next_local_cid(); 938 channel->con_handle = 0; 939 940 // set initial state 941 channel->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; 942 channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; 943 channel->remote_sig_id = L2CAP_SIG_ID_INVALID; 944 channel->local_sig_id = L2CAP_SIG_ID_INVALID; 945 return channel; 946 } 947 #endif 948 949 #ifdef ENABLE_CLASSIC 950 951 /** 952 * @brief Creates L2CAP channel to the PSM of a remote device with baseband address. A new baseband connection will be initiated if necessary. 953 * @param packet_handler 954 * @param address 955 * @param psm 956 * @param mtu 957 * @param local_cid 958 */ 959 960 uint8_t l2cap_create_channel(btstack_packet_handler_t channel_packet_handler, bd_addr_t address, uint16_t psm, uint16_t local_mtu, uint16_t * out_local_cid){ 961 log_info("L2CAP_CREATE_CHANNEL addr %s psm 0x%x mtu %u", bd_addr_to_str(address), psm, local_mtu); 962 963 if (local_mtu > l2cap_max_mtu()) { 964 local_mtu = l2cap_max_mtu(); 965 } 966 967 l2cap_channel_t * channel = l2cap_create_channel_entry(channel_packet_handler, address, BD_ADDR_TYPE_CLASSIC, psm, local_mtu, LEVEL_0); 968 if (!channel) { 969 return BTSTACK_MEMORY_ALLOC_FAILED; 970 } 971 972 // add to connections list 973 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 974 975 // store local_cid 976 if (out_local_cid){ 977 *out_local_cid = channel->local_cid; 978 } 979 980 // check if hci connection is already usable 981 hci_connection_t * conn = hci_connection_for_bd_addr_and_type(address, BD_ADDR_TYPE_CLASSIC); 982 if (conn){ 983 log_info("l2cap_create_channel, hci connection already exists"); 984 l2cap_handle_connection_complete(conn->con_handle, channel); 985 // check if remote supported fearures are already received 986 if (conn->bonding_flags & BONDING_RECEIVED_REMOTE_FEATURES) { 987 l2cap_handle_remote_supported_features_received(channel); 988 } 989 } 990 991 l2cap_run(); 992 993 return 0; 994 } 995 996 void 997 l2cap_disconnect(uint16_t local_cid, uint8_t reason){ 998 log_info("L2CAP_DISCONNECT local_cid 0x%x reason 0x%x", local_cid, reason); 999 // find channel for local_cid 1000 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1001 if (channel) { 1002 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1003 } 1004 // process 1005 l2cap_run(); 1006 } 1007 1008 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 1009 btstack_linked_list_iterator_t it; 1010 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1011 while (btstack_linked_list_iterator_has_next(&it)){ 1012 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1013 if ( bd_addr_cmp( channel->address, address) != 0) continue; 1014 // channel for this address found 1015 switch (channel->state){ 1016 case L2CAP_STATE_WAIT_CONNECTION_COMPLETE: 1017 case L2CAP_STATE_WILL_SEND_CREATE_CONNECTION: 1018 // failure, forward error code 1019 l2cap_emit_channel_opened(channel, status); 1020 // discard channel 1021 l2cap_stop_rtx(channel); 1022 btstack_linked_list_iterator_remove(&it); 1023 btstack_memory_l2cap_channel_free(channel); 1024 break; 1025 default: 1026 break; 1027 } 1028 } 1029 } 1030 1031 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 1032 btstack_linked_list_iterator_t it; 1033 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1034 while (btstack_linked_list_iterator_has_next(&it)){ 1035 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1036 if ( ! bd_addr_cmp( channel->address, address) ){ 1037 l2cap_handle_connection_complete(handle, channel); 1038 } 1039 } 1040 // process 1041 l2cap_run(); 1042 } 1043 #endif 1044 1045 static void l2cap_notify_channel_can_send(void){ 1046 1047 #ifdef ENABLE_CLASSIC 1048 btstack_linked_list_iterator_t it; 1049 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1050 while (btstack_linked_list_iterator_has_next(&it)){ 1051 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1052 if (!channel->waiting_for_can_send_now) continue; 1053 if (!hci_can_send_acl_packet_now(channel->con_handle)) continue; 1054 channel->waiting_for_can_send_now = 0; 1055 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid); 1056 } 1057 #endif 1058 1059 int i; 1060 for (i=0;i<L2CAP_FIXED_CHANNEL_TABLE_SIZE;i++){ 1061 if (!fixed_channels[i].callback) continue; 1062 if (!fixed_channels[i].waiting_for_can_send_now) continue; 1063 int can_send = 0; 1064 if (l2cap_fixed_channel_table_index_is_le(i)){ 1065 #ifdef ENABLE_BLE 1066 can_send = hci_can_send_acl_le_packet_now(); 1067 #endif 1068 } else { 1069 #ifdef ENABLE_CLASSIC 1070 can_send = hci_can_send_acl_classic_packet_now(); 1071 #endif 1072 } 1073 if (!can_send) continue; 1074 fixed_channels[i].waiting_for_can_send_now = 0; 1075 l2cap_emit_can_send_now(fixed_channels[i].callback, l2cap_fixed_channel_table_channel_id_for_index(i)); 1076 } 1077 } 1078 1079 static void l2cap_hci_event_handler(uint8_t packet_type, uint16_t cid, uint8_t *packet, uint16_t size){ 1080 1081 UNUSED(packet_type); 1082 UNUSED(cid); 1083 UNUSED(size); 1084 1085 bd_addr_t address; 1086 hci_con_handle_t handle; 1087 int hci_con_used; 1088 btstack_linked_list_iterator_t it; 1089 1090 // avoid unused warnings 1091 UNUSED(address); 1092 UNUSED(hci_con_used); 1093 UNUSED(it); 1094 UNUSED(handle); 1095 1096 switch(hci_event_packet_get_type(packet)){ 1097 1098 // Notify channel packet handler if they can send now 1099 case HCI_EVENT_TRANSPORT_PACKET_SENT: 1100 case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: 1101 l2cap_run(); // try sending signaling packets first 1102 l2cap_notify_channel_can_send(); 1103 break; 1104 1105 case HCI_EVENT_COMMAND_STATUS: 1106 l2cap_run(); // try sending signaling packets first 1107 break; 1108 1109 #ifdef ENABLE_CLASSIC 1110 // handle connection complete events 1111 case HCI_EVENT_CONNECTION_COMPLETE: 1112 reverse_bd_addr(&packet[5], address); 1113 if (packet[2] == 0){ 1114 handle = little_endian_read_16(packet, 3); 1115 l2cap_handle_connection_success_for_addr(address, handle); 1116 } else { 1117 l2cap_handle_connection_failed_for_addr(address, packet[2]); 1118 } 1119 break; 1120 1121 // handle successful create connection cancel command 1122 case HCI_EVENT_COMMAND_COMPLETE: 1123 if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_create_connection_cancel)) { 1124 if (packet[5] == 0){ 1125 reverse_bd_addr(&packet[6], address); 1126 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 1127 l2cap_handle_connection_failed_for_addr(address, 0x16); 1128 } 1129 } 1130 l2cap_run(); // try sending signaling packets first 1131 break; 1132 #endif 1133 1134 // handle disconnection complete events 1135 case HCI_EVENT_DISCONNECTION_COMPLETE: 1136 // send l2cap disconnect events for all channels on this handle and free them 1137 #ifdef ENABLE_CLASSIC 1138 handle = little_endian_read_16(packet, 3); 1139 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1140 while (btstack_linked_list_iterator_has_next(&it)){ 1141 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1142 if (channel->con_handle != handle) continue; 1143 l2cap_emit_channel_closed(channel); 1144 l2cap_stop_rtx(channel); 1145 btstack_linked_list_iterator_remove(&it); 1146 btstack_memory_l2cap_channel_free(channel); 1147 } 1148 #endif 1149 #ifdef ENABLE_LE_DATA_CHANNELS 1150 handle = little_endian_read_16(packet, 3); 1151 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1152 while (btstack_linked_list_iterator_has_next(&it)){ 1153 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1154 if (channel->con_handle != handle) continue; 1155 l2cap_emit_channel_closed(channel); 1156 btstack_linked_list_iterator_remove(&it); 1157 btstack_memory_l2cap_channel_free(channel); 1158 } 1159 #endif 1160 break; 1161 1162 // HCI Connection Timeouts 1163 #ifdef ENABLE_CLASSIC 1164 case L2CAP_EVENT_TIMEOUT_CHECK: 1165 handle = little_endian_read_16(packet, 2); 1166 if (gap_get_connection_type(handle) != GAP_CONNECTION_ACL) break; 1167 if (hci_authentication_active_for_handle(handle)) break; 1168 hci_con_used = 0; 1169 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1170 while (btstack_linked_list_iterator_has_next(&it)){ 1171 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1172 if (channel->con_handle != handle) continue; 1173 hci_con_used = 1; 1174 break; 1175 } 1176 if (hci_con_used) break; 1177 if (!hci_can_send_command_packet_now()) break; 1178 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 1179 break; 1180 1181 case HCI_EVENT_READ_REMOTE_SUPPORTED_FEATURES_COMPLETE: 1182 handle = little_endian_read_16(packet, 3); 1183 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1184 while (btstack_linked_list_iterator_has_next(&it)){ 1185 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1186 if (channel->con_handle != handle) continue; 1187 l2cap_handle_remote_supported_features_received(channel); 1188 break; 1189 } 1190 break; 1191 1192 case GAP_EVENT_SECURITY_LEVEL: 1193 handle = little_endian_read_16(packet, 2); 1194 log_info("l2cap - security level update"); 1195 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1196 while (btstack_linked_list_iterator_has_next(&it)){ 1197 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1198 if (channel->con_handle != handle) continue; 1199 1200 log_info("l2cap - state %u", channel->state); 1201 1202 gap_security_level_t actual_level = (gap_security_level_t) packet[4]; 1203 gap_security_level_t required_level = channel->required_security_level; 1204 1205 switch (channel->state){ 1206 case L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE: 1207 if (actual_level >= required_level){ 1208 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 1209 l2cap_emit_incoming_connection(channel); 1210 } else { 1211 channel->reason = 0x0003; // security block 1212 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1213 } 1214 break; 1215 1216 case L2CAP_STATE_WAIT_OUTGOING_SECURITY_LEVEL_UPDATE: 1217 if (actual_level >= required_level){ 1218 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_REQUEST; 1219 } else { 1220 // disconnnect, authentication not good enough 1221 hci_disconnect_security_block(handle); 1222 } 1223 break; 1224 1225 default: 1226 break; 1227 } 1228 } 1229 break; 1230 #endif 1231 1232 default: 1233 break; 1234 } 1235 1236 l2cap_run(); 1237 } 1238 1239 static void l2cap_register_signaling_response(hci_con_handle_t handle, uint8_t code, uint8_t sig_id, uint16_t cid, uint16_t data){ 1240 // Vol 3, Part A, 4.3: "The DCID and SCID fields shall be ignored when the result field indi- cates the connection was refused." 1241 if (signaling_responses_pending < NR_PENDING_SIGNALING_RESPONSES) { 1242 signaling_responses[signaling_responses_pending].handle = handle; 1243 signaling_responses[signaling_responses_pending].code = code; 1244 signaling_responses[signaling_responses_pending].sig_id = sig_id; 1245 signaling_responses[signaling_responses_pending].cid = cid; 1246 signaling_responses[signaling_responses_pending].data = data; 1247 signaling_responses_pending++; 1248 l2cap_run(); 1249 } 1250 } 1251 1252 #ifdef ENABLE_CLASSIC 1253 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 1254 channel->remote_sig_id = identifier; 1255 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 1256 l2cap_run(); 1257 } 1258 1259 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 1260 1261 // log_info("l2cap_handle_connection_request for handle %u, psm %u cid 0x%02x", handle, psm, source_cid); 1262 l2cap_service_t *service = l2cap_get_service(psm); 1263 if (!service) { 1264 // 0x0002 PSM not supported 1265 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 1266 return; 1267 } 1268 1269 hci_connection_t * hci_connection = hci_connection_for_handle( handle ); 1270 if (!hci_connection) { 1271 // 1272 log_error("no hci_connection for handle %u", handle); 1273 return; 1274 } 1275 1276 // alloc structure 1277 // log_info("l2cap_handle_connection_request register channel"); 1278 l2cap_channel_t * channel = l2cap_create_channel_entry(service->packet_handler, hci_connection->address, BD_ADDR_TYPE_CLASSIC, 1279 psm, service->mtu, service->required_security_level); 1280 if (!channel){ 1281 // 0x0004 No resources available 1282 l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 1283 return; 1284 } 1285 1286 channel->con_handle = handle; 1287 channel->remote_cid = source_cid; 1288 channel->remote_sig_id = sig_id; 1289 1290 // limit local mtu to max acl packet length - l2cap header 1291 if (channel->local_mtu > l2cap_max_mtu()) { 1292 channel->local_mtu = l2cap_max_mtu(); 1293 } 1294 1295 // set initial state 1296 channel->state = L2CAP_STATE_WAIT_INCOMING_SECURITY_LEVEL_UPDATE; 1297 channel->state_var = (L2CAP_CHANNEL_STATE_VAR) (L2CAP_CHANNEL_STATE_VAR_SEND_CONN_RESP_PEND | L2CAP_CHANNEL_STATE_VAR_INCOMING); 1298 1299 // add to connections list 1300 btstack_linked_list_add(&l2cap_channels, (btstack_linked_item_t *) channel); 1301 1302 // assert security requirements 1303 gap_request_security_level(handle, channel->required_security_level); 1304 } 1305 1306 void l2cap_accept_connection(uint16_t local_cid){ 1307 log_info("L2CAP_ACCEPT_CONNECTION local_cid 0x%x", local_cid); 1308 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 1309 if (!channel) { 1310 log_error("l2cap_accept_connection called but local_cid 0x%x not found", local_cid); 1311 return; 1312 } 1313 1314 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_ACCEPT; 1315 1316 // process 1317 l2cap_run(); 1318 } 1319 1320 void l2cap_decline_connection(uint16_t local_cid){ 1321 log_info("L2CAP_DECLINE_CONNECTION local_cid 0x%x", local_cid); 1322 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 1323 if (!channel) { 1324 log_error( "l2cap_decline_connection called but local_cid 0x%x not found", local_cid); 1325 return; 1326 } 1327 channel->state = L2CAP_STATE_WILL_SEND_CONNECTION_RESPONSE_DECLINE; 1328 channel->reason = 0x04; // no resources available 1329 l2cap_run(); 1330 } 1331 1332 static void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 1333 1334 channel->remote_sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1335 1336 uint16_t flags = little_endian_read_16(command, 6); 1337 if (flags & 1) { 1338 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT); 1339 } 1340 1341 // accept the other's configuration options 1342 uint16_t end_pos = 4 + little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 1343 uint16_t pos = 8; 1344 while (pos < end_pos){ 1345 uint8_t option_hint = command[pos] >> 7; 1346 uint8_t option_type = command[pos] & 0x7f; 1347 log_info("l2cap cid %u, hint %u, type %u", channel->local_cid, option_hint, option_type); 1348 pos++; 1349 uint8_t length = command[pos++]; 1350 // MTU { type(8): 1, len(8):2, MTU(16) } 1351 if (option_type == 1 && length == 2){ 1352 channel->remote_mtu = little_endian_read_16(command, pos); 1353 // log_info("l2cap cid 0x%02x, remote mtu %u", channel->local_cid, channel->remote_mtu); 1354 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_MTU); 1355 } 1356 // Flush timeout { type(8):2, len(8): 2, Flush Timeout(16)} 1357 if (option_type == 2 && length == 2){ 1358 channel->flush_timeout = little_endian_read_16(command, pos); 1359 } 1360 // check for unknown options 1361 if (option_hint == 0 && (option_type == 0 || option_type >= 0x07)){ 1362 log_info("l2cap cid %u, unknown options", channel->local_cid); 1363 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_INVALID); 1364 } 1365 pos += length; 1366 } 1367 } 1368 1369 static int l2cap_channel_ready_for_open(l2cap_channel_t *channel){ 1370 // log_info("l2cap_channel_ready_for_open 0x%02x", channel->state_var); 1371 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP) == 0) return 0; 1372 if ((channel->state_var & L2CAP_CHANNEL_STATE_VAR_SENT_CONF_RSP) == 0) return 0; 1373 // addition check that fixes re-entrance issue causing l2cap event channel opened twice 1374 if (channel->state == L2CAP_STATE_OPEN) return 0; 1375 return 1; 1376 } 1377 1378 1379 static void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 1380 1381 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 1382 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1383 uint16_t result = 0; 1384 1385 log_info("L2CAP signaling handler code %u, state %u", code, channel->state); 1386 1387 // handle DISCONNECT REQUESTS seperately 1388 if (code == DISCONNECTION_REQUEST){ 1389 switch (channel->state){ 1390 case L2CAP_STATE_CONFIG: 1391 case L2CAP_STATE_OPEN: 1392 case L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST: 1393 case L2CAP_STATE_WAIT_DISCONNECT: 1394 l2cap_handle_disconnect_request(channel, identifier); 1395 break; 1396 1397 default: 1398 // ignore in other states 1399 break; 1400 } 1401 return; 1402 } 1403 1404 // @STATEMACHINE(l2cap) 1405 switch (channel->state) { 1406 1407 case L2CAP_STATE_WAIT_CONNECT_RSP: 1408 switch (code){ 1409 case CONNECTION_RESPONSE: 1410 l2cap_stop_rtx(channel); 1411 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 1412 switch (result) { 1413 case 0: 1414 // successful connection 1415 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 1416 channel->state = L2CAP_STATE_CONFIG; 1417 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1418 break; 1419 case 1: 1420 // connection pending. get some coffee, but start the ERTX 1421 l2cap_start_ertx(channel); 1422 break; 1423 default: 1424 // channel closed 1425 channel->state = L2CAP_STATE_CLOSED; 1426 // map l2cap connection response result to BTstack status enumeration 1427 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 1428 1429 // drop link key if security block 1430 if (L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY){ 1431 gap_drop_link_key_for_bd_addr(channel->address); 1432 } 1433 1434 // discard channel 1435 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1436 btstack_memory_l2cap_channel_free(channel); 1437 break; 1438 } 1439 break; 1440 1441 default: 1442 //@TODO: implement other signaling packets 1443 break; 1444 } 1445 break; 1446 1447 case L2CAP_STATE_CONFIG: 1448 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 1449 switch (code) { 1450 case CONFIGURE_REQUEST: 1451 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP); 1452 l2cap_signaling_handle_configure_request(channel, command); 1453 if (!(channel->state_var & L2CAP_CHANNEL_STATE_VAR_SEND_CONF_RSP_CONT)){ 1454 // only done if continuation not set 1455 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_REQ); 1456 } 1457 break; 1458 case CONFIGURE_RESPONSE: 1459 l2cap_stop_rtx(channel); 1460 switch (result){ 1461 case 0: // success 1462 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_RCVD_CONF_RSP); 1463 break; 1464 case 4: // pending 1465 l2cap_start_ertx(channel); 1466 break; 1467 default: 1468 // retry on negative result 1469 channelStateVarSetFlag(channel, L2CAP_CHANNEL_STATE_VAR_SEND_CONF_REQ); 1470 break; 1471 } 1472 break; 1473 default: 1474 break; 1475 } 1476 if (l2cap_channel_ready_for_open(channel)){ 1477 // for open: 1478 channel->state = L2CAP_STATE_OPEN; 1479 l2cap_emit_channel_opened(channel, 0); 1480 } 1481 break; 1482 1483 case L2CAP_STATE_WAIT_DISCONNECT: 1484 switch (code) { 1485 case DISCONNECTION_RESPONSE: 1486 l2cap_finialize_channel_close(channel); 1487 break; 1488 default: 1489 //@TODO: implement other signaling packets 1490 break; 1491 } 1492 break; 1493 1494 case L2CAP_STATE_CLOSED: 1495 // @TODO handle incoming requests 1496 break; 1497 1498 case L2CAP_STATE_OPEN: 1499 //@TODO: implement other signaling packets, e.g. re-configure 1500 break; 1501 default: 1502 break; 1503 } 1504 // log_info("new state %u", channel->state); 1505 } 1506 1507 1508 static void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){ 1509 1510 // get code, signalind identifier and command len 1511 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 1512 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 1513 1514 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_REQUEST 1515 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_REQUEST){ 1516 l2cap_register_signaling_response(handle, COMMAND_REJECT, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 1517 return; 1518 } 1519 1520 // general commands without an assigned channel 1521 switch(code) { 1522 1523 case CONNECTION_REQUEST: { 1524 uint16_t psm = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 1525 uint16_t source_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 1526 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 1527 return; 1528 } 1529 1530 case ECHO_REQUEST: 1531 l2cap_register_signaling_response(handle, code, sig_id, 0, 0); 1532 return; 1533 1534 case INFORMATION_REQUEST: { 1535 uint16_t infoType = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 1536 l2cap_register_signaling_response(handle, code, sig_id, 0, infoType); 1537 return; 1538 } 1539 1540 default: 1541 break; 1542 } 1543 1544 1545 // Get potential destination CID 1546 uint16_t dest_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 1547 1548 // Find channel for this sig_id and connection handle 1549 btstack_linked_list_iterator_t it; 1550 btstack_linked_list_iterator_init(&it, &l2cap_channels); 1551 while (btstack_linked_list_iterator_has_next(&it)){ 1552 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1553 if (channel->con_handle != handle) continue; 1554 if (code & 1) { 1555 // match odd commands (responses) by previous signaling identifier 1556 if (channel->local_sig_id == sig_id) { 1557 l2cap_signaling_handler_channel(channel, command); 1558 break; 1559 } 1560 } else { 1561 // match even commands (requests) by local channel id 1562 if (channel->local_cid == dest_cid) { 1563 l2cap_signaling_handler_channel(channel, command); 1564 break; 1565 } 1566 } 1567 } 1568 } 1569 #endif 1570 1571 #ifdef ENABLE_BLE 1572 1573 static void l2cap_emit_connection_parameter_update_response(hci_con_handle_t con_handle, uint16_t result){ 1574 uint8_t event[6]; 1575 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE; 1576 event[1] = 4; 1577 little_endian_store_16(event, 2, con_handle); 1578 little_endian_store_16(event, 4, result); 1579 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1580 if (!l2cap_event_packet_handler) return; 1581 (*l2cap_event_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 1582 } 1583 1584 // @returns valid 1585 static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t * command, uint8_t sig_id){ 1586 hci_connection_t * connection; 1587 uint16_t result; 1588 uint8_t event[10]; 1589 1590 #ifdef ENABLE_LE_DATA_CHANNELS 1591 btstack_linked_list_iterator_t it; 1592 l2cap_channel_t * channel; 1593 uint16_t local_cid; 1594 uint16_t le_psm; 1595 uint16_t new_credits; 1596 uint16_t credits_before; 1597 l2cap_service_t * service; 1598 #endif 1599 1600 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 1601 log_info("l2cap_le_signaling_handler_dispatch: command 0x%02x, sig id %u", code, sig_id); 1602 1603 switch (code){ 1604 1605 case CONNECTION_PARAMETER_UPDATE_RESPONSE: 1606 result = little_endian_read_16(command, 4); 1607 l2cap_emit_connection_parameter_update_response(handle, result); 1608 break; 1609 1610 case CONNECTION_PARAMETER_UPDATE_REQUEST: 1611 connection = hci_connection_for_handle(handle); 1612 if (connection){ 1613 if (connection->role != HCI_ROLE_MASTER){ 1614 // reject command without notifying upper layer when not in master role 1615 return 0; 1616 } 1617 int update_parameter = 1; 1618 le_connection_parameter_range_t existing_range; 1619 gap_get_connection_parameter_range(&existing_range); 1620 uint16_t le_conn_interval_min = little_endian_read_16(command,8); 1621 uint16_t le_conn_interval_max = little_endian_read_16(command,10); 1622 uint16_t le_conn_latency = little_endian_read_16(command,12); 1623 uint16_t le_supervision_timeout = little_endian_read_16(command,14); 1624 1625 if (le_conn_interval_min < existing_range.le_conn_interval_min) update_parameter = 0; 1626 if (le_conn_interval_max > existing_range.le_conn_interval_max) update_parameter = 0; 1627 1628 if (le_conn_latency < existing_range.le_conn_latency_min) update_parameter = 0; 1629 if (le_conn_latency > existing_range.le_conn_latency_max) update_parameter = 0; 1630 1631 if (le_supervision_timeout < existing_range.le_supervision_timeout_min) update_parameter = 0; 1632 if (le_supervision_timeout > existing_range.le_supervision_timeout_max) update_parameter = 0; 1633 1634 if (update_parameter){ 1635 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_SEND_RESPONSE; 1636 connection->le_conn_interval_min = le_conn_interval_min; 1637 connection->le_conn_interval_max = le_conn_interval_max; 1638 connection->le_conn_latency = le_conn_latency; 1639 connection->le_supervision_timeout = le_supervision_timeout; 1640 } else { 1641 connection->le_con_parameter_update_state = CON_PARAMETER_UPDATE_DENY; 1642 } 1643 connection->le_con_param_update_identifier = sig_id; 1644 } 1645 1646 if (!l2cap_event_packet_handler) break; 1647 1648 event[0] = L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_REQUEST; 1649 event[1] = 8; 1650 memcpy(&event[2], &command[4], 8); 1651 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1652 (*l2cap_event_packet_handler)( HCI_EVENT_PACKET, 0, event, sizeof(event)); 1653 break; 1654 1655 #ifdef ENABLE_LE_DATA_CHANNELS 1656 1657 case COMMAND_REJECT: 1658 // Find channel for this sig_id and connection handle 1659 channel = NULL; 1660 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1661 while (btstack_linked_list_iterator_has_next(&it)){ 1662 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1663 if (a_channel->con_handle != handle) continue; 1664 if (a_channel->local_sig_id != sig_id) continue; 1665 channel = a_channel; 1666 break; 1667 } 1668 if (!channel) break; 1669 1670 // if received while waiting for le connection response, assume legacy device 1671 if (channel->state == L2CAP_STATE_WAIT_LE_CONNECTION_RESPONSE){ 1672 channel->state = L2CAP_STATE_CLOSED; 1673 // no official value for this, use: Connection refused – LE_PSM not supported - 0x0002 1674 l2cap_emit_le_channel_opened(channel, 0x0002); 1675 1676 // discard channel 1677 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 1678 btstack_memory_l2cap_channel_free(channel); 1679 break; 1680 } 1681 break; 1682 1683 case LE_CREDIT_BASED_CONNECTION_REQUEST: 1684 1685 // get hci connection, bail if not found (must not happen) 1686 connection = hci_connection_for_handle(handle); 1687 if (!connection) return 0; 1688 1689 // check if service registered 1690 le_psm = little_endian_read_16(command, 4); 1691 service = l2cap_le_get_service(le_psm); 1692 1693 if (service){ 1694 1695 uint16_t source_cid = little_endian_read_16(command, 6); 1696 if (source_cid < 0x40){ 1697 // 0x0009 Connection refused - Invalid Source CID 1698 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0009); 1699 return 1; 1700 } 1701 1702 // go through list of channels for this ACL connection and check if we get a match 1703 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1704 while (btstack_linked_list_iterator_has_next(&it)){ 1705 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1706 if (a_channel->con_handle != handle) continue; 1707 if (a_channel->remote_cid != source_cid) continue; 1708 // 0x000a Connection refused - Source CID already allocated 1709 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x000a); 1710 return 1; 1711 } 1712 1713 // security: check encryption 1714 if (service->required_security_level >= LEVEL_2){ 1715 if (sm_encryption_key_size(handle) == 0){ 1716 // 0x0008 Connection refused - insufficient encryption 1717 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0008); 1718 return 1; 1719 } 1720 // anything less than 16 byte key size is insufficient 1721 if (sm_encryption_key_size(handle) < 16){ 1722 // 0x0007 Connection refused – insufficient encryption key size 1723 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0007); 1724 return 1; 1725 } 1726 } 1727 1728 // security: check authencation 1729 if (service->required_security_level >= LEVEL_3){ 1730 if (!sm_authenticated(handle)){ 1731 // 0x0005 Connection refused – insufficient authentication 1732 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0005); 1733 return 1; 1734 } 1735 } 1736 1737 // security: check authorization 1738 if (service->required_security_level >= LEVEL_4){ 1739 if (sm_authorization_state(handle) != AUTHORIZATION_GRANTED){ 1740 // 0x0006 Connection refused – insufficient authorization 1741 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0006); 1742 return 1; 1743 } 1744 } 1745 1746 // allocate channel 1747 channel = l2cap_create_channel_entry(service->packet_handler, connection->address, 1748 BD_ADDR_TYPE_LE_RANDOM, le_psm, service->mtu, service->required_security_level); 1749 if (!channel){ 1750 // 0x0004 Connection refused – no resources available 1751 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0004); 1752 return 1; 1753 } 1754 1755 channel->con_handle = handle; 1756 channel->remote_cid = source_cid; 1757 channel->remote_sig_id = sig_id; 1758 channel->remote_mtu = little_endian_read_16(command, 8); 1759 channel->remote_mps = little_endian_read_16(command, 10); 1760 channel->credits_outgoing = little_endian_read_16(command, 12); 1761 1762 // set initial state 1763 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 1764 channel->state_var |= L2CAP_CHANNEL_STATE_VAR_INCOMING; 1765 1766 // add to connections list 1767 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 1768 1769 // post connection request event 1770 l2cap_emit_le_incoming_connection(channel); 1771 1772 } else { 1773 // Connection refused – LE_PSM not supported 1774 l2cap_register_signaling_response(handle, LE_CREDIT_BASED_CONNECTION_REQUEST, sig_id, source_cid, 0x0002); 1775 } 1776 break; 1777 1778 case LE_CREDIT_BASED_CONNECTION_RESPONSE: 1779 // Find channel for this sig_id and connection handle 1780 channel = NULL; 1781 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 1782 while (btstack_linked_list_iterator_has_next(&it)){ 1783 l2cap_channel_t * a_channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 1784 if (a_channel->con_handle != handle) continue; 1785 if (a_channel->local_sig_id != sig_id) continue; 1786 channel = a_channel; 1787 break; 1788 } 1789 if (!channel) break; 1790 1791 // cid + 0 1792 result = little_endian_read_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+8); 1793 if (result){ 1794 channel->state = L2CAP_STATE_CLOSED; 1795 // map l2cap connection response result to BTstack status enumeration 1796 l2cap_emit_le_channel_opened(channel, result); 1797 1798 // discard channel 1799 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 1800 btstack_memory_l2cap_channel_free(channel); 1801 break; 1802 } 1803 1804 // success 1805 channel->remote_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 1806 channel->remote_mtu = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 1807 channel->remote_mps = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 4); 1808 channel->credits_outgoing = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 6); 1809 channel->state = L2CAP_STATE_OPEN; 1810 l2cap_emit_le_channel_opened(channel, result); 1811 break; 1812 1813 case LE_FLOW_CONTROL_CREDIT: 1814 // find channel 1815 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 1816 channel = l2cap_le_get_channel_for_local_cid(local_cid); 1817 if (!channel) { 1818 log_error("l2cap: no channel for cid 0x%02x", local_cid); 1819 break; 1820 } 1821 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2); 1822 credits_before = channel->credits_outgoing; 1823 channel->credits_outgoing += new_credits; 1824 // check for credit overrun 1825 if (credits_before > channel->credits_outgoing){ 1826 log_error("l2cap: new credits caused overrrun for cid 0x%02x, disconnecting", local_cid); 1827 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1828 break; 1829 } 1830 log_info("l2cap: %u credits for 0x%02x, now %u", new_credits, local_cid, channel->credits_outgoing); 1831 break; 1832 1833 case DISCONNECTION_REQUEST: 1834 // find channel 1835 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0); 1836 channel = l2cap_le_get_channel_for_local_cid(local_cid); 1837 if (!channel) { 1838 log_error("l2cap: no channel for cid 0x%02x", local_cid); 1839 break; 1840 } 1841 channel->remote_sig_id = sig_id; 1842 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE; 1843 break; 1844 1845 #endif 1846 1847 case DISCONNECTION_RESPONSE: 1848 break; 1849 1850 default: 1851 // command unknown -> reject command 1852 return 0; 1853 } 1854 return 1; 1855 } 1856 #endif 1857 1858 static void l2cap_acl_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size ){ 1859 UNUSED(packet_type); 1860 UNUSED(channel); 1861 1862 l2cap_channel_t * l2cap_channel; 1863 UNUSED(l2cap_channel); 1864 1865 // Get Channel ID 1866 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 1867 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 1868 1869 switch (channel_id) { 1870 1871 #ifdef ENABLE_CLASSIC 1872 case L2CAP_CID_SIGNALING: { 1873 uint16_t command_offset = 8; 1874 while (command_offset < size) { 1875 // handle signaling commands 1876 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 1877 1878 // increment command_offset 1879 command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 1880 } 1881 break; 1882 } 1883 #endif 1884 1885 #ifdef ENABLE_BLE 1886 case L2CAP_CID_SIGNALING_LE: { 1887 uint16_t sig_id = packet[COMPLETE_L2CAP_HEADER + 1]; 1888 int valid = l2cap_le_signaling_handler_dispatch(handle, &packet[COMPLETE_L2CAP_HEADER], sig_id); 1889 if (!valid){ 1890 l2cap_register_signaling_response(handle, COMMAND_REJECT_LE, sig_id, 0, L2CAP_REJ_CMD_UNKNOWN); 1891 } 1892 break; 1893 } 1894 #endif 1895 1896 case L2CAP_CID_ATTRIBUTE_PROTOCOL: 1897 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback) { 1898 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_ATTRIBUTE_PROTOCOL].callback)(ATT_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 1899 } 1900 break; 1901 1902 case L2CAP_CID_SECURITY_MANAGER_PROTOCOL: 1903 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback) { 1904 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_SECURITY_MANAGER_PROTOCOL].callback)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 1905 } 1906 break; 1907 1908 case L2CAP_CID_CONNECTIONLESS_CHANNEL: 1909 if (fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback) { 1910 (*fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_INDEX_CONNECTIONLESS_CHANNEL].callback)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 1911 } 1912 break; 1913 1914 default: 1915 #ifdef ENABLE_CLASSIC 1916 // Find channel for this channel_id and connection handle 1917 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id); 1918 if (l2cap_channel) { 1919 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 1920 } 1921 #endif 1922 #ifdef ENABLE_LE_DATA_CHANNELS 1923 l2cap_channel = l2cap_le_get_channel_for_local_cid(channel_id); 1924 if (l2cap_channel) { 1925 // credit counting 1926 if (l2cap_channel->credits_incoming == 0){ 1927 log_error("LE Data Channel packet received but no incoming credits"); 1928 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 1929 break; 1930 } 1931 l2cap_channel->credits_incoming--; 1932 1933 // automatic credits 1934 if (l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK && l2cap_channel->automatic_credits){ 1935 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT; 1936 } 1937 1938 // first fragment 1939 uint16_t pos = 0; 1940 if (!l2cap_channel->receive_sdu_len){ 1941 l2cap_channel->receive_sdu_len = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER); 1942 l2cap_channel->receive_sdu_pos = 0; 1943 pos += 2; 1944 size -= 2; 1945 } 1946 memcpy(&l2cap_channel->receive_sdu_buffer[l2cap_channel->receive_sdu_pos], &packet[COMPLETE_L2CAP_HEADER+pos], size-COMPLETE_L2CAP_HEADER); 1947 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER; 1948 // done? 1949 log_info("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len); 1950 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){ 1951 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len); 1952 l2cap_channel->receive_sdu_len = 0; 1953 } 1954 } else { 1955 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id); 1956 } 1957 #endif 1958 break; 1959 } 1960 1961 l2cap_run(); 1962 } 1963 1964 // Bluetooth 4.0 - allows to register handler for Attribute Protocol and Security Manager Protocol 1965 void l2cap_register_fixed_channel(btstack_packet_handler_t the_packet_handler, uint16_t channel_id) { 1966 int index = l2cap_fixed_channel_table_index_for_channel_id(channel_id); 1967 if (index < 0) return; 1968 fixed_channels[index].callback = the_packet_handler; 1969 } 1970 1971 #ifdef ENABLE_CLASSIC 1972 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 1973 void l2cap_finialize_channel_close(l2cap_channel_t * channel){ 1974 channel->state = L2CAP_STATE_CLOSED; 1975 l2cap_emit_channel_closed(channel); 1976 // discard channel 1977 l2cap_stop_rtx(channel); 1978 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel); 1979 btstack_memory_l2cap_channel_free(channel); 1980 } 1981 1982 static l2cap_service_t * l2cap_get_service_internal(btstack_linked_list_t * services, uint16_t psm){ 1983 btstack_linked_list_iterator_t it; 1984 btstack_linked_list_iterator_init(&it, services); 1985 while (btstack_linked_list_iterator_has_next(&it)){ 1986 l2cap_service_t * service = (l2cap_service_t *) btstack_linked_list_iterator_next(&it); 1987 if ( service->psm == psm){ 1988 return service; 1989 }; 1990 } 1991 return NULL; 1992 } 1993 1994 static inline l2cap_service_t * l2cap_get_service(uint16_t psm){ 1995 return l2cap_get_service_internal(&l2cap_services, psm); 1996 } 1997 1998 1999 uint8_t l2cap_register_service(btstack_packet_handler_t service_packet_handler, uint16_t psm, uint16_t mtu, gap_security_level_t security_level){ 2000 2001 log_info("L2CAP_REGISTER_SERVICE psm 0x%x mtu %u", psm, mtu); 2002 2003 // check for alread registered psm 2004 l2cap_service_t *service = l2cap_get_service(psm); 2005 if (service) { 2006 log_error("l2cap_register_service: PSM %u already registered", psm); 2007 return L2CAP_SERVICE_ALREADY_REGISTERED; 2008 } 2009 2010 // alloc structure 2011 service = btstack_memory_l2cap_service_get(); 2012 if (!service) { 2013 log_error("l2cap_register_service: no memory for l2cap_service_t"); 2014 return BTSTACK_MEMORY_ALLOC_FAILED; 2015 } 2016 2017 // fill in 2018 service->psm = psm; 2019 service->mtu = mtu; 2020 service->packet_handler = service_packet_handler; 2021 service->required_security_level = security_level; 2022 2023 // add to services list 2024 btstack_linked_list_add(&l2cap_services, (btstack_linked_item_t *) service); 2025 2026 // enable page scan 2027 gap_connectable_control(1); 2028 2029 return 0; 2030 } 2031 2032 uint8_t l2cap_unregister_service(uint16_t psm){ 2033 2034 log_info("L2CAP_UNREGISTER_SERVICE psm 0x%x", psm); 2035 2036 l2cap_service_t *service = l2cap_get_service(psm); 2037 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 2038 btstack_linked_list_remove(&l2cap_services, (btstack_linked_item_t *) service); 2039 btstack_memory_l2cap_service_free(service); 2040 2041 // disable page scan when no services registered 2042 if (btstack_linked_list_empty(&l2cap_services)) { 2043 gap_connectable_control(0); 2044 } 2045 return 0; 2046 } 2047 #endif 2048 2049 2050 #ifdef ENABLE_LE_DATA_CHANNELS 2051 2052 static void l2cap_le_notify_channel_can_send(l2cap_channel_t *channel){ 2053 if (!channel->waiting_for_can_send_now) return; 2054 if (channel->send_sdu_buffer) return; 2055 channel->waiting_for_can_send_now = 0; 2056 log_info("L2CAP_EVENT_CHANNEL_LE_CAN_SEND_NOW local_cid 0x%x", channel->local_cid); 2057 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_LE_CAN_SEND_NOW); 2058 } 2059 2060 // 1BH2222 2061 static void l2cap_emit_le_incoming_connection(l2cap_channel_t *channel) { 2062 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", 2063 channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, channel->local_cid, channel->remote_cid, channel->remote_mtu); 2064 uint8_t event[19]; 2065 event[0] = L2CAP_EVENT_LE_INCOMING_CONNECTION; 2066 event[1] = sizeof(event) - 2; 2067 event[2] = channel->address_type; 2068 reverse_bd_addr(channel->address, &event[3]); 2069 little_endian_store_16(event, 9, channel->con_handle); 2070 little_endian_store_16(event, 11, channel->psm); 2071 little_endian_store_16(event, 13, channel->local_cid); 2072 little_endian_store_16(event, 15, channel->remote_cid); 2073 little_endian_store_16(event, 17, channel->remote_mtu); 2074 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2075 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 2076 } 2077 // 11BH22222 2078 static void l2cap_emit_le_channel_opened(l2cap_channel_t *channel, uint8_t status) { 2079 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", 2080 status, channel->address_type, bd_addr_to_str(channel->address), channel->con_handle, channel->psm, 2081 channel->local_cid, channel->remote_cid, channel->local_mtu, channel->remote_mtu); 2082 uint8_t event[23]; 2083 event[0] = L2CAP_EVENT_LE_CHANNEL_OPENED; 2084 event[1] = sizeof(event) - 2; 2085 event[2] = status; 2086 event[3] = channel->address_type; 2087 reverse_bd_addr(channel->address, &event[4]); 2088 little_endian_store_16(event, 10, channel->con_handle); 2089 event[12] = channel->state_var & L2CAP_CHANNEL_STATE_VAR_INCOMING ? 1 : 0; 2090 little_endian_store_16(event, 13, channel->psm); 2091 little_endian_store_16(event, 15, channel->local_cid); 2092 little_endian_store_16(event, 17, channel->remote_cid); 2093 little_endian_store_16(event, 19, channel->local_mtu); 2094 little_endian_store_16(event, 21, channel->remote_mtu); 2095 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 2096 l2cap_dispatch_to_channel(channel, HCI_EVENT_PACKET, event, sizeof(event)); 2097 } 2098 2099 static l2cap_channel_t * l2cap_le_get_channel_for_local_cid(uint16_t local_cid){ 2100 btstack_linked_list_iterator_t it; 2101 btstack_linked_list_iterator_init(&it, &l2cap_le_channels); 2102 while (btstack_linked_list_iterator_has_next(&it)){ 2103 l2cap_channel_t * channel = (l2cap_channel_t *) btstack_linked_list_iterator_next(&it); 2104 if ( channel->local_cid == local_cid) { 2105 return channel; 2106 } 2107 } 2108 return NULL; 2109 } 2110 2111 // finalize closed channel - l2cap_handle_disconnect_request & DISCONNECTION_RESPONSE 2112 void l2cap_le_finialize_channel_close(l2cap_channel_t * channel){ 2113 channel->state = L2CAP_STATE_CLOSED; 2114 l2cap_emit_simple_event_with_cid(channel, L2CAP_EVENT_CHANNEL_CLOSED); 2115 // discard channel 2116 btstack_linked_list_remove(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2117 btstack_memory_l2cap_channel_free(channel); 2118 } 2119 2120 static inline l2cap_service_t * l2cap_le_get_service(uint16_t le_psm){ 2121 return l2cap_get_service_internal(&l2cap_le_services, le_psm); 2122 } 2123 2124 uint8_t l2cap_le_register_service(btstack_packet_handler_t packet_handler, uint16_t psm, gap_security_level_t security_level){ 2125 2126 log_info("L2CAP_LE_REGISTER_SERVICE psm 0x%x", psm); 2127 2128 // check for alread registered psm 2129 l2cap_service_t *service = l2cap_le_get_service(psm); 2130 if (service) { 2131 return L2CAP_SERVICE_ALREADY_REGISTERED; 2132 } 2133 2134 // alloc structure 2135 service = btstack_memory_l2cap_service_get(); 2136 if (!service) { 2137 log_error("l2cap_register_service_internal: no memory for l2cap_service_t"); 2138 return BTSTACK_MEMORY_ALLOC_FAILED; 2139 } 2140 2141 // fill in 2142 service->psm = psm; 2143 service->mtu = 0; 2144 service->packet_handler = packet_handler; 2145 service->required_security_level = security_level; 2146 2147 // add to services list 2148 btstack_linked_list_add(&l2cap_le_services, (btstack_linked_item_t *) service); 2149 2150 // done 2151 return 0; 2152 } 2153 2154 uint8_t l2cap_le_unregister_service(uint16_t psm) { 2155 log_info("L2CAP_LE_UNREGISTER_SERVICE psm 0x%x", psm); 2156 l2cap_service_t *service = l2cap_le_get_service(psm); 2157 if (!service) return L2CAP_SERVICE_DOES_NOT_EXIST; 2158 2159 btstack_linked_list_remove(&l2cap_le_services, (btstack_linked_item_t *) service); 2160 btstack_memory_l2cap_service_free(service); 2161 return 0; 2162 } 2163 2164 uint8_t l2cap_le_accept_connection(uint16_t local_cid, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits){ 2165 // get channel 2166 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2167 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2168 2169 // validate state 2170 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 2171 return ERROR_CODE_COMMAND_DISALLOWED; 2172 } 2173 2174 // set state accept connection 2175 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_ACCEPT; 2176 channel->receive_sdu_buffer = receive_sdu_buffer; 2177 channel->local_mtu = mtu; 2178 channel->new_credits_incoming = initial_credits; 2179 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 2180 2181 // test 2182 // channel->new_credits_incoming = 1; 2183 2184 // go 2185 l2cap_run(); 2186 return 0; 2187 } 2188 2189 /** 2190 * @brief Deny incoming LE Data Channel connection due to resource constraints 2191 * @param local_cid L2CAP LE Data Channel Identifier 2192 */ 2193 2194 uint8_t l2cap_le_decline_connection(uint16_t local_cid){ 2195 // get channel 2196 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2197 if (!channel) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2198 2199 // validate state 2200 if (channel->state != L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT){ 2201 return ERROR_CODE_COMMAND_DISALLOWED; 2202 } 2203 2204 // set state decline connection 2205 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_RESPONSE_DECLINE; 2206 channel->reason = 0x04; // no resources available 2207 l2cap_run(); 2208 return 0; 2209 } 2210 2211 uint8_t l2cap_le_create_channel(btstack_packet_handler_t packet_handler, hci_con_handle_t con_handle, 2212 uint16_t psm, uint8_t * receive_sdu_buffer, uint16_t mtu, uint16_t initial_credits, gap_security_level_t security_level, 2213 uint16_t * out_local_cid) { 2214 2215 log_info("L2CAP_LE_CREATE_CHANNEL handle 0x%04x psm 0x%x mtu %u", con_handle, psm, mtu); 2216 2217 2218 hci_connection_t * connection = hci_connection_for_handle(con_handle); 2219 if (!connection) { 2220 log_error("no hci_connection for handle 0x%04x", con_handle); 2221 return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 2222 } 2223 2224 l2cap_channel_t * channel = l2cap_create_channel_entry(packet_handler, connection->address, connection->address_type, psm, mtu, security_level); 2225 if (!channel) { 2226 return BTSTACK_MEMORY_ALLOC_FAILED; 2227 } 2228 log_info("l2cap_le_create_channel %p", channel); 2229 2230 // store local_cid 2231 if (out_local_cid){ 2232 *out_local_cid = channel->local_cid; 2233 } 2234 2235 // provide buffer 2236 channel->con_handle = con_handle; 2237 channel->receive_sdu_buffer = receive_sdu_buffer; 2238 channel->state = L2CAP_STATE_WILL_SEND_LE_CONNECTION_REQUEST; 2239 channel->new_credits_incoming = initial_credits; 2240 channel->automatic_credits = initial_credits == L2CAP_LE_AUTOMATIC_CREDITS; 2241 2242 // add to connections list 2243 btstack_linked_list_add(&l2cap_le_channels, (btstack_linked_item_t *) channel); 2244 2245 // go 2246 l2cap_run(); 2247 return 0; 2248 } 2249 2250 /** 2251 * @brief Provide credtis for LE Data Channel 2252 * @param local_cid L2CAP LE Data Channel Identifier 2253 * @param credits Number additional credits for peer 2254 */ 2255 uint8_t l2cap_le_provide_credits(uint16_t local_cid, uint16_t credits){ 2256 2257 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2258 if (!channel) { 2259 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 2260 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2261 } 2262 2263 // check state 2264 if (channel->state != L2CAP_STATE_OPEN){ 2265 log_error("l2cap_le_provide_credits but channel 0x%02x not open yet", local_cid); 2266 } 2267 2268 // assert incoming credits + credits <= 0xffff 2269 uint32_t total_credits = channel->credits_incoming; 2270 total_credits += channel->new_credits_incoming; 2271 total_credits += credits; 2272 if (total_credits > 0xffff){ 2273 log_error("l2cap_le_provide_credits overrun: current %u, scheduled %u, additional %u", channel->credits_incoming, 2274 channel->new_credits_incoming, credits); 2275 } 2276 2277 // set credits_granted 2278 channel->new_credits_incoming += credits; 2279 2280 // go 2281 l2cap_run(); 2282 return 0; 2283 } 2284 2285 /** 2286 * @brief Check if outgoing buffer is available and that there's space on the Bluetooth module 2287 * @param local_cid L2CAP LE Data Channel Identifier 2288 */ 2289 int l2cap_le_can_send_now(uint16_t local_cid){ 2290 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2291 if (!channel) { 2292 log_error("l2cap_le_provide_credits no channel for cid 0x%02x", local_cid); 2293 return 0; 2294 } 2295 2296 // check state 2297 if (channel->state != L2CAP_STATE_OPEN) return 0; 2298 2299 // check queue 2300 if (channel->send_sdu_buffer) return 0; 2301 2302 // fine, go ahead 2303 return 1; 2304 } 2305 2306 /** 2307 * @brief Request emission of L2CAP_EVENT_CAN_SEND_NOW as soon as possible 2308 * @note L2CAP_EVENT_CAN_SEND_NOW might be emitted during call to this function 2309 * so packet handler should be ready to handle it 2310 * @param local_cid L2CAP LE Data Channel Identifier 2311 */ 2312 uint8_t l2cap_le_request_can_send_now_event(uint16_t local_cid){ 2313 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2314 if (!channel) { 2315 log_error("l2cap_le_request_can_send_now_event no channel for cid 0x%02x", local_cid); 2316 return 0; 2317 } 2318 channel->waiting_for_can_send_now = 1; 2319 l2cap_le_notify_channel_can_send(channel); 2320 return 0; 2321 } 2322 2323 /** 2324 * @brief Send data via LE Data Channel 2325 * @note Since data larger then the maximum PDU needs to be segmented into multiple PDUs, data needs to stay valid until ... event 2326 * @param local_cid L2CAP LE Data Channel Identifier 2327 * @param data data to send 2328 * @param size data size 2329 */ 2330 uint8_t l2cap_le_send_data(uint16_t local_cid, uint8_t * data, uint16_t len){ 2331 2332 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2333 if (!channel) { 2334 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 2335 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2336 } 2337 2338 if (len > channel->remote_mtu){ 2339 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", local_cid); 2340 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU; 2341 } 2342 2343 if (channel->send_sdu_buffer){ 2344 log_info("l2cap_send cid 0x%02x, cannot send", local_cid); 2345 return BTSTACK_ACL_BUFFERS_FULL; 2346 } 2347 2348 channel->send_sdu_buffer = data; 2349 channel->send_sdu_len = len; 2350 channel->send_sdu_pos = 0; 2351 2352 l2cap_run(); 2353 return 0; 2354 } 2355 2356 /** 2357 * @brief Disconnect from LE Data Channel 2358 * @param local_cid L2CAP LE Data Channel Identifier 2359 */ 2360 uint8_t l2cap_le_disconnect(uint16_t local_cid) 2361 { 2362 l2cap_channel_t * channel = l2cap_le_get_channel_for_local_cid(local_cid); 2363 if (!channel) { 2364 log_error("l2cap_send no channel for cid 0x%02x", local_cid); 2365 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 2366 } 2367 2368 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST; 2369 l2cap_run(); 2370 return 0; 2371 } 2372 2373 #endif 2374