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