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