1/* 2 * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com 3 * All rights reserved. Distributed under the terms of the MIT License. 4 */ 5/*- 6 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com> 7 * All rights reserved. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 1. Redistributions of source code must retain the above copyright 13 * notice, this list of conditions and the following disclaimer. 14 * 2. Redistributions in binary form must reproduce the above copyright 15 * notice, this list of conditions and the following disclaimer in the 16 * documentation and/or other materials provided with the distribution. 17*/ 18#include <KernelExport.h> 19#include <string.h> 20 21#include <NetBufferUtilities.h> 22 23 24#include <l2cap.h> 25#include "l2cap_internal.h" 26#include "l2cap_signal.h" 27#include "l2cap_command.h" 28#include "l2cap_upper.h" 29#include "l2cap_lower.h" 30 31#define BT_DEBUG_THIS_MODULE 32#define SUBMODULE_NAME "signal" 33#define SUBMODULE_COLOR 36 34#include <btDebug.h> 35 36#include <bluetooth/HCI/btHCI_command.h> 37 38typedef enum _option_status { 39 OPTION_NOT_PRESENT = 0, 40 OPTION_PRESENT = 1, 41 HEADER_TOO_SHORT = -1, 42 BAD_OPTION_LENGTH = -2, 43 OPTION_UNKNOWN = -3 44} option_status; 45 46 47l2cap_flow_t default_qos = { 48 /* flags */ 0x0, 49 /* service_type */ HCI_SERVICE_TYPE_BEST_EFFORT, 50 /* token_rate */ 0xffffffff, /* maximum */ 51 /* token_bucket_size */ 0xffffffff, /* maximum */ 52 /* peak_bandwidth */ 0x00000000, /* maximum */ 53 /* latency */ 0xffffffff, /* don't care */ 54 /* delay_variation */ 0xffffffff /* don't care */ 55}; 56 57 58static status_t 59l2cap_process_con_req(HciConnection* conn, uint8 ident, net_buffer* buffer); 60 61static status_t 62l2cap_process_con_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer); 63 64static status_t 65l2cap_process_cfg_req(HciConnection* conn, uint8 ident, net_buffer* buffer); 66 67static status_t 68l2cap_process_cfg_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer); 69 70static status_t 71l2cap_process_discon_req(HciConnection* conn, uint8 ident, net_buffer* buffer); 72 73static status_t 74l2cap_process_discon_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer); 75 76static status_t 77l2cap_process_echo_req(HciConnection* conn, uint8 ident, net_buffer* buffer); 78 79static status_t 80l2cap_process_echo_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer); 81 82static status_t 83l2cap_process_info_req(HciConnection* conn, uint8 ident, net_buffer* buffer); 84 85static status_t 86l2cap_process_info_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer); 87 88static status_t 89l2cap_process_cmd_rej(HciConnection* conn, uint8 ident, net_buffer* buffer); 90 91 92/* 93 * Process L2CAP signaling command. We already know that destination channel ID 94 * is 0x1 that means we have received signaling command from peer's L2CAP layer. 95 * So get command header, decode and process it. 96 * 97 * XXX do we need to check signaling MTU here? 98 */ 99 100status_t 101l2cap_process_signal_cmd(HciConnection* conn, net_buffer* buffer) 102{ 103 net_buffer* m = buffer; 104 105 debugf("Signal size=%ld\n", buffer->size); 106 107 while (m != NULL) { 108 109 /* Verify packet length */ 110 if (buffer->size < sizeof(l2cap_cmd_hdr_t)) { 111 debugf("small L2CAP signaling command len=%ld\n", buffer->size); 112 gBufferModule->free(buffer); 113 return EMSGSIZE; 114 } 115 116 /* Get COMMAND header */ 117 NetBufferHeaderReader<l2cap_cmd_hdr_t> commandHeader(buffer); 118 status_t status = commandHeader.Status(); 119 if (status < B_OK) { 120 return ENOBUFS; 121 } 122 123 uint8 processingCode = commandHeader->code; 124 uint8 processingIdent = commandHeader->ident; 125 uint16 processingLength = le16toh(commandHeader->length); 126 127 /* Verify command length */ 128 if (buffer->size < processingLength) { 129 debugf("invalid L2CAP signaling command, code=%#x, ident=%d," 130 " length=%d, buffer size=%ld\n", processingCode, 131 processingIdent, processingLength, buffer->size); 132 gBufferModule->free(buffer); 133 return (EMSGSIZE); 134 } 135 136 137 commandHeader.Remove(); // pulling the header of the command 138 139 /* Process command processors responsible to delete the command*/ 140 switch (processingCode) { 141 case L2CAP_CMD_REJ: 142 l2cap_process_cmd_rej(conn, processingIdent, buffer); 143 break; 144 145 case L2CAP_CON_REQ: 146 l2cap_process_con_req(conn, processingIdent, buffer); 147 break; 148 149 case L2CAP_CON_RSP: 150 l2cap_process_con_rsp(conn, processingIdent, buffer); 151 break; 152 153 case L2CAP_CFG_REQ: 154 l2cap_process_cfg_req(conn, processingIdent, buffer); 155 break; 156 157 case L2CAP_CFG_RSP: 158 l2cap_process_cfg_rsp(conn, processingIdent, buffer); 159 break; 160 161 case L2CAP_DISCON_REQ: 162 l2cap_process_discon_req(conn, processingIdent, buffer); 163 break; 164 165 case L2CAP_DISCON_RSP: 166 l2cap_process_discon_rsp(conn, processingIdent, buffer); 167 break; 168 169 case L2CAP_ECHO_REQ: 170 l2cap_process_echo_req(conn, processingIdent, buffer); 171 break; 172 173 case L2CAP_ECHO_RSP: 174 l2cap_process_echo_rsp(conn, processingIdent, buffer); 175 break; 176 177 case L2CAP_INFO_REQ: 178 l2cap_process_info_req(conn, processingIdent, buffer); 179 break; 180 181 case L2CAP_INFO_RSP: 182 l2cap_process_info_rsp(conn, processingIdent, buffer); 183 break; 184 185 default: 186 debugf("unknown L2CAP signaling command, code=%#x, ident=%d\n", 187 processingCode, processingIdent); 188 189 // Send L2CAP_CommandRej. Do not really care about the result 190 // ORD: Remiaining commands in the same packet are also to 191 // be rejected? 192 // send_l2cap_reject(con, processingIdent, 193 // L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); 194 gBufferModule->free(m); 195 break; 196 } 197 198 // Is there still remaining size? processors should have pulled its content... 199 if (m->size == 0) { 200 // free the buffer 201 gBufferModule->free(m); 202 m = NULL; 203 } 204 } 205 206 return B_OK; 207} 208 209 210#if 0 211#pragma mark - Processing Incoming signals 212#endif 213 214 215/* Process L2CAP_ConnectReq command */ 216static status_t 217l2cap_process_con_req(HciConnection* conn, uint8 ident, net_buffer* buffer) 218{ 219 L2capChannel* channel; 220 uint16 dcid, psm; 221 222 /* Get con req data */ 223 NetBufferHeaderReader<l2cap_con_req_cp> command(buffer); 224 status_t status = command.Status(); 225 if (status < B_OK) { 226 return ENOBUFS; 227 } 228 229 psm = le16toh(command->psm); 230 dcid = le16toh(command->scid); 231 232 command.Remove(); // pull the command body 233 234 // Create new channel and send L2CA_ConnectInd 235 // notification to the upper layer protocol. 236 channel = btCoreData->AddChannel(conn, psm /*, dcid, ident*/); 237 if (channel == NULL) { 238 flowf("No resources to create channel\n"); 239 return (send_l2cap_con_rej(conn, ident, 0, dcid, L2CAP_NO_RESOURCES)); 240 } else { 241 debugf("New channel created scid=%d\n", channel->scid); 242 } 243 244 channel->dcid = dcid; 245 channel->ident = ident; 246 247 status_t indicationStatus = l2cap_l2ca_con_ind(channel); 248 249 if ( indicationStatus == B_OK ) { 250 // channel->state = L2CAP_CHAN_W4_L2CA_CON_RSP; 251 252 } else if (indicationStatus == B_NO_MEMORY) { 253 /* send_l2cap_con_rej(con, ident, channel->scid, dcid, L2CAP_NO_RESOURCES);*/ 254 btCoreData->RemoveChannel(conn, channel->scid); 255 256 } else { 257 send_l2cap_con_rej(conn, ident, channel->scid, dcid, L2CAP_PSM_NOT_SUPPORTED); 258 btCoreData->RemoveChannel(conn, channel->scid); 259 } 260 261 return (indicationStatus); 262} 263 264 265static status_t 266l2cap_process_con_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer) 267{ 268 L2capFrame* cmd = NULL; 269 uint16 scid, dcid, result, status; 270 status_t error = 0; 271 272 /* Get command parameters */ 273 NetBufferHeaderReader<l2cap_con_rsp_cp> command(buffer); 274 if (command.Status() < B_OK) { 275 return ENOBUFS; 276 } 277 278 dcid = le16toh(command->dcid); 279 scid = le16toh(command->scid); 280 result = le16toh(command->result); 281 status = le16toh(command->status); 282 283 command.Remove(); // pull the command body 284 285 debugf("dcid=%d scid=%d result=%d status%d\n", dcid, scid, result, status); 286 287 /* Check if we have pending command descriptor */ 288 cmd = btCoreData->SignalByIdent(conn, ident); 289 if (cmd == NULL) { 290 debugf("unexpected L2CAP_ConnectRsp command. ident=%d, " 291 "con_handle=%d\n", ident, conn->handle); 292 return ENOENT; 293 } 294 295 /* Verify channel state, if invalid - do nothing */ 296 if (cmd->channel->state != L2CAP_CHAN_W4_L2CAP_CON_RSP) { 297 debugf("unexpected L2CAP_ConnectRsp. Invalid channel state, " 298 "cid=%d, state=%d\n", scid, cmd->channel->state); 299 goto reject; 300 } 301 302 /* Verify CIDs and send reject if does not match */ 303 if (cmd->channel->scid != scid) { 304 debugf("unexpected L2CAP_ConnectRsp. Channel IDs do not match, " 305 "scid=%d(%d)\n", cmd->channel->scid, scid); 306 goto reject; 307 } 308 309 /* 310 * Looks good. We got confirmation from our peer. Now process 311 * it. First disable RTX timer. Then check the result and send 312 * notification to the upper layer. If command timeout already 313 * happened then ignore response. 314 */ 315 316 if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0) 317 return error; 318 319 if (result == L2CAP_PENDING) { 320 /* 321 * Our peer wants more time to complete connection. We shall 322 * start ERTX timer and wait. Keep command in the list. 323 */ 324 325 cmd->channel->dcid = dcid; 326 btCoreData->TimeoutSignal(cmd, bluetooth_l2cap_ertx_timeout); 327 328 // TODO: 329 // INDICATION error = ng_l2cap_l2ca_con_rsp(cmd->channel, cmd->token, 330 // result, status); 331 // if (error != B_OK) 332 // btCoreData->RemoveChannel(conn, cmd->channel->scid); 333 334 } else { 335 336 if (result == L2CAP_SUCCESS) { 337 /* 338 * Channel is open. Complete command and move to CONFIG 339 * state. Since we have sent positive confirmation we 340 * expect to receive L2CA_Config request from the upper 341 * layer protocol. 342 */ 343 344 cmd->channel->dcid = dcid; 345 cmd->channel->state = L2CAP_CHAN_CONFIG; 346 } 347 348 error = l2cap_con_rsp_ind(conn, cmd->channel); 349 350 /* XXX do we have to remove the channel on error? */ 351 if (error != 0 || result != L2CAP_SUCCESS) { 352 debugf("failed to open L2CAP channel, result=%d, status=%d\n", 353 result, status); 354 btCoreData->RemoveChannel(conn, cmd->channel->scid); 355 } 356 357 btCoreData->AcknowledgeSignal(cmd); 358 } 359 360 return error; 361 362reject: 363 /* Send reject. Do not really care about the result */ 364 send_l2cap_reject(conn, ident, L2CAP_REJ_INVALID_CID, 0, scid, dcid); 365 366 return 0; 367} 368 369 370static option_status 371getNextSignalOption(net_buffer* nbuf, size_t* off, l2cap_cfg_opt_t* hdr, 372 l2cap_cfg_opt_val_t* val) 373{ 374 int hint; 375 size_t len = nbuf->size - (*off); 376 377 if (len == 0) 378 return OPTION_NOT_PRESENT; 379 if (len < 0 || len < sizeof(*hdr)) 380 return HEADER_TOO_SHORT; 381 382 gBufferModule->read(nbuf, *off, hdr, sizeof(*hdr)); 383 *off += sizeof(*hdr); 384 len -= sizeof(*hdr); 385 386 hint = L2CAP_OPT_HINT(hdr->type); 387 hdr->type &= L2CAP_OPT_HINT_MASK; 388 389 switch (hdr->type) { 390 case L2CAP_OPT_MTU: 391 if (hdr->length != L2CAP_OPT_MTU_SIZE || len < hdr->length) 392 return BAD_OPTION_LENGTH; 393 394 gBufferModule->read(nbuf, *off, val, L2CAP_OPT_MTU_SIZE); 395 val->mtu = le16toh(val->mtu); 396 *off += L2CAP_OPT_MTU_SIZE; 397 debugf("mtu %d specified\n", val->mtu); 398 break; 399 400 case L2CAP_OPT_FLUSH_TIMO: 401 if (hdr->length != L2CAP_OPT_FLUSH_TIMO_SIZE || len < hdr->length) 402 return BAD_OPTION_LENGTH; 403 404 gBufferModule->read(nbuf, *off, val, L2CAP_OPT_FLUSH_TIMO_SIZE); 405 val->flush_timo = le16toh(val->flush_timo); 406 flowf("flush specified\n"); 407 *off += L2CAP_OPT_FLUSH_TIMO_SIZE; 408 break; 409 410 case L2CAP_OPT_QOS: 411 if (hdr->length != L2CAP_OPT_QOS_SIZE || len < hdr->length) 412 return BAD_OPTION_LENGTH; 413 414 gBufferModule->read(nbuf, *off, val, L2CAP_OPT_QOS_SIZE); 415 val->flow.token_rate = le32toh(val->flow.token_rate); 416 val->flow.token_bucket_size = le32toh(val->flow.token_bucket_size); 417 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); 418 val->flow.latency = le32toh(val->flow.latency); 419 val->flow.delay_variation = le32toh(val->flow.delay_variation); 420 *off += L2CAP_OPT_QOS_SIZE; 421 flowf("qos specified\n"); 422 break; 423 424 default: 425 if (hint) 426 *off += hdr->length; 427 else 428 return OPTION_UNKNOWN; 429 break; 430 } 431 432 return OPTION_PRESENT; 433} 434 435 436static status_t 437l2cap_process_cfg_req(HciConnection* conn, uint8 ident, net_buffer* buffer) 438{ 439 L2capChannel* channel = NULL; 440 uint16 dcid; 441 uint16 respond; 442 uint16 result; 443 444 l2cap_cfg_opt_t hdr; 445 l2cap_cfg_opt_val_t val; 446 447 size_t off; 448 status_t error = 0; 449 450 debugf("configuration=%ld\n", buffer->size); 451 452 /* Get command parameters */ 453 NetBufferHeaderReader<l2cap_cfg_req_cp> command(buffer); 454 status_t status = command.Status(); 455 if (status < B_OK) { 456 return ENOBUFS; 457 } 458 459 dcid = le16toh(command->dcid); 460 respond = L2CAP_OPT_CFLAG(le16toh(command->flags)); 461 462 command.Remove(); // pull configuration header 463 464 /* Check if we have this channel and it is in valid state */ 465 channel = btCoreData->ChannelBySourceID(conn, dcid); 466 if (channel == NULL) { 467 debugf("unexpected L2CAP_ConfigReq command. " 468 "Channel does not exist, cid=%d\n", dcid); 469 goto reject; 470 } 471 472 /* Verify channel state */ 473 if (channel->state != L2CAP_CHAN_CONFIG 474 && channel->state != L2CAP_CHAN_OPEN) { 475 debugf("unexpected L2CAP_ConfigReq. Invalid channel state, " 476 "cid=%d, state=%d\n", dcid, channel->state); 477 goto reject; 478 } 479 480 if (channel->state == L2CAP_CHAN_OPEN) { /* Re-configuration */ 481 channel->cfgState = 0; // Reset configuration state 482 channel->state = L2CAP_CHAN_CONFIG; 483 } 484 485 for (result = 0, off = 0; ; ) { 486 error = getNextSignalOption(buffer, &off, &hdr, &val); 487 488 if (error == OPTION_NOT_PRESENT) { /* We done with this packet */ 489 // TODO: configurations should have been pulled 490 break; 491 } else if (error > 0) { /* Got option */ 492 switch (hdr.type) { 493 case L2CAP_OPT_MTU: 494 channel->configuration->omtu = val.mtu; 495 break; 496 497 case L2CAP_OPT_FLUSH_TIMO: 498 channel->configuration->flush_timo = val.flush_timo; 499 break; 500 501 case L2CAP_OPT_QOS: 502 memcpy(&val.flow, &channel->configuration->iflow, 503 sizeof(channel->configuration->iflow)); 504 break; 505 506 default: /* Ignore unknown hint option */ 507 break; 508 } 509 } else { /* Oops, something is wrong */ 510 respond = 1; 511 if (error == OPTION_UNKNOWN) { 512 // TODO: Remote to get the next possible option 513 result = L2CAP_UNKNOWN_OPTION; 514 } else { 515 /* XXX FIXME Send other reject codes? */ 516 gBufferModule->free(buffer); 517 result = L2CAP_REJECT; 518 } 519 520 break; 521 } 522 } 523 524 debugf("Pulled %ld of configuration fields respond=%d remaining=%ld\n", 525 off, respond, buffer->size); 526 gBufferModule->remove_header(buffer, off); 527 528 529 /* 530 * Now check and see if we have to respond. If everything was OK then 531 * respond contain "C flag" and (if set) we will respond with empty 532 * packet and will wait for more options. 533 * 534 * Other case is that we did not like peer's options and will respond 535 * with L2CAP_Config response command with Reject error code. 536 * 537 * When "respond == 0" than we have received all options and we will 538 * sent L2CA_ConfigInd event to the upper layer protocol. 539 */ 540 541 if (respond) { 542 flowf("Refusing cfg\n"); 543 error = send_l2cap_cfg_rsp(conn, ident, channel->dcid, result, buffer); 544 if (error != 0) { 545 // TODO: 546 // INDICATION ng_l2cap_l2ca_discon_ind(ch); 547 btCoreData->RemoveChannel(conn, channel->scid); 548 } 549 } else { 550 /* Send L2CA_ConfigInd event to the upper layer protocol */ 551 channel->cfgState |= L2CAP_CFG_IN; 552 channel->ident = ident; // sent ident to reply 553 error = l2cap_cfg_req_ind(channel); 554 if (error != 0) 555 btCoreData->RemoveChannel(conn, channel->scid); 556 } 557 558 return error; 559 560reject: 561 /* Send reject. Do not really care about the result */ 562 gBufferModule->free(buffer); 563 564 send_l2cap_reject(conn, ident, L2CAP_REJ_INVALID_CID, 0, 0, dcid); 565 566 return B_OK; 567} 568 569 570/* Process L2CAP_ConfigRsp command */ 571static status_t 572l2cap_process_cfg_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer) 573{ 574 L2capFrame* cmd = NULL; 575 uint16 scid, cflag, result; 576 577 l2cap_cfg_opt_t hdr; 578 l2cap_cfg_opt_val_t val; 579 580 size_t off; 581 status_t error = 0; 582 583 NetBufferHeaderReader<l2cap_cfg_rsp_cp> command(buffer); 584 status_t status = command.Status(); 585 if (status < B_OK) { 586 return ENOBUFS; 587 } 588 589 scid = le16toh(command->scid); 590 cflag = L2CAP_OPT_CFLAG(le16toh(command->flags)); 591 result = le16toh(command->result); 592 593 command.Remove(); 594 595 debugf("scid=%d cflag=%d result=%d\n", scid, cflag, result); 596 597 /* Check if we have this command */ 598 cmd = btCoreData->SignalByIdent(conn, ident); 599 if (cmd == NULL) { 600 debugf("unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", 601 ident, conn->handle); 602 gBufferModule->free(buffer); 603 return ENOENT; 604 } 605 606 /* Verify CIDs and send reject if does not match */ 607 if (cmd->channel->scid != scid) { 608 debugf("unexpected L2CAP_ConfigRsp.Channel ID does not match, " 609 "scid=%d(%d)\n", cmd->channel->scid, scid); 610 goto reject; 611 } 612 613 /* Verify channel state and reject if invalid */ 614 if (cmd->channel->state != L2CAP_CHAN_CONFIG) { 615 debugf("unexpected L2CAP_ConfigRsp. Invalid channel state, scid=%d, " 616 "state=%d\n", cmd->channel->scid, cmd->channel->state); 617 goto reject; 618 } 619 620 /* 621 * Looks like it is our response, so process it. First parse options, 622 * then verify C flag. If it is set then we shall expect more 623 * configuration options from the peer and we will wait. Otherwise we 624 * have received all options and we will send L2CA_ConfigRsp event to 625 * the upper layer protocol. If command timeout already happened then 626 * ignore response. 627 */ 628 629 if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0) { 630 gBufferModule->free(buffer); 631 return error; 632 } 633 634 for (off = 0; ; ) { 635 error = getNextSignalOption(buffer, &off, &hdr, &val); 636 // TODO: pull the option 637 638 if (error == OPTION_NOT_PRESENT) /* We done with this packet */ 639 break; 640 else if (error > 0) { /* Got option */ 641 switch (hdr.type) { 642 case L2CAP_OPT_MTU: 643 cmd->channel->configuration->imtu = val.mtu; 644 break; 645 646 case L2CAP_OPT_FLUSH_TIMO: 647 cmd->channel->configuration->flush_timo = val.flush_timo; 648 break; 649 650 case L2CAP_OPT_QOS: 651 memcpy(&val.flow, &cmd->channel->configuration->oflow, 652 sizeof(cmd->channel->configuration->oflow)); 653 break; 654 655 default: /* Ignore unknown hint option */ 656 break; 657 } 658 } else { 659 /* 660 * XXX FIXME What to do here? 661 * 662 * This is really BAD :( options packet was broken, or 663 * peer sent us option that we did not understand. Let 664 * upper layer know and do not wait for more options. 665 */ 666 667 debugf("fail parsing configuration options, error=%ld\n", error); 668 // INDICATION 669 result = L2CAP_UNKNOWN; 670 cflag = 0; 671 672 break; 673 } 674 } 675 676 if (cflag) /* Restart timer and wait for more options */ 677 btCoreData->TimeoutSignal(cmd, bluetooth_l2cap_rtx_timeout); 678 else { 679 /* Send L2CA_Config response to the upper layer protocol */ 680 error = l2cap_cfg_rsp_ind(cmd->channel /*, cmd->token, result*/); 681 if (error != 0) { 682 /* 683 * XXX FIXME what to do here? we were not able to send 684 * response to the upper layer protocol, so for now 685 * just close the channel. Send L2CAP_Disconnect to 686 * remote peer? 687 */ 688 689 debugf("failed to send L2CA_Config response, error=%ld\n", error); 690 691 btCoreData->RemoveChannel(conn, cmd->channel->scid); 692 } 693 694 btCoreData->AcknowledgeSignal(cmd); 695 } 696 697 return error; 698 699reject: 700 /* Send reject. Do not really care about the result */ 701 gBufferModule->free(buffer); 702 send_l2cap_reject(conn, ident, L2CAP_REJ_INVALID_CID, 0, scid, 0); 703 704 return B_OK; 705} 706 707 708/* Process L2CAP_DisconnectReq command */ 709static status_t 710l2cap_process_discon_req(HciConnection* conn, uint8 ident, net_buffer* buffer) 711{ 712 L2capChannel* channel = NULL; 713 L2capFrame* cmd = NULL; 714 net_buffer* buff = NULL; 715 uint16 scid; 716 uint16 dcid; 717 718 NetBufferHeaderReader<l2cap_discon_req_cp> command(buffer); 719 status_t status = command.Status(); 720 if (status < B_OK) { 721 return ENOBUFS; 722 } 723 724 dcid = le16toh(command->dcid); 725 scid = le16toh(command->scid); 726 727 command.Remove(); 728 729 /* Check if we have this channel and it is in valid state */ 730 channel = btCoreData->ChannelBySourceID(conn, dcid); 731 if (channel == NULL) { 732 debugf("unexpected L2CAP_DisconnectReq message.Channel does not exist, " 733 "cid=%x\n", dcid); 734 goto reject; 735 } 736 737 /* XXX Verify channel state and reject if invalid -- is that true? */ 738 if (channel->state != L2CAP_CHAN_OPEN 739 && channel->state != L2CAP_CHAN_CONFIG 740 && channel->state != L2CAP_CHAN_W4_L2CAP_DISCON_RSP) { 741 debugf("unexpected L2CAP_DisconnectReq. Invalid channel state, cid=%d, " 742 "state=%d\n", dcid, channel->state); 743 goto reject; 744 } 745 746 /* Match destination channel ID */ 747 if (channel->dcid != scid || channel->scid != dcid) { 748 debugf("unexpected L2CAP_DisconnectReq. Channel IDs does not match, " 749 "channel: scid=%d, dcid=%d, request: scid=%d, dcid=%d\n", 750 channel->scid, channel->dcid, scid, dcid); 751 goto reject; 752 } 753 754 /* 755 * Looks good, so notify upper layer protocol that channel is about 756 * to be disconnected and send L2CA_DisconnectInd message. Then respond 757 * with L2CAP_DisconnectRsp. 758 */ 759 760 flowf("Responding\n"); 761 762 // inform upper if we were not actually already waiting 763 if (channel->state != L2CAP_CHAN_W4_L2CAP_DISCON_RSP) { 764 l2cap_discon_req_ind(channel); // do not care about result 765 } 766 767 /* Send L2CAP_DisconnectRsp */ 768 buff = l2cap_discon_rsp(ident, dcid, scid); 769 cmd = btCoreData->SpawnSignal(conn, channel, buff, ident, L2CAP_DISCON_RSP); 770 if (cmd == NULL) 771 return ENOMEM; 772 773 /* Link command to the queue */ 774 SchedConnectionPurgeThread(conn); 775 776 return B_OK; 777 778reject: 779 flowf("Rejecting\n"); 780 /* Send reject. Do not really care about the result */ 781 send_l2cap_reject(conn, ident, L2CAP_REJ_INVALID_CID, 0, scid, dcid); 782 783 return B_OK; 784} 785 786 787/* Process L2CAP_DisconnectRsp command */ 788static status_t 789l2cap_process_discon_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer) 790{ 791 L2capFrame* cmd = NULL; 792 int16 scid, dcid; 793 status_t error = 0; 794 795 /* Get command parameters */ 796 NetBufferHeaderReader<l2cap_discon_rsp_cp> command(buffer); 797 status_t status = command.Status(); 798 if (status < B_OK) { 799 return ENOBUFS; 800 } 801 802 dcid = le16toh(command->dcid); 803 scid = le16toh(command->scid); 804 805 command.Remove(); 806 //? NG_FREE_M(con->rx_pkt); 807 808 /* Check if we have pending command descriptor */ 809 cmd = btCoreData->SignalByIdent(conn, ident); 810 if (cmd == NULL) { 811 debugf("unexpected L2CAP_DisconnectRsp command. ident=%d, " 812 "con_handle=%d\n", ident, conn->handle); 813 goto out; 814 } 815 816 /* Verify channel state, do nothing if invalid */ 817 if (cmd->channel->state != L2CAP_CHAN_W4_L2CA_DISCON_RSP) { 818 debugf("unexpected L2CAP_DisconnectRsp. Invalid state, cid=%d, " 819 "state=%d\n", scid, cmd->channel->state); 820 goto out; 821 } 822 823 /* Verify CIDs and send reject if does not match */ 824 if (cmd->channel->scid != scid || cmd->channel->dcid != dcid) { 825 debugf("unexpected L2CAP_DisconnectRsp. Channel IDs do not match, " 826 "scid=%d(%d), dcid=%d(%d)\n", cmd->channel->scid, scid, 827 cmd->channel->dcid, dcid); 828 goto out; 829 } 830 831 /* 832 * Looks like we have successfuly disconnected channel, so notify 833 * upper layer. If command timeout already happened then ignore 834 * response. 835 */ 836 837 if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0) 838 goto out; 839 840 l2cap_discon_rsp_ind(cmd->channel/* results? */); 841 btCoreData->RemoveChannel(conn, scid); /* this will free commands too */ 842 843out: 844 return error; 845} 846 847 848static status_t 849l2cap_process_echo_req(HciConnection *conn, uint8 ident, net_buffer *buffer) 850{ 851 L2capFrame* cmd = NULL; 852 853 cmd = btCoreData->SpawnSignal(conn, NULL, l2cap_echo_req(ident, NULL, 0), 854 ident, L2CAP_ECHO_RSP); 855 if (cmd == NULL) { 856 gBufferModule->free(buffer); 857 return ENOMEM; 858 } 859 860 /* Attach data and link command to the queue */ 861 SchedConnectionPurgeThread(conn); 862 863 return B_OK; 864} 865 866 867/* Process L2CAP_EchoRsp command */ 868static status_t 869l2cap_process_echo_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer) 870{ 871 L2capFrame* cmd = NULL; 872 status_t error = 0; 873 874 /* Check if we have this command */ 875 cmd = btCoreData->SignalByIdent(conn, ident); 876 if (cmd != NULL) { 877 /* If command timeout already happened then ignore response */ 878 if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0) { 879 return error; 880 } 881 882 // INDICATION error = ng_l2cap_l2ca_ping_rsp(cmd->conn, cmd->token, 883 // L2CAP_SUCCESS, conn->rx_pkt); 884 btCoreData->AcknowledgeSignal(cmd); 885 886 } else { 887 debugf("unexpected L2CAP_EchoRsp command. ident does not exist, " 888 "ident=%d\n", ident); 889 gBufferModule->free(buffer); 890 error = B_ERROR; 891 } 892 893 return error; 894} 895 896 897/* Process L2CAP_InfoReq command */ 898static status_t 899l2cap_process_info_req(HciConnection* conn, uint8 ident, net_buffer* buffer) 900{ 901 L2capFrame* cmd = NULL; 902 net_buffer* buf = NULL; 903 uint16 type; 904 905 /* Get command parameters */ 906 NetBufferHeaderReader<l2cap_info_req_cp> command(buffer); 907 status_t status = command.Status(); 908 if (status < B_OK) { 909 return ENOBUFS; 910 } 911 912 // ?? 913 // command->type = le16toh(mtod(conn->rx_pkt, 914 // ng_l2cap_info_req_cp *)->type); 915 type = le16toh(command->type); 916 917 command.Remove(); 918 919 switch (type) { 920 case L2CAP_CONNLESS_MTU: 921 buf = l2cap_info_rsp(ident, L2CAP_CONNLESS_MTU, L2CAP_SUCCESS, 922 L2CAP_MTU_DEFAULT); 923 break; 924 925 default: 926 buf = l2cap_info_rsp(ident, type, L2CAP_NOT_SUPPORTED, 0); 927 break; 928 } 929 930 cmd = btCoreData->SpawnSignal(conn, NULL, buf, ident, L2CAP_INFO_RSP); 931 if (cmd == NULL) 932 return ENOMEM; 933 934 /* Link command to the queue */ 935 SchedConnectionPurgeThread(conn); 936 937 return B_OK; 938} 939 940 941/* Process L2CAP_InfoRsp command */ 942static status_t 943l2cap_process_info_rsp(HciConnection* conn, uint8 ident, net_buffer* buffer) 944{ 945 l2cap_info_rsp_cp* cp = NULL; 946 L2capFrame* cmd = NULL; 947 status_t error = B_OK; 948 949 /* Get command parameters */ 950 NetBufferHeaderReader<l2cap_info_rsp_cp> command(buffer); 951 status_t status = command.Status(); 952 if (status < B_OK) { 953 return ENOBUFS; 954 } 955 956 command->type = le16toh(command->type); 957 command->result = le16toh(command->result); 958 959 command.Remove(); 960 961 /* Check if we have pending command descriptor */ 962 cmd = btCoreData->SignalByIdent(conn, ident); 963 if (cmd == NULL) { 964 debugf("unexpected L2CAP_InfoRsp command. Requested ident does not " 965 "exist, ident=%d\n", ident); 966 gBufferModule->free(buffer); 967 return ENOENT; 968 } 969 970 /* If command timeout already happened then ignore response */ 971 if ((error = btCoreData->UnTimeoutSignal(cmd)) != 0) { 972 gBufferModule->free(buffer); 973 return error; 974 } 975 976 if (command->result == L2CAP_SUCCESS) { 977 switch (command->type) { 978 979 case L2CAP_CONNLESS_MTU: 980 /* TODO: Check specs ?? 981 if (conn->rx_pkt->m_pkthdr.len == sizeof(uint16)) { 982 *mtod(conn->rx_pkt, uint16 *) = le16toh(*mtod(conn->rx_pkt,uint16 *)); 983 } else { 984 cp->result = L2CAP_UNKNOWN; XXX 985 debugf("invalid L2CAP_InfoRsp command. Bad connectionless MTU parameter, len=%d\n", conn->rx_pkt->m_pkthdr.len); 986 }*/ 987 break; 988 989 default: 990 debugf("invalid L2CAP_InfoRsp command. Unknown info type=%d\n", cp->type); 991 break; 992 } 993 } 994 995 //INDICATION error = ng_l2cap_l2ca_get_info_rsp(cmd->conn, cmd->token, cp->result, conn->rx_pkt); 996 btCoreData->AcknowledgeSignal(cmd); 997 998 return error; 999} 1000 1001 1002static status_t 1003l2cap_process_cmd_rej(HciConnection* conn, uint8 ident, net_buffer* buffer) 1004{ 1005 L2capFrame* cmd = NULL; 1006 1007 /* TODO: review this command... Get command data */ 1008 NetBufferHeaderReader<l2cap_cmd_rej_cp> command(buffer); 1009 status_t status = command.Status(); 1010 if (status < B_OK) { 1011 return ENOBUFS; 1012 } 1013 1014 command->reason = le16toh(command->reason); 1015 1016 debugf("reason=%d\n", command->reason); 1017 1018 command.Remove(); 1019 1020 /* Check if we have pending command descriptor */ 1021 cmd = btCoreData->SignalByIdent(conn, ident); 1022 if (cmd != NULL) { 1023 /* If command timeout already happened then ignore reject */ 1024 if (btCoreData->UnTimeoutSignal(cmd) != 0) { 1025 gBufferModule->free(buffer); 1026 return ETIMEDOUT; 1027 } 1028 1029 1030 switch (cmd->code) { 1031 case L2CAP_CON_REQ: 1032 //INDICATION l2cap_l2ca_con_rsp(cmd->channel, cmd->token, cp->reason, 0); 1033 btCoreData->RemoveChannel(conn, cmd->channel->scid); 1034 break; 1035 1036 case L2CAP_CFG_REQ: 1037 //INDICATION l2cap_l2ca_cfg_rsp(cmd->channel, cmd->token, cp->reason); 1038 break; 1039 1040 case L2CAP_DISCON_REQ: 1041 //INDICATION l2cap_l2ca_discon_rsp(cmd->channel, cmd->token, cp->reason); 1042 btCoreData->RemoveChannel(conn, cmd->channel->scid); 1043 break; 1044 1045 case L2CAP_ECHO_REQ: 1046 //INDICATION l2cap_l2ca_ping_rsp(cmd->con, cmd->token, cp->reason, NULL); 1047 break; 1048 1049 case L2CAP_INFO_REQ: 1050 //INDICATION l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, cp->reason, NULL); 1051 break; 1052 1053 default: 1054 debugf("unexpected L2CAP_CommandRej. Unexpected opcode=%d\n", cmd->code); 1055 break; 1056 } 1057 1058 btCoreData->AcknowledgeSignal(cmd); 1059 1060 } else 1061 debugf("unexpected L2CAP_CommandRej command. Requested ident does not exist, ident=%d\n", ident); 1062 1063 return B_OK; 1064} 1065 1066 1067#if 0 1068#pragma mark - Queuing Outgoing signals 1069#endif 1070 1071 1072status_t 1073send_l2cap_reject(HciConnection* conn, uint8 ident, uint16 reason, 1074 uint16 mtu, uint16 scid, uint16 dcid) 1075{ 1076 L2capFrame* cmd = NULL; 1077 1078 cmd = btCoreData->SpawnSignal(conn, NULL, l2cap_cmd_rej(ident, reason, 1079 mtu, scid, dcid), ident, L2CAP_CMD_REJ); 1080 if (cmd == NULL) 1081 return ENOMEM; 1082 1083 /* Link command to the queue */ 1084 SchedConnectionPurgeThread(conn); 1085 1086 return B_OK; 1087} 1088 1089 1090status_t 1091send_l2cap_con_rej(HciConnection* conn, uint8 ident, uint16 scid, uint16 dcid, 1092 uint16 result) 1093{ 1094 L2capFrame* cmd = NULL; 1095 1096 cmd = btCoreData->SpawnSignal(conn, NULL, 1097 l2cap_con_rsp(ident, scid, dcid, result, 0), ident, L2CAP_CON_RSP); 1098 if (cmd == NULL) 1099 return ENOMEM; 1100 1101 /* Link command to the queue */ 1102 SchedConnectionPurgeThread(conn); 1103 1104 return B_OK; 1105} 1106 1107 1108status_t 1109send_l2cap_cfg_rsp(HciConnection* conn, uint8 ident, uint16 scid, 1110 uint16 result, net_buffer* opt) 1111{ 1112 L2capFrame* cmd = NULL; 1113 1114 cmd = btCoreData->SpawnSignal(conn, NULL, 1115 l2cap_cfg_rsp(ident, scid, 0, result, opt), ident, L2CAP_CFG_RSP); 1116 if (cmd == NULL) { 1117 gBufferModule->free(opt); 1118 return ENOMEM; 1119 } 1120 1121 /* Link command to the queue */ 1122 SchedConnectionPurgeThread(conn); 1123 1124 return B_OK; 1125} 1126