1 /* 2 * Copyright (C) 2009 by Matthias Ringwald 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 * 17 * THIS SOFTWARE IS PROVIDED BY MATTHIAS RINGWALD AND CONTRIBUTORS 18 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 20 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 21 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 22 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 23 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 24 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 25 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 27 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 28 * SUCH DAMAGE. 29 * 30 */ 31 32 /* 33 * l2cap.c 34 * 35 * Logical Link Control and Adaption Protocl (L2CAP) 36 * 37 * Created by Matthias Ringwald on 5/16/09. 38 */ 39 40 #include "l2cap.h" 41 #include "hci.h" 42 #include "hci_dump.h" 43 44 #include <stdarg.h> 45 #include <string.h> 46 47 #include <stdio.h> 48 49 // size of HCI ACL + L2CAP Header for regular data packets 50 #define COMPLETE_L2CAP_HEADER 8 51 52 static void null_event_handler(uint8_t *packet, uint16_t size); 53 static void null_data_handler(uint16_t local_cid, uint8_t *packet, uint16_t size); 54 55 static uint8_t * sig_buffer = NULL; 56 static linked_list_t l2cap_channels = NULL; 57 static linked_list_t l2cap_services = NULL; 58 static uint8_t * acl_buffer = NULL; 59 static void (*event_packet_handler) (uint8_t *packet, uint16_t size) = null_event_handler; 60 static void (*data_packet_handler) (uint16_t local_cid, uint8_t *packet, uint16_t size) = null_data_handler; 61 static connection_t * capture_connection = NULL; 62 63 static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 64 65 void l2cap_init(){ 66 sig_buffer = malloc( 48 ); 67 acl_buffer = malloc( 255 + 8 ); 68 69 // 70 // register callbacks with HCI 71 // 72 hci_register_event_packet_handler(&l2cap_event_handler); 73 hci_register_acl_packet_handler(&l2cap_acl_handler); 74 } 75 76 77 /** Register L2CAP packet handlers */ 78 static void null_event_handler(uint8_t *packet, uint16_t size){ 79 } 80 static void null_data_handler(uint16_t local_cid, uint8_t *packet, uint16_t size){ 81 } 82 void l2cap_register_event_packet_handler(void (*handler)(uint8_t *packet, uint16_t size)){ 83 event_packet_handler = handler; 84 } 85 void l2cap_register_data_packet_handler (void (*handler)(uint16_t local_cid, uint8_t *packet, uint16_t size)){ 86 data_packet_handler = handler; 87 } 88 89 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 90 // printf("l2cap_send_signaling_packet type %u\n", cmd); 91 va_list argptr; 92 va_start(argptr, identifier); 93 uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr); 94 va_end(argptr); 95 return hci_send_acl_packet(sig_buffer, len); 96 } 97 98 l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){ 99 linked_item_t *it; 100 l2cap_channel_t * channel; 101 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 102 channel = (l2cap_channel_t *) it; 103 if ( channel->local_cid == local_cid) { 104 return channel; 105 } 106 } 107 return NULL; 108 } 109 110 // open outgoing L2CAP channel 111 void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address, uint16_t psm){ 112 113 // alloc structure 114 l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t)); 115 // TODO: emit error event 116 if (!chan) return; 117 118 // fill in 119 BD_ADDR_COPY(chan->address, address); 120 chan->psm = psm; 121 chan->handle = 0; 122 chan->connection = connection; 123 124 // set initial state 125 chan->state = L2CAP_STATE_CLOSED; 126 chan->sig_id = L2CAP_SIG_ID_INVALID; 127 128 // add to connections list 129 linked_list_add(&l2cap_channels, (linked_item_t *) chan); 130 131 // send connection request 132 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 133 hci_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0); 134 } 135 136 void l2cap_disconnect_internal(uint16_t local_cid, uint8_t reason){ 137 // find channel for local_cid 138 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 139 if (channel) { 140 channel->sig_id = l2cap_next_sig_id(); 141 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->remote_cid, channel->local_cid); 142 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 143 } 144 } 145 146 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 147 linked_item_t *it; 148 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 149 l2cap_channel_t * channel = (l2cap_channel_t *) it; 150 if ( ! BD_ADDR_CMP( channel->address, address) ){ 151 if (channel->state == L2CAP_STATE_CLOSED) { 152 // failure, forward error code 153 l2cap_emit_channel_opened(channel, status); 154 // discard channel 155 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 156 free (channel); 157 } 158 } 159 } 160 } 161 162 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 163 linked_item_t *it; 164 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 165 l2cap_channel_t * channel = (l2cap_channel_t *) it; 166 if ( ! BD_ADDR_CMP( channel->address, address) ){ 167 if (channel->state == L2CAP_STATE_CLOSED) { 168 // success, start l2cap handshake 169 channel->handle = handle; 170 channel->sig_id = l2cap_next_sig_id(); 171 channel->local_cid = l2cap_next_local_cid(); 172 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 173 l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->sig_id, channel->psm, channel->local_cid); 174 } 175 } 176 } 177 } 178 179 void l2cap_event_handler( uint8_t *packet, uint16_t size ){ 180 181 bd_addr_t address; 182 hci_con_handle_t handle; 183 184 switch(packet[0]){ 185 186 // handle connection complete events 187 case HCI_EVENT_CONNECTION_COMPLETE: 188 bt_flip_addr(address, &packet[5]); 189 if (packet[2] == 0){ 190 handle = READ_BT_16(packet, 3); 191 l2cap_handle_connection_success_for_addr(address, handle); 192 } else { 193 l2cap_handle_connection_failed_for_addr(address, packet[2]); 194 } 195 break; 196 197 // handle successful create connection cancel command 198 case HCI_EVENT_COMMAND_COMPLETE: 199 if ( COMMAND_COMPLETE_EVENT(packet, hci_create_connection_cancel) ) { 200 if (packet[5] == 0){ 201 bt_flip_addr(address, &packet[6]); 202 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 203 l2cap_handle_connection_failed_for_addr(address, 0x16); 204 } 205 } 206 break; 207 208 // handle disconnection complete events 209 case HCI_EVENT_DISCONNECTION_COMPLETE: 210 // send l2cap disconnect events for all channels on this handle 211 handle = READ_BT_16(packet, 3); 212 linked_item_t *it; 213 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 214 l2cap_channel_t * channel = (l2cap_channel_t *) it; 215 if ( channel->handle == handle ){ 216 l2cap_finialize_channel_close(channel); 217 } 218 } 219 break; 220 221 // HCI Connection Timeouts 222 case L2CAP_EVENT_TIMEOUT_CHECK: 223 if (!capture_connection){ 224 hci_con_handle_t handle = READ_BT_16(packet, 2); 225 linked_item_t *it; 226 l2cap_channel_t * channel; 227 int used = 0; 228 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 229 channel = (l2cap_channel_t *) it; 230 if (channel->handle == handle) { 231 used = 1; 232 } 233 } 234 if (!used) { 235 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 236 } 237 } 238 break; 239 240 default: 241 break; 242 } 243 244 // pass on 245 (*event_packet_handler)(packet, size); 246 } 247 248 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 249 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, identifier, channel->remote_cid, channel->local_cid); 250 l2cap_finialize_channel_close(channel); 251 } 252 253 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 254 255 // printf("l2cap_handle_connection_request for handle %u, psm %u cid %u\n", handle, psm, source_cid); 256 l2cap_service_t *service = l2cap_get_service(psm); 257 if (!service) { 258 // 0x0002 PSM not supported 259 // printf("l2cap_handle_connection_request no PSM for psm %u/n", psm); 260 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, 0, 0, 0x0002, 0); 261 return; 262 } 263 264 hci_connection_t * hci_connection = connection_for_handle( handle ); 265 if (!hci_connection) { 266 fprintf(stderr, "no hci_connection for handle %u\n", handle); 267 // TODO: emit error 268 return; 269 } 270 // alloc structure 271 // printf("l2cap_handle_connection_request register channel\n"); 272 l2cap_channel_t * channel = malloc(sizeof(l2cap_channel_t)); 273 // TODO: emit error event 274 if (!channel) return; 275 276 // fill in 277 BD_ADDR_COPY(channel->address, hci_connection->address); 278 channel->psm = psm; 279 channel->handle = handle; 280 channel->connection = service->connection; 281 channel->local_cid = l2cap_next_local_cid(); 282 channel->remote_cid = source_cid; 283 284 // set initial state 285 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 286 287 // temp. store req sig id 288 channel->sig_id = sig_id; 289 290 // add to connections list 291 linked_list_add(&l2cap_channels, (linked_item_t *) channel); 292 293 // emit incoming connection request 294 l2cap_emit_connection_request(channel); 295 } 296 297 void l2cap_accept_connection_internal(uint16_t local_cid){ 298 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 299 if (!channel) { 300 fprintf(stderr, "l2cap_accept_connection_internal called but local_cid 0x%x not found", local_cid); 301 return; 302 } 303 304 // accept connection 305 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->sig_id, channel->local_cid, channel->remote_cid, 0, 0); 306 307 // set real sig and state and start config 308 channel->sig_id = l2cap_next_sig_id(); 309 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ; 310 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->remote_cid, 0, 4, &config_options); 311 312 // printf("new state %u\n", channel->state); 313 } 314 315 void l2cap_decline_connection_internal(uint16_t local_cid, uint8_t reason){ 316 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 317 if (!channel) { 318 fprintf(stderr, "l2cap_decline_connection_internal called but local_cid 0x%x not found", local_cid,reason); 319 return; 320 } 321 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->sig_id, 0, 0, reason, 0); 322 323 // discard channel 324 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 325 free (channel); 326 } 327 328 void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){ 329 330 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 331 uint8_t identifier = READ_L2CAP_SIGNALING_IDENTIFIER( packet ); 332 uint16_t result = 0; 333 334 // printf("signaling handler code %u\n", code); 335 336 switch (channel->state) { 337 338 case L2CAP_STATE_WAIT_CONNECT_RSP: 339 switch (code){ 340 case CONNECTION_RESPONSE: 341 result = READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+4); 342 switch (result) { 343 case 0: 344 // successful connection 345 channel->remote_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 346 channel->sig_id = l2cap_next_sig_id(); 347 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->remote_cid, 0, 4, &config_options); 348 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ; 349 break; 350 case 1: 351 // connection pending. get some coffee 352 break; 353 default: 354 // map l2cap connection response result to BTstack status enumeration 355 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 356 break; 357 } 358 break; 359 360 case DISCONNECTION_REQUEST: 361 l2cap_handle_disconnect_request(channel, identifier); 362 break; 363 364 default: 365 //@TODO: implement other signaling packets 366 break; 367 } 368 break; 369 370 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ: 371 switch (code) { 372 case CONFIGURE_RESPONSE: 373 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; 374 break; 375 case CONFIGURE_REQUEST: 376 // accept the other's configuration options 377 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->remote_cid, 0, 0, size - 16, &packet[16]); 378 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; 379 break; 380 case DISCONNECTION_REQUEST: 381 l2cap_handle_disconnect_request(channel, identifier); 382 break; 383 default: 384 //@TODO: implement other signaling packets 385 break; 386 } 387 break; 388 389 case L2CAP_STATE_WAIT_CONFIG_REQ: 390 switch (code) { 391 case CONFIGURE_REQUEST: 392 // accept the other's configuration options 393 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->remote_cid, 0, 0, size - 16, &packet[16]); 394 channel->state = L2CAP_STATE_OPEN; 395 l2cap_emit_channel_opened(channel, 0); // success 396 break; 397 case DISCONNECTION_REQUEST: 398 l2cap_handle_disconnect_request(channel, identifier); 399 break; 400 default: 401 //@TODO: implement other signaling packets 402 break; 403 } 404 break; 405 406 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: 407 switch (code) { 408 case CONFIGURE_RESPONSE: 409 channel->state = L2CAP_STATE_OPEN; 410 l2cap_emit_channel_opened(channel, 0); // success 411 break; 412 case DISCONNECTION_REQUEST: 413 l2cap_handle_disconnect_request(channel, identifier); 414 break; 415 default: 416 //@TODO: implement other signaling packets 417 break; 418 } 419 break; 420 421 case L2CAP_STATE_WAIT_DISCONNECT: 422 switch (code) { 423 case DISCONNECTION_RESPONSE: 424 l2cap_finialize_channel_close(channel); 425 break; 426 case DISCONNECTION_REQUEST: 427 l2cap_handle_disconnect_request(channel, identifier); 428 break; 429 default: 430 //@TODO: implement other signaling packets 431 break; 432 } 433 break; 434 435 case L2CAP_STATE_CLOSED: 436 // @TODO handle incoming requests 437 break; 438 439 case L2CAP_STATE_OPEN: 440 switch (code) { 441 case DISCONNECTION_REQUEST: 442 l2cap_handle_disconnect_request(channel, identifier); 443 break; 444 default: 445 //@TODO: implement other signaling packets, e.g. re-configure 446 break; 447 } 448 break; 449 } 450 // printf("new state %u\n", channel->state); 451 } 452 453 // finalize closed channel 454 void l2cap_finialize_channel_close(l2cap_channel_t *channel){ 455 channel->state = L2CAP_STATE_CLOSED; 456 l2cap_emit_channel_closed(channel); 457 458 // discard channel 459 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 460 free (channel); 461 } 462 463 l2cap_service_t * l2cap_get_service(uint16_t psm){ 464 linked_item_t *it; 465 466 // close open channels 467 for (it = (linked_item_t *) l2cap_services; it ; it = it->next){ 468 l2cap_service_t * service = ((l2cap_service_t *) it); 469 if ( service->psm == psm){ 470 return service; 471 }; 472 } 473 return NULL; 474 } 475 476 void l2cap_register_service_internal(connection_t *connection, uint16_t psm, uint16_t mtu){ 477 // check for alread registered psm // TODO: emit error event 478 l2cap_service_t *service = l2cap_get_service(psm); 479 if (service) return; 480 481 // alloc structure // TODO: emit error event 482 service = malloc(sizeof(l2cap_service_t)); 483 if (!service) return; 484 485 // fill in 486 service->psm = psm; 487 service->mtu = mtu; 488 service->connection = connection; 489 490 // add to services list 491 linked_list_add(&l2cap_services, (linked_item_t *) service); 492 } 493 494 void l2cap_unregister_service_internal(connection_t *connection, uint16_t psm){ 495 l2cap_service_t *service = l2cap_get_service(psm); 496 if (!service) return; 497 linked_list_remove(&l2cap_services, (linked_item_t *) service); 498 free(service); 499 } 500 501 // 502 void l2cap_close_connection(connection_t *connection){ 503 linked_item_t *it; 504 505 // close open channels 506 l2cap_channel_t * channel; 507 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 508 channel = (l2cap_channel_t *) it; 509 if (channel->connection == connection) { 510 channel->sig_id = l2cap_next_sig_id(); 511 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->remote_cid, channel->local_cid); 512 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 513 } 514 } 515 516 // unregister services 517 l2cap_service_t *service; 518 for (it = (linked_item_t *) l2cap_services; it ; ){ 519 service = (l2cap_service_t *) it->next; 520 if (service->connection == connection){ 521 it->next = service->item.next; 522 free(service); 523 } else { 524 it = it->next; 525 } 526 } 527 } 528 529 // notify client 530 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 531 uint8_t event[17]; 532 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 533 event[1] = sizeof(event) - 2; 534 event[2] = status; 535 bt_flip_addr(&event[3], channel->address); 536 bt_store_16(event, 9, channel->handle); 537 bt_store_16(event, 11, channel->psm); 538 bt_store_16(event, 13, channel->local_cid); 539 bt_store_16(event, 15, channel->remote_cid); 540 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 541 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 542 } 543 544 void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 545 uint8_t event[4]; 546 event[0] = L2CAP_EVENT_CHANNEL_CLOSED; 547 event[1] = sizeof(event) - 2; 548 bt_store_16(event, 2, channel->local_cid); 549 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 550 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 551 } 552 553 void l2cap_emit_connection_request(l2cap_channel_t *channel) { 554 uint8_t event[16]; 555 event[0] = L2CAP_EVENT_INCOMING_CONNECTION; 556 event[1] = sizeof(event) - 2; 557 bt_flip_addr(&event[2], channel->address); 558 bt_store_16(event, 8, channel->handle); 559 bt_store_16(event, 10, channel->psm); 560 bt_store_16(event, 12, channel->local_cid); 561 bt_store_16(event, 14, channel->remote_cid); 562 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 563 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 564 } 565 566 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 567 568 // Capturing? 569 if (capture_connection) { 570 socket_connection_send_packet(capture_connection, HCI_ACL_DATA_PACKET, 0, packet, size); 571 return; 572 } 573 574 // forward to higher layers - not needed yet 575 // (*data_packet_handler)(channel_id, packet, size); 576 577 // Get Channel ID and command code 578 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 579 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 580 581 // Get Connection 582 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 583 584 // printf("l2cap_acl_handler channel %u, code %u\n", channel_id, code); 585 586 // Signaling Packet? 587 if (channel_id == 1) { 588 589 if (code < 1 || code >= 8){ 590 // not for a particular channel, and not CONNECTION_REQUEST 591 return; 592 } 593 594 // Get Signaling Identifier 595 uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); 596 597 // CONNECTION_REQUEST 598 if (code == CONNECTION_REQUEST){ 599 uint16_t psm = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 600 uint16_t source_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET+2); 601 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 602 return; 603 } 604 605 // Get potential destination CID 606 uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 607 608 // Find channel for this sig_id and connection handle 609 linked_item_t *it; 610 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 611 l2cap_channel_t * channel = (l2cap_channel_t *) it; 612 if (channel->handle == handle) { 613 if (code & 1) { 614 // match odd commands by previous signaling identifier 615 if (channel->sig_id == sig_id) { 616 l2cap_signaling_handler( channel, packet, size); 617 } 618 } else { 619 // match even commands by source channel id 620 if (channel->local_cid == dest_cid) { 621 l2cap_signaling_handler( channel, packet, size); 622 } 623 } 624 } 625 } 626 return; 627 } 628 629 // Find channel for this channel_id and connection handle 630 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(channel_id); 631 if (channel) { 632 socket_connection_send_packet(channel->connection, L2CAP_DATA_PACKET, channel_id, 633 &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 634 } 635 } 636 637 638 void l2cap_send_internal(uint16_t local_cid, uint8_t *data, uint16_t len){ 639 // find channel for local_cid, construct l2cap packet and send 640 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 641 if (channel) { 642 // 0 - Connection handle : PB=10 : BC=00 643 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 644 // 2 - ACL length 645 bt_store_16(acl_buffer, 2, len + 4); 646 // 4 - L2CAP packet length 647 bt_store_16(acl_buffer, 4, len + 0); 648 // 6 - L2CAP channel DEST 649 bt_store_16(acl_buffer, 6, channel->remote_cid); 650 // 8 - data 651 memcpy(&acl_buffer[8], data, len); 652 // send 653 hci_send_acl_packet(acl_buffer, len+8); 654 } 655 } 656 657 void l2cap_set_capture_connection(connection_t * connection){ 658 capture_connection = connection; 659 } 660 661