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 // minimum signaling MTU 53 #define L2CAP_MINIMAL_MTU 48 54 #define L2CAP_DEFAULT_MTU 672 55 56 // offsets for L2CAP SIGNALING COMMANDS 57 #define L2CAP_SIGNALING_COMMAND_CODE_OFFSET 0 58 #define L2CAP_SIGNALING_COMMAND_SIGID_OFFSET 1 59 #define L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET 2 60 #define L2CAP_SIGNALING_COMMAND_DATA_OFFSET 4 61 62 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 63 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size); 64 65 static uint8_t * sig_buffer = NULL; 66 static linked_list_t l2cap_channels = NULL; 67 static linked_list_t l2cap_services = NULL; 68 static uint8_t * acl_buffer = NULL; 69 static void (*packet_handler) (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) = null_packet_handler; 70 71 static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 72 73 void l2cap_init(){ 74 sig_buffer = malloc( L2CAP_MINIMAL_MTU ); 75 acl_buffer = malloc( HCI_ACL_3DH5_SIZE); 76 77 // 78 // register callback with HCI 79 // 80 hci_register_packet_handler(&l2cap_packet_handler); 81 } 82 83 84 /** Register L2CAP packet handlers */ 85 static void null_packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 86 } 87 void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 88 packet_handler = handler; 89 } 90 91 // notify client/protocol handler 92 void l2cap_dispatch(l2cap_channel_t *channel, uint8_t type, uint8_t * data, uint16_t size){ 93 if (channel->packet_handler) { 94 (* (channel->packet_handler))(type, channel->local_cid, data, size); 95 } else { 96 (*packet_handler)(channel->connection, type, channel->local_cid, data, size); 97 } 98 } 99 100 void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) { 101 uint8_t event[17]; 102 event[0] = L2CAP_EVENT_CHANNEL_OPENED; 103 event[1] = sizeof(event) - 2; 104 event[2] = status; 105 bt_flip_addr(&event[3], channel->address); 106 bt_store_16(event, 9, channel->handle); 107 bt_store_16(event, 11, channel->psm); 108 bt_store_16(event, 13, channel->local_cid); 109 bt_store_16(event, 15, channel->remote_cid); 110 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 111 l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event)); 112 } 113 114 void l2cap_emit_channel_closed(l2cap_channel_t *channel) { 115 uint8_t event[4]; 116 event[0] = L2CAP_EVENT_CHANNEL_CLOSED; 117 event[1] = sizeof(event) - 2; 118 bt_store_16(event, 2, channel->local_cid); 119 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 120 l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event)); 121 } 122 123 void l2cap_emit_connection_request(l2cap_channel_t *channel) { 124 uint8_t event[16]; 125 event[0] = L2CAP_EVENT_INCOMING_CONNECTION; 126 event[1] = sizeof(event) - 2; 127 bt_flip_addr(&event[2], channel->address); 128 bt_store_16(event, 8, channel->handle); 129 bt_store_16(event, 10, channel->psm); 130 bt_store_16(event, 12, channel->local_cid); 131 bt_store_16(event, 14, channel->remote_cid); 132 hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event)); 133 l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event)); 134 } 135 136 l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){ 137 linked_item_t *it; 138 l2cap_channel_t * channel; 139 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 140 channel = (l2cap_channel_t *) it; 141 if ( channel->local_cid == local_cid) { 142 return channel; 143 } 144 } 145 return NULL; 146 } 147 148 uint16_t l2cap_get_remote_mtu_for_local_cid(uint16_t local_cid){ 149 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 150 if (channel) { 151 return channel->remote_mtu; 152 } 153 return 0; 154 } 155 156 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 157 // printf("l2cap_send_signaling_packet type %u\n", cmd); 158 va_list argptr; 159 va_start(argptr, identifier); 160 uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr); 161 va_end(argptr); 162 return hci_send_acl_packet(sig_buffer, len); 163 } 164 165 void l2cap_send_internal(uint16_t local_cid, uint8_t *data, uint16_t len){ 166 // find channel for local_cid, construct l2cap packet and send 167 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 168 if (channel) { 169 // 0 - Connection handle : PB=10 : BC=00 170 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 171 // 2 - ACL length 172 bt_store_16(acl_buffer, 2, len + 4); 173 // 4 - L2CAP packet length 174 bt_store_16(acl_buffer, 4, len + 0); 175 // 6 - L2CAP channel DEST 176 bt_store_16(acl_buffer, 6, channel->remote_cid); 177 // 8 - data 178 memcpy(&acl_buffer[8], data, len); 179 // send 180 hci_send_acl_packet(acl_buffer, len+8); 181 } 182 } 183 184 // open outgoing L2CAP channel 185 void l2cap_create_channel_internal(void * connection, btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm){ 186 187 // alloc structure 188 l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t)); 189 // TODO: emit error event 190 if (!chan) return; 191 192 // fill in 193 BD_ADDR_COPY(chan->address, address); 194 chan->psm = psm; 195 chan->handle = 0; 196 chan->connection = connection; 197 chan->packet_handler = packet_handler; 198 chan->remote_mtu = L2CAP_MINIMAL_MTU; 199 200 // set initial state 201 chan->state = L2CAP_STATE_CLOSED; 202 chan->sig_id = L2CAP_SIG_ID_INVALID; 203 204 // add to connections list 205 linked_list_add(&l2cap_channels, (linked_item_t *) chan); 206 207 // send connection request 208 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 209 hci_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0); 210 } 211 212 void l2cap_disconnect_internal(uint16_t local_cid, uint8_t reason){ 213 // find channel for local_cid 214 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 215 if (channel) { 216 channel->sig_id = l2cap_next_sig_id(); 217 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->remote_cid, channel->local_cid); 218 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 219 } 220 } 221 222 static void l2cap_handle_connection_failed_for_addr(bd_addr_t address, uint8_t status){ 223 linked_item_t *it; 224 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 225 l2cap_channel_t * channel = (l2cap_channel_t *) it; 226 if ( ! BD_ADDR_CMP( channel->address, address) ){ 227 if (channel->state == L2CAP_STATE_CLOSED) { 228 // failure, forward error code 229 l2cap_emit_channel_opened(channel, status); 230 // discard channel 231 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 232 free (channel); 233 } 234 } 235 } 236 } 237 238 static void l2cap_handle_connection_success_for_addr(bd_addr_t address, hci_con_handle_t handle){ 239 linked_item_t *it; 240 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 241 l2cap_channel_t * channel = (l2cap_channel_t *) it; 242 if ( ! BD_ADDR_CMP( channel->address, address) ){ 243 if (channel->state == L2CAP_STATE_CLOSED) { 244 // success, start l2cap handshake 245 channel->handle = handle; 246 channel->sig_id = l2cap_next_sig_id(); 247 channel->local_cid = l2cap_next_local_cid(); 248 channel->state = L2CAP_STATE_WAIT_CONNECT_RSP; 249 l2cap_send_signaling_packet( channel->handle, CONNECTION_REQUEST, channel->sig_id, channel->psm, channel->local_cid); 250 } 251 } 252 } 253 } 254 255 void l2cap_event_handler( uint8_t *packet, uint16_t size ){ 256 257 bd_addr_t address; 258 hci_con_handle_t handle; 259 linked_item_t *it; 260 261 switch(packet[0]){ 262 263 // handle connection complete events 264 case HCI_EVENT_CONNECTION_COMPLETE: 265 bt_flip_addr(address, &packet[5]); 266 if (packet[2] == 0){ 267 handle = READ_BT_16(packet, 3); 268 l2cap_handle_connection_success_for_addr(address, handle); 269 } else { 270 l2cap_handle_connection_failed_for_addr(address, packet[2]); 271 } 272 break; 273 274 // handle successful create connection cancel command 275 case HCI_EVENT_COMMAND_COMPLETE: 276 if ( COMMAND_COMPLETE_EVENT(packet, hci_create_connection_cancel) ) { 277 if (packet[5] == 0){ 278 bt_flip_addr(address, &packet[6]); 279 // CONNECTION TERMINATED BY LOCAL HOST (0X16) 280 l2cap_handle_connection_failed_for_addr(address, 0x16); 281 } 282 } 283 break; 284 285 // handle disconnection complete events 286 case HCI_EVENT_DISCONNECTION_COMPLETE: 287 // send l2cap disconnect events for all channels on this handle 288 handle = READ_BT_16(packet, 3); 289 // only access next element to allows for removal 290 for (it = (linked_item_t *) &l2cap_channels; it->next ; it = it->next){ 291 l2cap_channel_t * channel = (l2cap_channel_t *) it->next; 292 if ( channel->handle == handle ){ 293 // update prev item before free'ing next element 294 it->next = it->next->next; 295 l2cap_finialize_channel_close(channel); 296 } 297 } 298 break; 299 300 // HCI Connection Timeouts 301 case L2CAP_EVENT_TIMEOUT_CHECK: 302 handle = READ_BT_16(packet, 2); 303 l2cap_channel_t * channel; 304 int used = 0; 305 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 306 channel = (l2cap_channel_t *) it; 307 if (channel->handle == handle) { 308 used = 1; 309 } 310 } 311 if (!used) { 312 hci_send_cmd(&hci_disconnect, handle, 0x13); // remote closed connection 313 } 314 break; 315 316 default: 317 break; 318 } 319 320 // pass on 321 (*packet_handler)(NULL, HCI_EVENT_PACKET, 0, packet, size); 322 } 323 324 static void l2cap_handle_disconnect_request(l2cap_channel_t *channel, uint16_t identifier){ 325 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_RESPONSE, identifier, channel->local_cid, channel->remote_cid); 326 l2cap_finialize_channel_close(channel); 327 } 328 329 static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ 330 331 // printf("l2cap_handle_connection_request for handle %u, psm %u cid %u\n", handle, psm, source_cid); 332 l2cap_service_t *service = l2cap_get_service(psm); 333 if (!service) { 334 // 0x0002 PSM not supported 335 // printf("l2cap_handle_connection_request no PSM for psm %u/n", psm); 336 l2cap_send_signaling_packet(handle, CONNECTION_RESPONSE, sig_id, 0, 0, 0x0002, 0); 337 return; 338 } 339 340 hci_connection_t * hci_connection = connection_for_handle( handle ); 341 if (!hci_connection) { 342 fprintf(stderr, "no hci_connection for handle %u\n", handle); 343 // TODO: emit error 344 return; 345 } 346 // alloc structure 347 // printf("l2cap_handle_connection_request register channel\n"); 348 l2cap_channel_t * channel = malloc(sizeof(l2cap_channel_t)); 349 // TODO: emit error event 350 if (!channel) return; 351 352 // fill in 353 BD_ADDR_COPY(channel->address, hci_connection->address); 354 channel->psm = psm; 355 channel->handle = handle; 356 channel->connection = service->connection; 357 channel->packet_handler = service->packet_handler; 358 channel->local_cid = l2cap_next_local_cid(); 359 channel->remote_cid = source_cid; 360 channel->remote_mtu = L2CAP_MINIMAL_MTU; 361 362 // set initial state 363 channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; 364 365 // temp. store req sig id 366 channel->sig_id = sig_id; 367 368 // add to connections list 369 linked_list_add(&l2cap_channels, (linked_item_t *) channel); 370 371 // emit incoming connection request 372 l2cap_emit_connection_request(channel); 373 } 374 375 void l2cap_accept_connection_internal(uint16_t local_cid){ 376 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(local_cid); 377 if (!channel) { 378 fprintf(stderr, "l2cap_accept_connection_internal called but local_cid 0x%x not found", local_cid); 379 return; 380 } 381 382 // accept connection 383 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->sig_id, channel->local_cid, channel->remote_cid, 0, 0); 384 385 // set real sig and state and start config 386 channel->sig_id = l2cap_next_sig_id(); 387 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ; 388 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->remote_cid, 0, 4, &config_options); 389 390 // printf("new state %u\n", channel->state); 391 } 392 393 void l2cap_decline_connection_internal(uint16_t local_cid, uint8_t reason){ 394 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid( local_cid); 395 if (!channel) { 396 fprintf(stderr, "l2cap_decline_connection_internal called but local_cid 0x%x not found", local_cid); 397 return; 398 } 399 l2cap_send_signaling_packet(channel->handle, CONNECTION_RESPONSE, channel->sig_id, 0, 0, reason, 0); 400 401 // discard channel 402 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 403 free (channel); 404 } 405 406 void l2cap_signaling_handle_configure_request(l2cap_channel_t *channel, uint8_t *command){ 407 // accept the other's configuration options 408 uint16_t end_pos = 4 + READ_BT_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 409 uint16_t pos = 8; 410 while (pos < end_pos){ 411 uint8_t type = command[pos++]; 412 uint8_t length = command[pos++]; 413 // MTU { type(8): 1, len(8):2, MTU(16) } 414 if ((type & 0x7f) == 1 && length == 2){ 415 channel->remote_mtu = READ_BT_16(command, pos); 416 // printf("l2cap cid %u, remote mtu %u\n", channel->local_cid, channel->remote_mtu); 417 } 418 pos += length; 419 } 420 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 421 // send back received options 422 // l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->remote_cid, 0, 0, len-4, &command[8]); 423 // send back OK 424 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->remote_cid, 0, 0, 0, NULL); 425 } 426 427 void l2cap_signaling_handler_channel(l2cap_channel_t *channel, uint8_t *command){ 428 429 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 430 uint8_t identifier = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 431 uint16_t result = 0; 432 433 // printf("signaling handler code %u\n", code); 434 435 switch (channel->state) { 436 437 case L2CAP_STATE_WAIT_CONNECT_RSP: 438 switch (code){ 439 case CONNECTION_RESPONSE: 440 result = READ_BT_16 (command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+4); 441 switch (result) { 442 case 0: 443 // successful connection 444 channel->remote_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 445 channel->sig_id = l2cap_next_sig_id(); 446 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->remote_cid, 0, 4, &config_options); 447 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ; 448 break; 449 case 1: 450 // connection pending. get some coffee 451 break; 452 default: 453 // map l2cap connection response result to BTstack status enumeration 454 l2cap_emit_channel_opened(channel, L2CAP_CONNECTION_RESPONSE_RESULT_SUCCESSFUL + result); 455 break; 456 } 457 break; 458 459 case DISCONNECTION_REQUEST: 460 l2cap_handle_disconnect_request(channel, identifier); 461 break; 462 463 default: 464 //@TODO: implement other signaling packets 465 break; 466 } 467 break; 468 469 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP_OR_CONFIG_REQ: 470 switch (code) { 471 case CONFIGURE_RESPONSE: 472 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; 473 break; 474 case CONFIGURE_REQUEST: 475 l2cap_signaling_handle_configure_request(channel, command); 476 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; 477 break; 478 case DISCONNECTION_REQUEST: 479 l2cap_handle_disconnect_request(channel, identifier); 480 break; 481 default: 482 //@TODO: implement other signaling packets 483 break; 484 } 485 break; 486 487 case L2CAP_STATE_WAIT_CONFIG_REQ: 488 switch (code) { 489 case CONFIGURE_REQUEST: 490 l2cap_signaling_handle_configure_request(channel, command); 491 channel->state = L2CAP_STATE_OPEN; 492 l2cap_emit_channel_opened(channel, 0); // success 493 break; 494 case DISCONNECTION_REQUEST: 495 l2cap_handle_disconnect_request(channel, identifier); 496 break; 497 default: 498 //@TODO: implement other signaling packets 499 break; 500 } 501 break; 502 503 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: 504 switch (code) { 505 case CONFIGURE_RESPONSE: 506 channel->state = L2CAP_STATE_OPEN; 507 l2cap_emit_channel_opened(channel, 0); // success 508 break; 509 case DISCONNECTION_REQUEST: 510 l2cap_handle_disconnect_request(channel, identifier); 511 break; 512 default: 513 //@TODO: implement other signaling packets 514 break; 515 } 516 break; 517 518 case L2CAP_STATE_WAIT_DISCONNECT: 519 switch (code) { 520 case DISCONNECTION_RESPONSE: 521 l2cap_finialize_channel_close(channel); 522 break; 523 case DISCONNECTION_REQUEST: 524 l2cap_handle_disconnect_request(channel, identifier); 525 break; 526 default: 527 //@TODO: implement other signaling packets 528 break; 529 } 530 break; 531 532 case L2CAP_STATE_CLOSED: 533 // @TODO handle incoming requests 534 break; 535 536 case L2CAP_STATE_OPEN: 537 switch (code) { 538 case DISCONNECTION_REQUEST: 539 l2cap_handle_disconnect_request(channel, identifier); 540 break; 541 default: 542 //@TODO: implement other signaling packets, e.g. re-configure 543 break; 544 } 545 break; 546 default: 547 break; 548 } 549 // printf("new state %u\n", channel->state); 550 } 551 552 553 void l2cap_signaling_handler_dispatch( hci_con_handle_t handle, uint8_t * command){ 554 555 // get code, signalind identifier and command len 556 uint8_t code = command[L2CAP_SIGNALING_COMMAND_CODE_OFFSET]; 557 uint8_t sig_id = command[L2CAP_SIGNALING_COMMAND_SIGID_OFFSET]; 558 uint16_t len = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 559 560 // not for a particular channel, and not CONNECTION_REQUEST, ECHO_[REQUEST|RESPONSE], INFORMATION_REQUEST 561 if (code < 1 || code == ECHO_RESPONSE || code > INFORMATION_REQUEST){ 562 return; 563 } 564 565 // general commands without an assigned channel 566 switch(code) { 567 568 case CONNECTION_REQUEST: { 569 uint16_t psm = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 570 uint16_t source_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET+2); 571 l2cap_handle_connection_request(handle, sig_id, psm, source_cid); 572 break; 573 } 574 575 case ECHO_REQUEST: { 576 // send back packet 577 l2cap_send_signaling_packet(handle, ECHO_RESPONSE, sig_id, len, &command[L2CAP_SIGNALING_COMMAND_DATA_OFFSET]); 578 break; 579 } 580 581 case INFORMATION_REQUEST: { 582 // we neither support connectionless L2CAP data nor support any flow control modes yet 583 uint16_t infoType = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 584 if (infoType == 2) { 585 uint32_t features = 0; 586 // extended features request supported, however no features present 587 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 0, 4, &features); 588 } else { 589 // all other types are not supported 590 l2cap_send_signaling_packet(handle, INFORMATION_RESPONSE, sig_id, infoType, 1, 0, NULL); 591 } 592 break;; 593 } 594 595 default: 596 break; 597 } 598 599 600 // Get potential destination CID 601 uint16_t dest_cid = READ_BT_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET); 602 603 // Find channel for this sig_id and connection handle 604 linked_item_t *it; 605 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 606 l2cap_channel_t * channel = (l2cap_channel_t *) it; 607 if (channel->handle == handle) { 608 if (code & 1) { 609 // match odd commands by previous signaling identifier 610 if (channel->sig_id == sig_id) { 611 l2cap_signaling_handler_channel(channel, command); 612 } 613 } else { 614 // match even commands by local channel id 615 if (channel->local_cid == dest_cid) { 616 l2cap_signaling_handler_channel(channel, command); 617 } 618 } 619 } 620 } 621 } 622 623 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 624 625 // Get Channel ID 626 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 627 628 // Signaling Packet? 629 if (channel_id == 1) { 630 631 // Get Connection 632 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 633 634 uint16_t command_offset = 8; 635 while (command_offset < size) { 636 637 // handle signaling commands 638 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]); 639 640 // increment command_offset 641 command_offset += L2CAP_SIGNALING_COMMAND_DATA_OFFSET + READ_BT_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET); 642 } 643 return; 644 } 645 646 // Find channel for this channel_id and connection handle 647 l2cap_channel_t * channel = l2cap_get_channel_for_local_cid(channel_id); 648 if (channel) { 649 l2cap_dispatch(channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER); 650 } 651 } 652 653 static void l2cap_packet_handler(uint8_t packet_type, uint8_t *packet, uint16_t size){ 654 switch (packet_type) { 655 case HCI_EVENT_PACKET: 656 l2cap_event_handler(packet, size); 657 break; 658 case HCI_ACL_DATA_PACKET: 659 l2cap_acl_handler(packet, size); 660 break; 661 default: 662 break; 663 } 664 } 665 666 // finalize closed channel 667 void l2cap_finialize_channel_close(l2cap_channel_t *channel){ 668 channel->state = L2CAP_STATE_CLOSED; 669 l2cap_emit_channel_closed(channel); 670 671 // discard channel 672 linked_list_remove(&l2cap_channels, (linked_item_t *) channel); 673 free (channel); 674 } 675 676 l2cap_service_t * l2cap_get_service(uint16_t psm){ 677 linked_item_t *it; 678 679 // close open channels 680 for (it = (linked_item_t *) l2cap_services; it ; it = it->next){ 681 l2cap_service_t * service = ((l2cap_service_t *) it); 682 if ( service->psm == psm){ 683 return service; 684 }; 685 } 686 return NULL; 687 } 688 689 void l2cap_register_service_internal(void *connection, btstack_packet_handler_t packet_handler, uint16_t psm, uint16_t mtu){ 690 // check for alread registered psm // TODO: emit error event 691 l2cap_service_t *service = l2cap_get_service(psm); 692 if (service) return; 693 694 // alloc structure // TODO: emit error event 695 service = malloc(sizeof(l2cap_service_t)); 696 if (!service) return; 697 698 // fill in 699 service->psm = psm; 700 service->mtu = mtu; 701 service->connection = connection; 702 service->packet_handler = packet_handler; 703 704 // add to services list 705 linked_list_add(&l2cap_services, (linked_item_t *) service); 706 } 707 708 void l2cap_unregister_service_internal(void *connection, uint16_t psm){ 709 l2cap_service_t *service = l2cap_get_service(psm); 710 if (!service) return; 711 linked_list_remove(&l2cap_services, (linked_item_t *) service); 712 free(service); 713 } 714 715 // 716 void l2cap_close_connection(void *connection){ 717 linked_item_t *it; 718 719 // close open channels 720 l2cap_channel_t * channel; 721 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 722 channel = (l2cap_channel_t *) it; 723 if (channel->connection == connection) { 724 channel->sig_id = l2cap_next_sig_id(); 725 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->remote_cid, channel->local_cid); 726 channel->state = L2CAP_STATE_WAIT_DISCONNECT; 727 } 728 } 729 730 // unregister services 731 it = (linked_item_t *) &l2cap_services; 732 while (it->next){ 733 l2cap_service_t * service = (l2cap_service_t *) it->next; 734 if (service->connection == connection){ 735 it->next = it->next->next; 736 free(service); 737 } else { 738 it = it->next; 739 } 740 } 741 } 742