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