194 else 195 m = NULL; 196 197 /* Process command */ 198 switch (hdr->code) { 199 case NG_L2CAP_CMD_REJ: 200 ng_l2cap_process_cmd_rej(con, hdr->ident); 201 break; 202 203 case NG_L2CAP_CON_REQ: 204 ng_l2cap_process_con_req(con, hdr->ident); 205 break; 206 207 case NG_L2CAP_CON_RSP: 208 ng_l2cap_process_con_rsp(con, hdr->ident); 209 break; 210 211 case NG_L2CAP_CFG_REQ: 212 ng_l2cap_process_cfg_req(con, hdr->ident); 213 break; 214 215 case NG_L2CAP_CFG_RSP: 216 ng_l2cap_process_cfg_rsp(con, hdr->ident); 217 break; 218 219 case NG_L2CAP_DISCON_REQ: 220 ng_l2cap_process_discon_req(con, hdr->ident); 221 break; 222 223 case NG_L2CAP_DISCON_RSP: 224 ng_l2cap_process_discon_rsp(con, hdr->ident); 225 break; 226 227 case NG_L2CAP_ECHO_REQ: 228 ng_l2cap_process_echo_req(con, hdr->ident); 229 break; 230 231 case NG_L2CAP_ECHO_RSP: 232 ng_l2cap_process_echo_rsp(con, hdr->ident); 233 break; 234 235 case NG_L2CAP_INFO_REQ: 236 ng_l2cap_process_info_req(con, hdr->ident); 237 break; 238 239 case NG_L2CAP_INFO_RSP: 240 ng_l2cap_process_info_rsp(con, hdr->ident); 241 break; 242 243 default: 244 NG_L2CAP_ERR( 245"%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", 246 __func__, NG_NODE_NAME(l2cap->node), 247 hdr->code, hdr->ident, hdr->length); 248 249 /* 250 * Send L2CAP_CommandRej. Do not really care 251 * about the result 252 */ 253 254 send_l2cap_reject(con, hdr->ident, 255 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); 256 NG_FREE_M(con->rx_pkt); 257 break; 258 } 259 260 con->rx_pkt = m; 261 } 262 263 return (0); 264} /* ng_l2cap_process_signal_cmd */ 265 266/* 267 * Process L2CAP_CommandRej command 268 */ 269 270static int 271ng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident) 272{ 273 ng_l2cap_p l2cap = con->l2cap; 274 ng_l2cap_cmd_rej_cp *cp = NULL; 275 ng_l2cap_cmd_p cmd = NULL; 276 277 /* Get command parameters */ 278 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 279 if (con->rx_pkt == NULL) 280 return (ENOBUFS); 281 282 cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *); 283 cp->reason = le16toh(cp->reason); 284 285 /* Check if we have pending command descriptor */ 286 cmd = ng_l2cap_cmd_by_ident(con, ident); 287 if (cmd != NULL) { 288 KASSERT((cmd->con == con), 289("%s: %s - invalid connection pointer!\n", 290 __func__, NG_NODE_NAME(con->l2cap->node))); 291 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 292("%s: %s - invalid command state, flags=%#x\n", 293 __func__, NG_NODE_NAME(con->l2cap->node), cmd->flags)); 294 295 ng_l2cap_command_untimeout(cmd); 296 ng_l2cap_unlink_cmd(cmd); 297 298 switch (cmd->code) { 299 case NG_L2CAP_CON_REQ: 300 ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0); 301 ng_l2cap_free_chan(cmd->ch); 302 break; 303 304 case NG_L2CAP_CFG_REQ: 305 ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason); 306 break; 307 308 case NG_L2CAP_DISCON_REQ: 309 ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason); 310 ng_l2cap_free_chan(cmd->ch); /* XXX free channel */ 311 break; 312 313 case NG_L2CAP_ECHO_REQ: 314 ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 315 cp->reason, NULL); 316 break; 317 318 case NG_L2CAP_INFO_REQ: 319 ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 320 cp->reason, NULL); 321 break; 322 323 default: 324 NG_L2CAP_ALERT( 325"%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n", 326 __func__, NG_NODE_NAME(l2cap->node), cmd->code); 327 break; 328 } 329 330 ng_l2cap_free_cmd(cmd); 331 } else 332 NG_L2CAP_ERR( 333"%s: %s - unexpected L2CAP_CommandRej command. " \ 334"Requested ident does not exist, ident=%d\n", 335 __func__, NG_NODE_NAME(l2cap->node), ident); 336 337 NG_FREE_M(con->rx_pkt); 338 339 return (0); 340} /* ng_l2cap_process_cmd_rej */ 341 342/* 343 * Process L2CAP_ConnectReq command 344 */ 345 346static int 347ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident) 348{ 349 ng_l2cap_p l2cap = con->l2cap; 350 struct mbuf *m = con->rx_pkt; 351 ng_l2cap_con_req_cp *cp = NULL; 352 ng_l2cap_chan_p ch = NULL; 353 int error = 0; 354 u_int16_t dcid, psm; 355 356 /* Get command parameters */ 357 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 358 if (m == NULL) 359 return (ENOBUFS); 360 361 cp = mtod(m, ng_l2cap_con_req_cp *); 362 psm = le16toh(cp->psm); 363 dcid = le16toh(cp->scid); 364 365 NG_FREE_M(m); 366 con->rx_pkt = NULL; 367 368 /* 369 * Create new channel and send L2CA_ConnectInd notification 370 * to the upper layer protocol. 371 */ 372 373 ch = ng_l2cap_new_chan(l2cap, con, psm); 374 if (ch == NULL) 375 return (send_l2cap_con_rej(con, ident, 0, dcid, 376 NG_L2CAP_NO_RESOURCES)); 377 378 /* Update channel IDs */ 379 ch->dcid = dcid; 380 381 /* Sent L2CA_ConnectInd notification to the upper layer */ 382 ch->ident = ident; 383 ch->state = NG_L2CAP_W4_L2CA_CON_RSP; 384 385 error = ng_l2cap_l2ca_con_ind(ch); 386 if (error != 0) { 387 send_l2cap_con_rej(con, ident, ch->scid, dcid, 388 (error == ENOMEM)? NG_L2CAP_NO_RESOURCES : 389 NG_L2CAP_PSM_NOT_SUPPORTED); 390 ng_l2cap_free_chan(ch); 391 } 392 393 return (error); 394} /* ng_l2cap_process_con_req */ 395 396/* 397 * Process L2CAP_ConnectRsp command 398 */ 399 400static int 401ng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident) 402{ 403 ng_l2cap_p l2cap = con->l2cap; 404 struct mbuf *m = con->rx_pkt; 405 ng_l2cap_con_rsp_cp *cp = NULL; 406 ng_l2cap_cmd_p cmd = NULL; 407 u_int16_t scid, dcid, result, status; 408 int error = 0; 409 410 /* Get command parameters */ 411 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 412 if (m == NULL) 413 return (ENOBUFS); 414 415 cp = mtod(m, ng_l2cap_con_rsp_cp *); 416 dcid = le16toh(cp->dcid); 417 scid = le16toh(cp->scid); 418 result = le16toh(cp->result); 419 status = le16toh(cp->status); 420 421 NG_FREE_M(m); 422 con->rx_pkt = NULL; 423 424 /* Check if we have pending command descriptor */ 425 cmd = ng_l2cap_cmd_by_ident(con, ident); 426 if (cmd == NULL) { 427 NG_L2CAP_ERR( 428"%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n", 429 __func__, NG_NODE_NAME(l2cap->node), ident, 430 con->con_handle); 431 432 return (ENOENT); 433 } 434 435 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 436("%s: %s - invalid command state, flags=%#x\n", 437 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 438 439 /* Verify channel state, if invalid - do nothing */ 440 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) { 441 NG_L2CAP_ERR( 442"%s: %s - unexpected L2CAP_ConnectRsp. " \ 443"Invalid channel state, cid=%d, state=%d\n", 444 __func__, NG_NODE_NAME(l2cap->node), scid, 445 cmd->ch->state); 446 goto reject; 447 } 448 449 /* Verify CIDs and send reject if does not match */ 450 if (cmd->ch->scid != scid) { 451 NG_L2CAP_ERR( 452"%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n", 453 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 454 scid); 455 goto reject; 456 } 457 458 /* 459 * Looks good. We got confirmation from our peer. Now process 460 * it. First disable RTX timer. Then check the result and send 461 * notification to the upper layer. 462 */ 463 464 ng_l2cap_command_untimeout(cmd); 465 466 if (result == NG_L2CAP_PENDING) { 467 /* 468 * Our peer wants more time to complete connection. We shall 469 * start ERTX timer and wait. Keep command in the list. 470 */ 471 472 cmd->ch->dcid = dcid; 473 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout()); 474 475 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 476 result, status); 477 if (error != 0) 478 ng_l2cap_free_chan(cmd->ch); 479 } else { 480 ng_l2cap_unlink_cmd(cmd); 481 482 if (result == NG_L2CAP_SUCCESS) { 483 /* 484 * Channel is open. Complete command and move to CONFIG 485 * state. Since we have sent positive confirmation we 486 * expect to receive L2CA_Config request from the upper 487 * layer protocol. 488 */ 489 490 cmd->ch->dcid = dcid; 491 cmd->ch->state = NG_L2CAP_CONFIG; 492 } else 493 /* There was an error, so close the channel */ 494 NG_L2CAP_INFO( 495"%s: %s - failed to open L2CAP channel, result=%d, status=%d\n", 496 __func__, NG_NODE_NAME(l2cap->node), result, 497 status); 498 499 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 500 result, status); 501 502 /* XXX do we have to remove the channel on error? */ 503 if (error != 0 || result != NG_L2CAP_SUCCESS) 504 ng_l2cap_free_chan(cmd->ch); 505 506 ng_l2cap_free_cmd(cmd); 507 } 508 509 return (error); 510 511reject: 512 /* Send reject. Do not really care about the result */ 513 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 514 515 return (0); 516} /* ng_l2cap_process_con_rsp */ 517 518/* 519 * Process L2CAP_ConfigReq command 520 */ 521 522static int 523ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident) 524{ 525 ng_l2cap_p l2cap = con->l2cap; 526 struct mbuf *m = con->rx_pkt; 527 ng_l2cap_cfg_req_cp *cp = NULL; 528 ng_l2cap_chan_p ch = NULL; 529 u_int16_t dcid, respond, result; 530 ng_l2cap_cfg_opt_t hdr; 531 ng_l2cap_cfg_opt_val_t val; 532 int off, error = 0; 533 534 /* Get command parameters */ 535 con->rx_pkt = NULL; 536 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 537 if (m == NULL) 538 return (ENOBUFS); 539 540 cp = mtod(m, ng_l2cap_cfg_req_cp *); 541 dcid = le16toh(cp->dcid); 542 respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 543 m_adj(m, sizeof(*cp)); 544 545 /* Check if we have this channel and it is in valid state */ 546 ch = ng_l2cap_chan_by_scid(l2cap, dcid); 547 if (ch == NULL) { 548 NG_L2CAP_ERR( 549"%s: %s - unexpected L2CAP_ConfigReq command. " \ 550"Channel does not exist, cid=%d\n", 551 __func__, NG_NODE_NAME(l2cap->node), dcid); 552 goto reject; 553 } 554 555 /* Verify channel state */ 556 if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) { 557 NG_L2CAP_ERR( 558"%s: %s - unexpected L2CAP_ConfigReq. " \ 559"Invalid channel state, cid=%d, state=%d\n", 560 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 561 goto reject; 562 } 563 564 if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */ 565 ch->cfg_state = 0; 566 ch->state = NG_L2CAP_CONFIG; 567 } 568 569 for (result = 0, off = 0; ; ) { 570 error = get_next_l2cap_opt(m, &off, &hdr, &val); 571 if (error == 0) { /* We done with this packet */ 572 NG_FREE_M(m); 573 break; 574 } else if (error > 0) { /* Got option */ 575 switch (hdr.type) { 576 case NG_L2CAP_OPT_MTU: 577 ch->omtu = val.mtu; 578 break; 579 580 case NG_L2CAP_OPT_FLUSH_TIMO: 581 ch->flush_timo = val.flush_timo; 582 break; 583 584 case NG_L2CAP_OPT_QOS: 585 bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow)); 586 break; 587 588 default: 589 KASSERT(0, 590("%s: %s - unknown option: %d\n", __func__, NG_NODE_NAME(l2cap->node), 591 hdr.type)); 592 break; 593 } 594 } else { /* Oops, something is wrong */ 595 respond = 1; 596 597 if (error == -3) { 598 599 /* 600 * Adjust mbuf so we can get to the start 601 * of the first option we did not like. 602 */ 603 604 m_adj(m, off - sizeof(hdr)); 605 m->m_pkthdr.len = sizeof(hdr) + hdr.length; 606 607 result = NG_L2CAP_UNKNOWN_OPTION; 608 } else { 609 /* XXX FIXME Send other reject codes? */ 610 NG_FREE_M(m); 611 result = NG_L2CAP_REJECT; 612 } 613 614 break; 615 } 616 } 617 618 /* 619 * Now check and see if we have to respond. If everything was OK then 620 * respond contain "C flag" and (if set) we will respond with empty 621 * packet and will wait for more options. 622 * 623 * Other case is that we did not like peer's options and will respond 624 * with L2CAP_Config response command with Reject error code. 625 * 626 * When "respond == 0" than we have received all options and we will 627 * sent L2CA_ConfigInd event to the upper layer protocol. 628 */ 629 630 if (respond) { 631 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m); 632 if (error != 0) { 633 ng_l2cap_l2ca_discon_ind(ch); 634 ng_l2cap_free_chan(ch); 635 } 636 } else { 637 /* Send L2CA_ConfigInd event to the upper layer protocol */ 638 ch->ident = ident; 639 error = ng_l2cap_l2ca_cfg_ind(ch); 640 if (error != 0) 641 ng_l2cap_free_chan(ch); 642 } 643 644 return (error); 645 646reject: 647 /* Send reject. Do not really care about the result */ 648 NG_FREE_M(m); 649 650 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid); 651 652 return (0); 653} /* ng_l2cap_process_cfg_req */ 654 655/* 656 * Process L2CAP_ConfigRsp command 657 */ 658 659static int 660ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident) 661{ 662 ng_l2cap_p l2cap = con->l2cap; 663 struct mbuf *m = con->rx_pkt; 664 ng_l2cap_cfg_rsp_cp *cp = NULL; 665 ng_l2cap_cmd_p cmd = NULL; 666 u_int16_t scid, cflag, result; 667 ng_l2cap_cfg_opt_t hdr; 668 ng_l2cap_cfg_opt_val_t val; 669 int off, error = 0; 670 671 /* Get command parameters */ 672 con->rx_pkt = NULL; 673 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 674 if (m == NULL) 675 return (ENOBUFS); 676 677 cp = mtod(m, ng_l2cap_cfg_rsp_cp *); 678 scid = le16toh(cp->scid); 679 cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 680 result = le16toh(cp->result); 681 m_adj(m, sizeof(*cp)); 682 683 /* Check if we have this command */ 684 cmd = ng_l2cap_cmd_by_ident(con, ident); 685 if (cmd == NULL) { 686 NG_L2CAP_ERR( 687"%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", 688 __func__, NG_NODE_NAME(l2cap->node), ident, 689 con->con_handle); 690 NG_FREE_M(m); 691 692 return (ENOENT); 693 } 694 695 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 696("%s: %s - invalid command state, flags=%#x\n", 697 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 698 699 /* Verify CIDs and send reject if does not match */ 700 if (cmd->ch->scid != scid) { 701 NG_L2CAP_ERR( 702"%s: %s - unexpected L2CAP_ConfigRsp. " \ 703"Channel ID does not match, scid=%d(%d)\n", 704 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 705 scid); 706 goto reject; 707 } 708 709 /* Verify channel state and reject if invalid */ 710 if (cmd->ch->state != NG_L2CAP_CONFIG) { 711 NG_L2CAP_ERR( 712"%s: %s - unexpected L2CAP_ConfigRsp. " \ 713"Invalid channel state, scid=%d, state=%d\n", 714 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 715 cmd->ch->state); 716 goto reject; 717 } 718 719 /* 720 * Looks like it is our response, so process it. First parse options, 721 * then verify C flag. If it is set then we shall expect more 722 * configuration options from the peer and we will wait. Otherwise we 723 * have received all options and we will send L2CA_ConfigRsp event to 724 * the upper layer protocol. 725 */ 726 727 ng_l2cap_command_untimeout(cmd); 728 729 for (off = 0; ; ) { 730 error = get_next_l2cap_opt(m, &off, &hdr, &val); 731 if (error == 0) /* We done with this packet */ 732 break; 733 else if (error > 0) { /* Got option */ 734 switch (hdr.type) { 735 case NG_L2CAP_OPT_MTU: 736 cmd->ch->imtu = val.mtu; 737 break; 738 739 case NG_L2CAP_OPT_FLUSH_TIMO: 740 cmd->ch->flush_timo = val.flush_timo; 741 break; 742 743 case NG_L2CAP_OPT_QOS: 744 bcopy(&val.flow, &cmd->ch->oflow, 745 sizeof(cmd->ch->oflow)); 746 break; 747 748 default: 749 KASSERT(0, 750("%s: %s - unknown option: %d\n", __func__, NG_NODE_NAME(l2cap->node), 751 hdr.type)); 752 break; 753 } 754 } else { 755 /* 756 * XXX FIXME What to do here? 757 * 758 * This is really BAD :( options packet was broken, 759 * so let upper layer know and do not wait for more 760 * options 761 */ 762 763 NG_L2CAP_ALERT( 764"%s: %s - failed to parse configuration options, error=%d\n", 765 __func__, NG_NODE_NAME(l2cap->node), error); 766 767 result = NG_L2CAP_UNKNOWN; 768 cflag = 0; 769 770 break; 771 } 772 } 773 774 NG_FREE_M(m); 775 776 if (cflag) /* Restart timer and wait for more options */ 777 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout()); 778 else { 779 ng_l2cap_unlink_cmd(cmd); 780 781 /* Send L2CA_Config response to the upper layer protocol */ 782 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result); 783 if (error != 0) { 784 /* 785 * XXX FIXME what to do here? we were not able to send 786 * response to the upper layer protocol, so for now 787 * just close the channel. Send L2CAP_Disconnect to 788 * remote peer? 789 */ 790 791 NG_L2CAP_ERR( 792"%s: %s - failed to send L2CA_Config response, error=%d\n", 793 __func__, NG_NODE_NAME(l2cap->node), error); 794 795 ng_l2cap_free_chan(cmd->ch); 796 } 797 798 ng_l2cap_free_cmd(cmd); 799 } 800 801 return (error); 802 803reject: 804 /* Send reject. Do not really care about the result */ 805 NG_FREE_M(m); 806 807 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0); 808 809 return (0); 810} /* ng_l2cap_process_cfg_rsp */ 811 812/* 813 * Process L2CAP_DisconnectReq command 814 */ 815 816static int 817ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident) 818{ 819 ng_l2cap_p l2cap = con->l2cap; 820 ng_l2cap_discon_req_cp *cp = NULL; 821 ng_l2cap_chan_p ch = NULL; 822 ng_l2cap_cmd_p cmd = NULL; 823 u_int16_t scid, dcid; 824 825 /* Get command parameters */ 826 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 827 if (con->rx_pkt == NULL) 828 return (ENOBUFS); 829 830 cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *); 831 dcid = le16toh(cp->dcid); 832 scid = le16toh(cp->scid); 833 834 NG_FREE_M(con->rx_pkt); 835 836 /* Check if we have this channel and it is in valid state */ 837 ch = ng_l2cap_chan_by_scid(l2cap, dcid); 838 if (ch == NULL) { 839 NG_L2CAP_ERR( 840"%s: %s - unexpected L2CAP_DisconnectReq message. " \ 841"Channel does not exist, cid=%d\n", 842 __func__, NG_NODE_NAME(l2cap->node), dcid); 843 goto reject; 844 } 845 846 /* XXX Verify channel state and reject if invalid -- is that true? */ 847 if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) { 848 NG_L2CAP_ERR( 849"%s: %s - unexpected L2CAP_DisconnectReq. " \ 850"Invalid channel state, cid=%d, state=%d\n", 851 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 852 goto reject; 853 } 854 855 /* Match destination channel ID */ 856 if (ch->dcid != scid || ch->scid != dcid) { 857 NG_L2CAP_ERR( 858"%s: %s - unexpected L2CAP_DisconnectReq. " \ 859"Channel IDs does not match, channel: scid=%d, dcid=%d, " \ 860"request: scid=%d, dcid=%d\n", 861 __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid, 862 scid, dcid); 863 goto reject; 864 } 865 866 /* 867 * Looks good, so notify upper layer protocol that channel is about 868 * to be disconnected and send L2CA_DisconnectInd message. Then respond 869 * with L2CAP_DisconnectRsp. 870 */ 871 872 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */ 873 ng_l2cap_free_chan(ch); 874 875 /* Send L2CAP_DisconnectRsp */ 876 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0); 877 if (cmd == NULL) 878 return (ENOMEM); 879 880 _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid); 881 if (cmd->aux == NULL) { 882 ng_l2cap_free_cmd(cmd); 883 884 return (ENOBUFS); 885 } 886 887 /* Link command to the queue */ 888 ng_l2cap_link_cmd(con, cmd); 889 ng_l2cap_lp_deliver(con); 890 891 return (0); 892 893reject: 894 /* Send reject. Do not really care about the result */ 895 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 896 897 return (0); 898} /* ng_l2cap_process_discon_req */ 899 900/* 901 * Process L2CAP_DisconnectRsp command 902 */ 903 904static int 905ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident) 906{ 907 ng_l2cap_p l2cap = con->l2cap; 908 ng_l2cap_discon_rsp_cp *cp = NULL; 909 ng_l2cap_cmd_p cmd = NULL; 910 u_int16_t scid, dcid; 911 int error = 0; 912 913 /* Get command parameters */ 914 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 915 if (con->rx_pkt == NULL) 916 return (ENOBUFS); 917 918 cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *); 919 dcid = le16toh(cp->dcid); 920 scid = le16toh(cp->scid); 921 922 NG_FREE_M(con->rx_pkt); 923 924 /* Check if we have pending command descriptor */ 925 cmd = ng_l2cap_cmd_by_ident(con, ident); 926 if (cmd == NULL) { 927 NG_L2CAP_ERR( 928"%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", 929 __func__, NG_NODE_NAME(l2cap->node), ident, 930 con->con_handle); 931 goto out; 932 } 933 934 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 935("%s: %s - invalid command state, flags=%#x\n", 936 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 937 938 /* Verify channel state, do nothing if invalid */ 939 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 940 NG_L2CAP_ERR( 941"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 942"Invalid channel state, cid=%d, state=%d\n", 943 __func__, NG_NODE_NAME(l2cap->node), scid, 944 cmd->ch->state); 945 goto out; 946 } 947 948 /* Verify CIDs and send reject if does not match */ 949 if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) { 950 NG_L2CAP_ERR( 951"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 952"Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n", 953 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 954 scid, cmd->ch->dcid, dcid); 955 goto out; 956 } 957 958 /* 959 * Looks like we have successfuly disconnected channel, 960 * so notify upper layer. 961 */ 962 963 ng_l2cap_command_untimeout(cmd); 964 error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS); 965 ng_l2cap_free_chan(cmd->ch); /* this will free commands too */ 966out: 967 return (error); 968} /* ng_l2cap_process_discon_rsp */ 969 970/* 971 * Process L2CAP_EchoReq command 972 */ 973 974static int 975ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident) 976{ 977 ng_l2cap_p l2cap = con->l2cap; 978 ng_l2cap_cmd_hdr_t *hdr = NULL; 979 ng_l2cap_cmd_p cmd = NULL; 980 981 con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr)); 982 if (con->rx_pkt == NULL) { 983 NG_L2CAP_ALERT( 984"%s: %s - ng_l2cap_prepend() failed, size=%d\n", 985 __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr)); 986 987 return (ENOBUFS); 988 } 989 990 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 991 hdr->code = NG_L2CAP_ECHO_RSP; 992 hdr->ident = ident; 993 hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 994 995 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0); 996 if (cmd == NULL) { 997 NG_FREE_M(con->rx_pkt); 998 999 return (ENOBUFS); 1000 } 1001 1002 /* Attach data and link command to the queue */ 1003 cmd->aux = con->rx_pkt; 1004 con->rx_pkt = NULL; 1005 ng_l2cap_link_cmd(con, cmd); 1006 ng_l2cap_lp_deliver(con); 1007 1008 return (0); 1009} /* ng_l2cap_process_echo_req */ 1010 1011/* 1012 * Process L2CAP_EchoRsp command 1013 */ 1014 1015static int 1016ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident) 1017{ 1018 ng_l2cap_p l2cap = con->l2cap; 1019 ng_l2cap_cmd_p cmd = NULL; 1020 int error = 0; 1021 1022 /* Check if we have this command */ 1023 cmd = ng_l2cap_cmd_by_ident(con, ident); 1024 if (cmd != NULL) { 1025 KASSERT((cmd->con == con), 1026("%s: %s - invalid connection pointer!\n", 1027 __func__, NG_NODE_NAME(l2cap->node))); 1028 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 1029("%s: %s - invalid command state, flags=%#x\n", 1030 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 1031 1032 ng_l2cap_command_untimeout(cmd); 1033 ng_l2cap_unlink_cmd(cmd); 1034 1035 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 1036 NG_L2CAP_SUCCESS, con->rx_pkt); 1037 1038 ng_l2cap_free_cmd(cmd); 1039 con->rx_pkt = NULL; 1040 } else { 1041 NG_L2CAP_ERR( 1042"%s: %s - unexpected L2CAP_EchoRsp command. " \ 1043"Requested ident does not exist, ident=%d\n", 1044 __func__, NG_NODE_NAME(l2cap->node), ident); 1045 NG_FREE_M(con->rx_pkt); 1046 } 1047 1048 return (error); 1049} /* ng_l2cap_process_echo_rsp */ 1050 1051/* 1052 * Process L2CAP_InfoReq command 1053 */ 1054 1055static int 1056ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident) 1057{ 1058 ng_l2cap_p l2cap = con->l2cap; 1059 ng_l2cap_cmd_p cmd = NULL; 1060 u_int16_t type; 1061 1062 /* Get command parameters */ 1063 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp)); 1064 if (con->rx_pkt == NULL) 1065 return (ENOBUFS); 1066 1067 type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type); 1068 NG_FREE_M(con->rx_pkt); 1069 1070 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0); 1071 if (cmd == NULL) 1072 return (ENOMEM); 1073 1074 switch (type) { 1075 case NG_L2CAP_CONNLESS_MTU: 1076 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU, 1077 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT); 1078 break; 1079 1080 default: 1081 _ng_l2cap_info_rsp(cmd->aux, ident, type, 1082 NG_L2CAP_NOT_SUPPORTED, 0); 1083 break; 1084 } 1085 1086 if (cmd->aux == NULL) { 1087 ng_l2cap_free_cmd(cmd); 1088 1089 return (ENOBUFS); 1090 } 1091 1092 /* Link command to the queue */ 1093 ng_l2cap_link_cmd(con, cmd); 1094 ng_l2cap_lp_deliver(con); 1095 1096 return (0); 1097} /* ng_l2cap_process_info_req */ 1098 1099/* 1100 * Process L2CAP_InfoRsp command 1101 */ 1102 1103static int 1104ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident) 1105{ 1106 ng_l2cap_p l2cap = con->l2cap; 1107 ng_l2cap_info_rsp_cp *cp = NULL; 1108 ng_l2cap_cmd_p cmd = NULL; 1109 int error = 0; 1110 1111 /* Get command parameters */ 1112 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1113 if (con->rx_pkt == NULL) 1114 return (ENOBUFS); 1115 1116 cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *); 1117 cp->type = le16toh(cp->type); 1118 cp->result = le16toh(cp->result); 1119 m_adj(con->rx_pkt, sizeof(*cp)); 1120 1121 /* Check if we have pending command descriptor */ 1122 cmd = ng_l2cap_cmd_by_ident(con, ident); 1123 if (cmd == NULL) { 1124 NG_L2CAP_ERR( 1125"%s: %s - unexpected L2CAP_InfoRsp command. " \ 1126"Requested ident does not exist, ident=%d\n", 1127 __func__, NG_NODE_NAME(l2cap->node), ident); 1128 NG_FREE_M(con->rx_pkt); 1129 1130 return (ENOENT); 1131 } 1132 1133 KASSERT((cmd->con == con), 1134("%s: %s - invalid connection pointer!\n", 1135 __func__, NG_NODE_NAME(l2cap->node))); 1136 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 1137("%s: %s - invalid command state, flags=%#x\n", 1138 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 1139 1140 ng_l2cap_command_untimeout(cmd); 1141 ng_l2cap_unlink_cmd(cmd); 1142 1143 if (cp->result == NG_L2CAP_SUCCESS) { 1144 switch (cp->type) { 1145 case NG_L2CAP_CONNLESS_MTU: 1146 if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t)) 1147 *mtod(con->rx_pkt, u_int16_t *) = 1148 le16toh(*mtod(con->rx_pkt,u_int16_t *)); 1149 else { 1150 cp->result = NG_L2CAP_UNKNOWN; /* XXX */ 1151 1152 NG_L2CAP_ERR( 1153"%s: %s - invalid L2CAP_InfoRsp command. " \ 1154"Bad connectionless MTU parameter, len=%d\n", 1155 __func__, NG_NODE_NAME(l2cap->node), 1156 con->rx_pkt->m_pkthdr.len); 1157 } 1158 break; 1159 1160 default: 1161 NG_L2CAP_WARN( 1162"%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n", 1163 __func__, NG_NODE_NAME(l2cap->node), cp->type); 1164 break; 1165 } 1166 } 1167 1168 error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 1169 cp->result, con->rx_pkt); 1170 1171 ng_l2cap_free_cmd(cmd); 1172 con->rx_pkt = NULL; 1173 1174 return (error); 1175} /* ng_l2cap_process_info_rsp */ 1176 1177/* 1178 * Send L2CAP reject 1179 */ 1180 1181static int 1182send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason, 1183 u_int16_t mtu, u_int16_t scid, u_int16_t dcid) 1184{ 1185 ng_l2cap_cmd_p cmd = NULL; 1186 1187 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0); 1188 if (cmd == NULL) 1189 return (ENOMEM); 1190 1191 _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid); 1192 if (cmd->aux == NULL) { 1193 ng_l2cap_free_cmd(cmd); 1194 1195 return (ENOBUFS); 1196 } 1197 1198 /* Link command to the queue */ 1199 ng_l2cap_link_cmd(con, cmd); 1200 ng_l2cap_lp_deliver(con); 1201 1202 return (0); 1203} /* send_l2cap_reject */ 1204 1205/* 1206 * Send L2CAP connection reject 1207 */ 1208 1209static int 1210send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1211 u_int16_t dcid, u_int16_t result) 1212{ 1213 ng_l2cap_cmd_p cmd = NULL; 1214 1215 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0); 1216 if (cmd == NULL) 1217 return (ENOMEM); 1218 1219 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0); 1220 if (cmd->aux == NULL) { 1221 ng_l2cap_free_cmd(cmd); 1222 1223 return (ENOBUFS); 1224 } 1225 1226 /* Link command to the queue */ 1227 ng_l2cap_link_cmd(con, cmd); 1228 ng_l2cap_lp_deliver(con); 1229 1230 return (0); 1231} /* send_l2cap_con_rej */ 1232 1233/* 1234 * Send L2CAP config response 1235 */ 1236 1237static int 1238send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1239 u_int16_t result, struct mbuf *opt) 1240{ 1241 ng_l2cap_cmd_p cmd = NULL; 1242 1243 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0); 1244 if (cmd == NULL) { 1245 NG_FREE_M(opt); 1246 1247 return (ENOMEM); 1248 } 1249 1250 _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt); 1251 if (cmd->aux == NULL) { 1252 ng_l2cap_free_cmd(cmd); 1253 1254 return (ENOBUFS); 1255 } 1256 1257 /* Link command to the queue */ 1258 ng_l2cap_link_cmd(con, cmd); 1259 ng_l2cap_lp_deliver(con); 1260 1261 return (0); 1262} /* send_l2cap_cfg_rsp */ 1263 1264/* 1265 * Get next L2CAP configuration option 1266 * 1267 * Return codes: 1268 * 0 no option 1269 * 1 we have got option 1270 * -1 header too short 1271 * -2 bad option value or length 1272 * -3 unknown option 1273 */ 1274 1275static int 1276get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr, 1277 ng_l2cap_cfg_opt_val_p val) 1278{ 1279 int hint, len = m->m_pkthdr.len - (*off); 1280 1281 if (len == 0) 1282 return (0); 1283 if (len < 0 || len < sizeof(*hdr)) 1284 return (-1); 1285 1286 m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr); 1287 *off += sizeof(*hdr); 1288 len -= sizeof(*hdr); 1289 1290 hint = NG_L2CAP_OPT_HINT(hdr->type); 1291 hdr->type &= NG_L2CAP_OPT_HINT_MASK; 1292 1293 switch (hdr->type) { 1294 case NG_L2CAP_OPT_MTU: 1295 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length) 1296 return (-2); 1297 1298 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val); 1299 val->mtu = le16toh(val->mtu); 1300 *off += NG_L2CAP_OPT_MTU_SIZE; 1301 break; 1302 1303 case NG_L2CAP_OPT_FLUSH_TIMO: 1304 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE || 1305 len < hdr->length) 1306 return (-2); 1307 1308 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val); 1309 val->flush_timo = le16toh(val->flush_timo); 1310 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE; 1311 break; 1312 1313 case NG_L2CAP_OPT_QOS: 1314 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length) 1315 return (-2); 1316 1317 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val); 1318 val->flow.token_rate = le32toh(val->flow.token_rate); 1319 val->flow.token_bucket_size = 1320 le32toh(val->flow.token_bucket_size); 1321 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); 1322 val->flow.latency = le32toh(val->flow.latency); 1323 val->flow.delay_variation = le32toh(val->flow.delay_variation); 1324 *off += NG_L2CAP_OPT_QOS_SIZE; 1325 break; 1326 1327 default: 1328 if (hint) 1329 *off += hdr->length; 1330 else 1331 return (-3); 1332 break; 1333 } 1334 1335 return (1); 1336} /* get_next_l2cap_opt */ 1337
| 194 else 195 m = NULL; 196 197 /* Process command */ 198 switch (hdr->code) { 199 case NG_L2CAP_CMD_REJ: 200 ng_l2cap_process_cmd_rej(con, hdr->ident); 201 break; 202 203 case NG_L2CAP_CON_REQ: 204 ng_l2cap_process_con_req(con, hdr->ident); 205 break; 206 207 case NG_L2CAP_CON_RSP: 208 ng_l2cap_process_con_rsp(con, hdr->ident); 209 break; 210 211 case NG_L2CAP_CFG_REQ: 212 ng_l2cap_process_cfg_req(con, hdr->ident); 213 break; 214 215 case NG_L2CAP_CFG_RSP: 216 ng_l2cap_process_cfg_rsp(con, hdr->ident); 217 break; 218 219 case NG_L2CAP_DISCON_REQ: 220 ng_l2cap_process_discon_req(con, hdr->ident); 221 break; 222 223 case NG_L2CAP_DISCON_RSP: 224 ng_l2cap_process_discon_rsp(con, hdr->ident); 225 break; 226 227 case NG_L2CAP_ECHO_REQ: 228 ng_l2cap_process_echo_req(con, hdr->ident); 229 break; 230 231 case NG_L2CAP_ECHO_RSP: 232 ng_l2cap_process_echo_rsp(con, hdr->ident); 233 break; 234 235 case NG_L2CAP_INFO_REQ: 236 ng_l2cap_process_info_req(con, hdr->ident); 237 break; 238 239 case NG_L2CAP_INFO_RSP: 240 ng_l2cap_process_info_rsp(con, hdr->ident); 241 break; 242 243 default: 244 NG_L2CAP_ERR( 245"%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", 246 __func__, NG_NODE_NAME(l2cap->node), 247 hdr->code, hdr->ident, hdr->length); 248 249 /* 250 * Send L2CAP_CommandRej. Do not really care 251 * about the result 252 */ 253 254 send_l2cap_reject(con, hdr->ident, 255 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); 256 NG_FREE_M(con->rx_pkt); 257 break; 258 } 259 260 con->rx_pkt = m; 261 } 262 263 return (0); 264} /* ng_l2cap_process_signal_cmd */ 265 266/* 267 * Process L2CAP_CommandRej command 268 */ 269 270static int 271ng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident) 272{ 273 ng_l2cap_p l2cap = con->l2cap; 274 ng_l2cap_cmd_rej_cp *cp = NULL; 275 ng_l2cap_cmd_p cmd = NULL; 276 277 /* Get command parameters */ 278 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 279 if (con->rx_pkt == NULL) 280 return (ENOBUFS); 281 282 cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *); 283 cp->reason = le16toh(cp->reason); 284 285 /* Check if we have pending command descriptor */ 286 cmd = ng_l2cap_cmd_by_ident(con, ident); 287 if (cmd != NULL) { 288 KASSERT((cmd->con == con), 289("%s: %s - invalid connection pointer!\n", 290 __func__, NG_NODE_NAME(con->l2cap->node))); 291 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 292("%s: %s - invalid command state, flags=%#x\n", 293 __func__, NG_NODE_NAME(con->l2cap->node), cmd->flags)); 294 295 ng_l2cap_command_untimeout(cmd); 296 ng_l2cap_unlink_cmd(cmd); 297 298 switch (cmd->code) { 299 case NG_L2CAP_CON_REQ: 300 ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0); 301 ng_l2cap_free_chan(cmd->ch); 302 break; 303 304 case NG_L2CAP_CFG_REQ: 305 ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason); 306 break; 307 308 case NG_L2CAP_DISCON_REQ: 309 ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason); 310 ng_l2cap_free_chan(cmd->ch); /* XXX free channel */ 311 break; 312 313 case NG_L2CAP_ECHO_REQ: 314 ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 315 cp->reason, NULL); 316 break; 317 318 case NG_L2CAP_INFO_REQ: 319 ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 320 cp->reason, NULL); 321 break; 322 323 default: 324 NG_L2CAP_ALERT( 325"%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n", 326 __func__, NG_NODE_NAME(l2cap->node), cmd->code); 327 break; 328 } 329 330 ng_l2cap_free_cmd(cmd); 331 } else 332 NG_L2CAP_ERR( 333"%s: %s - unexpected L2CAP_CommandRej command. " \ 334"Requested ident does not exist, ident=%d\n", 335 __func__, NG_NODE_NAME(l2cap->node), ident); 336 337 NG_FREE_M(con->rx_pkt); 338 339 return (0); 340} /* ng_l2cap_process_cmd_rej */ 341 342/* 343 * Process L2CAP_ConnectReq command 344 */ 345 346static int 347ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident) 348{ 349 ng_l2cap_p l2cap = con->l2cap; 350 struct mbuf *m = con->rx_pkt; 351 ng_l2cap_con_req_cp *cp = NULL; 352 ng_l2cap_chan_p ch = NULL; 353 int error = 0; 354 u_int16_t dcid, psm; 355 356 /* Get command parameters */ 357 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 358 if (m == NULL) 359 return (ENOBUFS); 360 361 cp = mtod(m, ng_l2cap_con_req_cp *); 362 psm = le16toh(cp->psm); 363 dcid = le16toh(cp->scid); 364 365 NG_FREE_M(m); 366 con->rx_pkt = NULL; 367 368 /* 369 * Create new channel and send L2CA_ConnectInd notification 370 * to the upper layer protocol. 371 */ 372 373 ch = ng_l2cap_new_chan(l2cap, con, psm); 374 if (ch == NULL) 375 return (send_l2cap_con_rej(con, ident, 0, dcid, 376 NG_L2CAP_NO_RESOURCES)); 377 378 /* Update channel IDs */ 379 ch->dcid = dcid; 380 381 /* Sent L2CA_ConnectInd notification to the upper layer */ 382 ch->ident = ident; 383 ch->state = NG_L2CAP_W4_L2CA_CON_RSP; 384 385 error = ng_l2cap_l2ca_con_ind(ch); 386 if (error != 0) { 387 send_l2cap_con_rej(con, ident, ch->scid, dcid, 388 (error == ENOMEM)? NG_L2CAP_NO_RESOURCES : 389 NG_L2CAP_PSM_NOT_SUPPORTED); 390 ng_l2cap_free_chan(ch); 391 } 392 393 return (error); 394} /* ng_l2cap_process_con_req */ 395 396/* 397 * Process L2CAP_ConnectRsp command 398 */ 399 400static int 401ng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident) 402{ 403 ng_l2cap_p l2cap = con->l2cap; 404 struct mbuf *m = con->rx_pkt; 405 ng_l2cap_con_rsp_cp *cp = NULL; 406 ng_l2cap_cmd_p cmd = NULL; 407 u_int16_t scid, dcid, result, status; 408 int error = 0; 409 410 /* Get command parameters */ 411 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 412 if (m == NULL) 413 return (ENOBUFS); 414 415 cp = mtod(m, ng_l2cap_con_rsp_cp *); 416 dcid = le16toh(cp->dcid); 417 scid = le16toh(cp->scid); 418 result = le16toh(cp->result); 419 status = le16toh(cp->status); 420 421 NG_FREE_M(m); 422 con->rx_pkt = NULL; 423 424 /* Check if we have pending command descriptor */ 425 cmd = ng_l2cap_cmd_by_ident(con, ident); 426 if (cmd == NULL) { 427 NG_L2CAP_ERR( 428"%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n", 429 __func__, NG_NODE_NAME(l2cap->node), ident, 430 con->con_handle); 431 432 return (ENOENT); 433 } 434 435 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 436("%s: %s - invalid command state, flags=%#x\n", 437 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 438 439 /* Verify channel state, if invalid - do nothing */ 440 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) { 441 NG_L2CAP_ERR( 442"%s: %s - unexpected L2CAP_ConnectRsp. " \ 443"Invalid channel state, cid=%d, state=%d\n", 444 __func__, NG_NODE_NAME(l2cap->node), scid, 445 cmd->ch->state); 446 goto reject; 447 } 448 449 /* Verify CIDs and send reject if does not match */ 450 if (cmd->ch->scid != scid) { 451 NG_L2CAP_ERR( 452"%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n", 453 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 454 scid); 455 goto reject; 456 } 457 458 /* 459 * Looks good. We got confirmation from our peer. Now process 460 * it. First disable RTX timer. Then check the result and send 461 * notification to the upper layer. 462 */ 463 464 ng_l2cap_command_untimeout(cmd); 465 466 if (result == NG_L2CAP_PENDING) { 467 /* 468 * Our peer wants more time to complete connection. We shall 469 * start ERTX timer and wait. Keep command in the list. 470 */ 471 472 cmd->ch->dcid = dcid; 473 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout()); 474 475 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 476 result, status); 477 if (error != 0) 478 ng_l2cap_free_chan(cmd->ch); 479 } else { 480 ng_l2cap_unlink_cmd(cmd); 481 482 if (result == NG_L2CAP_SUCCESS) { 483 /* 484 * Channel is open. Complete command and move to CONFIG 485 * state. Since we have sent positive confirmation we 486 * expect to receive L2CA_Config request from the upper 487 * layer protocol. 488 */ 489 490 cmd->ch->dcid = dcid; 491 cmd->ch->state = NG_L2CAP_CONFIG; 492 } else 493 /* There was an error, so close the channel */ 494 NG_L2CAP_INFO( 495"%s: %s - failed to open L2CAP channel, result=%d, status=%d\n", 496 __func__, NG_NODE_NAME(l2cap->node), result, 497 status); 498 499 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 500 result, status); 501 502 /* XXX do we have to remove the channel on error? */ 503 if (error != 0 || result != NG_L2CAP_SUCCESS) 504 ng_l2cap_free_chan(cmd->ch); 505 506 ng_l2cap_free_cmd(cmd); 507 } 508 509 return (error); 510 511reject: 512 /* Send reject. Do not really care about the result */ 513 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 514 515 return (0); 516} /* ng_l2cap_process_con_rsp */ 517 518/* 519 * Process L2CAP_ConfigReq command 520 */ 521 522static int 523ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident) 524{ 525 ng_l2cap_p l2cap = con->l2cap; 526 struct mbuf *m = con->rx_pkt; 527 ng_l2cap_cfg_req_cp *cp = NULL; 528 ng_l2cap_chan_p ch = NULL; 529 u_int16_t dcid, respond, result; 530 ng_l2cap_cfg_opt_t hdr; 531 ng_l2cap_cfg_opt_val_t val; 532 int off, error = 0; 533 534 /* Get command parameters */ 535 con->rx_pkt = NULL; 536 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 537 if (m == NULL) 538 return (ENOBUFS); 539 540 cp = mtod(m, ng_l2cap_cfg_req_cp *); 541 dcid = le16toh(cp->dcid); 542 respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 543 m_adj(m, sizeof(*cp)); 544 545 /* Check if we have this channel and it is in valid state */ 546 ch = ng_l2cap_chan_by_scid(l2cap, dcid); 547 if (ch == NULL) { 548 NG_L2CAP_ERR( 549"%s: %s - unexpected L2CAP_ConfigReq command. " \ 550"Channel does not exist, cid=%d\n", 551 __func__, NG_NODE_NAME(l2cap->node), dcid); 552 goto reject; 553 } 554 555 /* Verify channel state */ 556 if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) { 557 NG_L2CAP_ERR( 558"%s: %s - unexpected L2CAP_ConfigReq. " \ 559"Invalid channel state, cid=%d, state=%d\n", 560 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 561 goto reject; 562 } 563 564 if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */ 565 ch->cfg_state = 0; 566 ch->state = NG_L2CAP_CONFIG; 567 } 568 569 for (result = 0, off = 0; ; ) { 570 error = get_next_l2cap_opt(m, &off, &hdr, &val); 571 if (error == 0) { /* We done with this packet */ 572 NG_FREE_M(m); 573 break; 574 } else if (error > 0) { /* Got option */ 575 switch (hdr.type) { 576 case NG_L2CAP_OPT_MTU: 577 ch->omtu = val.mtu; 578 break; 579 580 case NG_L2CAP_OPT_FLUSH_TIMO: 581 ch->flush_timo = val.flush_timo; 582 break; 583 584 case NG_L2CAP_OPT_QOS: 585 bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow)); 586 break; 587 588 default: 589 KASSERT(0, 590("%s: %s - unknown option: %d\n", __func__, NG_NODE_NAME(l2cap->node), 591 hdr.type)); 592 break; 593 } 594 } else { /* Oops, something is wrong */ 595 respond = 1; 596 597 if (error == -3) { 598 599 /* 600 * Adjust mbuf so we can get to the start 601 * of the first option we did not like. 602 */ 603 604 m_adj(m, off - sizeof(hdr)); 605 m->m_pkthdr.len = sizeof(hdr) + hdr.length; 606 607 result = NG_L2CAP_UNKNOWN_OPTION; 608 } else { 609 /* XXX FIXME Send other reject codes? */ 610 NG_FREE_M(m); 611 result = NG_L2CAP_REJECT; 612 } 613 614 break; 615 } 616 } 617 618 /* 619 * Now check and see if we have to respond. If everything was OK then 620 * respond contain "C flag" and (if set) we will respond with empty 621 * packet and will wait for more options. 622 * 623 * Other case is that we did not like peer's options and will respond 624 * with L2CAP_Config response command with Reject error code. 625 * 626 * When "respond == 0" than we have received all options and we will 627 * sent L2CA_ConfigInd event to the upper layer protocol. 628 */ 629 630 if (respond) { 631 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m); 632 if (error != 0) { 633 ng_l2cap_l2ca_discon_ind(ch); 634 ng_l2cap_free_chan(ch); 635 } 636 } else { 637 /* Send L2CA_ConfigInd event to the upper layer protocol */ 638 ch->ident = ident; 639 error = ng_l2cap_l2ca_cfg_ind(ch); 640 if (error != 0) 641 ng_l2cap_free_chan(ch); 642 } 643 644 return (error); 645 646reject: 647 /* Send reject. Do not really care about the result */ 648 NG_FREE_M(m); 649 650 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid); 651 652 return (0); 653} /* ng_l2cap_process_cfg_req */ 654 655/* 656 * Process L2CAP_ConfigRsp command 657 */ 658 659static int 660ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident) 661{ 662 ng_l2cap_p l2cap = con->l2cap; 663 struct mbuf *m = con->rx_pkt; 664 ng_l2cap_cfg_rsp_cp *cp = NULL; 665 ng_l2cap_cmd_p cmd = NULL; 666 u_int16_t scid, cflag, result; 667 ng_l2cap_cfg_opt_t hdr; 668 ng_l2cap_cfg_opt_val_t val; 669 int off, error = 0; 670 671 /* Get command parameters */ 672 con->rx_pkt = NULL; 673 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 674 if (m == NULL) 675 return (ENOBUFS); 676 677 cp = mtod(m, ng_l2cap_cfg_rsp_cp *); 678 scid = le16toh(cp->scid); 679 cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 680 result = le16toh(cp->result); 681 m_adj(m, sizeof(*cp)); 682 683 /* Check if we have this command */ 684 cmd = ng_l2cap_cmd_by_ident(con, ident); 685 if (cmd == NULL) { 686 NG_L2CAP_ERR( 687"%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", 688 __func__, NG_NODE_NAME(l2cap->node), ident, 689 con->con_handle); 690 NG_FREE_M(m); 691 692 return (ENOENT); 693 } 694 695 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 696("%s: %s - invalid command state, flags=%#x\n", 697 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 698 699 /* Verify CIDs and send reject if does not match */ 700 if (cmd->ch->scid != scid) { 701 NG_L2CAP_ERR( 702"%s: %s - unexpected L2CAP_ConfigRsp. " \ 703"Channel ID does not match, scid=%d(%d)\n", 704 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 705 scid); 706 goto reject; 707 } 708 709 /* Verify channel state and reject if invalid */ 710 if (cmd->ch->state != NG_L2CAP_CONFIG) { 711 NG_L2CAP_ERR( 712"%s: %s - unexpected L2CAP_ConfigRsp. " \ 713"Invalid channel state, scid=%d, state=%d\n", 714 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 715 cmd->ch->state); 716 goto reject; 717 } 718 719 /* 720 * Looks like it is our response, so process it. First parse options, 721 * then verify C flag. If it is set then we shall expect more 722 * configuration options from the peer and we will wait. Otherwise we 723 * have received all options and we will send L2CA_ConfigRsp event to 724 * the upper layer protocol. 725 */ 726 727 ng_l2cap_command_untimeout(cmd); 728 729 for (off = 0; ; ) { 730 error = get_next_l2cap_opt(m, &off, &hdr, &val); 731 if (error == 0) /* We done with this packet */ 732 break; 733 else if (error > 0) { /* Got option */ 734 switch (hdr.type) { 735 case NG_L2CAP_OPT_MTU: 736 cmd->ch->imtu = val.mtu; 737 break; 738 739 case NG_L2CAP_OPT_FLUSH_TIMO: 740 cmd->ch->flush_timo = val.flush_timo; 741 break; 742 743 case NG_L2CAP_OPT_QOS: 744 bcopy(&val.flow, &cmd->ch->oflow, 745 sizeof(cmd->ch->oflow)); 746 break; 747 748 default: 749 KASSERT(0, 750("%s: %s - unknown option: %d\n", __func__, NG_NODE_NAME(l2cap->node), 751 hdr.type)); 752 break; 753 } 754 } else { 755 /* 756 * XXX FIXME What to do here? 757 * 758 * This is really BAD :( options packet was broken, 759 * so let upper layer know and do not wait for more 760 * options 761 */ 762 763 NG_L2CAP_ALERT( 764"%s: %s - failed to parse configuration options, error=%d\n", 765 __func__, NG_NODE_NAME(l2cap->node), error); 766 767 result = NG_L2CAP_UNKNOWN; 768 cflag = 0; 769 770 break; 771 } 772 } 773 774 NG_FREE_M(m); 775 776 if (cflag) /* Restart timer and wait for more options */ 777 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout()); 778 else { 779 ng_l2cap_unlink_cmd(cmd); 780 781 /* Send L2CA_Config response to the upper layer protocol */ 782 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result); 783 if (error != 0) { 784 /* 785 * XXX FIXME what to do here? we were not able to send 786 * response to the upper layer protocol, so for now 787 * just close the channel. Send L2CAP_Disconnect to 788 * remote peer? 789 */ 790 791 NG_L2CAP_ERR( 792"%s: %s - failed to send L2CA_Config response, error=%d\n", 793 __func__, NG_NODE_NAME(l2cap->node), error); 794 795 ng_l2cap_free_chan(cmd->ch); 796 } 797 798 ng_l2cap_free_cmd(cmd); 799 } 800 801 return (error); 802 803reject: 804 /* Send reject. Do not really care about the result */ 805 NG_FREE_M(m); 806 807 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0); 808 809 return (0); 810} /* ng_l2cap_process_cfg_rsp */ 811 812/* 813 * Process L2CAP_DisconnectReq command 814 */ 815 816static int 817ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident) 818{ 819 ng_l2cap_p l2cap = con->l2cap; 820 ng_l2cap_discon_req_cp *cp = NULL; 821 ng_l2cap_chan_p ch = NULL; 822 ng_l2cap_cmd_p cmd = NULL; 823 u_int16_t scid, dcid; 824 825 /* Get command parameters */ 826 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 827 if (con->rx_pkt == NULL) 828 return (ENOBUFS); 829 830 cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *); 831 dcid = le16toh(cp->dcid); 832 scid = le16toh(cp->scid); 833 834 NG_FREE_M(con->rx_pkt); 835 836 /* Check if we have this channel and it is in valid state */ 837 ch = ng_l2cap_chan_by_scid(l2cap, dcid); 838 if (ch == NULL) { 839 NG_L2CAP_ERR( 840"%s: %s - unexpected L2CAP_DisconnectReq message. " \ 841"Channel does not exist, cid=%d\n", 842 __func__, NG_NODE_NAME(l2cap->node), dcid); 843 goto reject; 844 } 845 846 /* XXX Verify channel state and reject if invalid -- is that true? */ 847 if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) { 848 NG_L2CAP_ERR( 849"%s: %s - unexpected L2CAP_DisconnectReq. " \ 850"Invalid channel state, cid=%d, state=%d\n", 851 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 852 goto reject; 853 } 854 855 /* Match destination channel ID */ 856 if (ch->dcid != scid || ch->scid != dcid) { 857 NG_L2CAP_ERR( 858"%s: %s - unexpected L2CAP_DisconnectReq. " \ 859"Channel IDs does not match, channel: scid=%d, dcid=%d, " \ 860"request: scid=%d, dcid=%d\n", 861 __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid, 862 scid, dcid); 863 goto reject; 864 } 865 866 /* 867 * Looks good, so notify upper layer protocol that channel is about 868 * to be disconnected and send L2CA_DisconnectInd message. Then respond 869 * with L2CAP_DisconnectRsp. 870 */ 871 872 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */ 873 ng_l2cap_free_chan(ch); 874 875 /* Send L2CAP_DisconnectRsp */ 876 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0); 877 if (cmd == NULL) 878 return (ENOMEM); 879 880 _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid); 881 if (cmd->aux == NULL) { 882 ng_l2cap_free_cmd(cmd); 883 884 return (ENOBUFS); 885 } 886 887 /* Link command to the queue */ 888 ng_l2cap_link_cmd(con, cmd); 889 ng_l2cap_lp_deliver(con); 890 891 return (0); 892 893reject: 894 /* Send reject. Do not really care about the result */ 895 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 896 897 return (0); 898} /* ng_l2cap_process_discon_req */ 899 900/* 901 * Process L2CAP_DisconnectRsp command 902 */ 903 904static int 905ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident) 906{ 907 ng_l2cap_p l2cap = con->l2cap; 908 ng_l2cap_discon_rsp_cp *cp = NULL; 909 ng_l2cap_cmd_p cmd = NULL; 910 u_int16_t scid, dcid; 911 int error = 0; 912 913 /* Get command parameters */ 914 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 915 if (con->rx_pkt == NULL) 916 return (ENOBUFS); 917 918 cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *); 919 dcid = le16toh(cp->dcid); 920 scid = le16toh(cp->scid); 921 922 NG_FREE_M(con->rx_pkt); 923 924 /* Check if we have pending command descriptor */ 925 cmd = ng_l2cap_cmd_by_ident(con, ident); 926 if (cmd == NULL) { 927 NG_L2CAP_ERR( 928"%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", 929 __func__, NG_NODE_NAME(l2cap->node), ident, 930 con->con_handle); 931 goto out; 932 } 933 934 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 935("%s: %s - invalid command state, flags=%#x\n", 936 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 937 938 /* Verify channel state, do nothing if invalid */ 939 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 940 NG_L2CAP_ERR( 941"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 942"Invalid channel state, cid=%d, state=%d\n", 943 __func__, NG_NODE_NAME(l2cap->node), scid, 944 cmd->ch->state); 945 goto out; 946 } 947 948 /* Verify CIDs and send reject if does not match */ 949 if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) { 950 NG_L2CAP_ERR( 951"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 952"Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n", 953 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 954 scid, cmd->ch->dcid, dcid); 955 goto out; 956 } 957 958 /* 959 * Looks like we have successfuly disconnected channel, 960 * so notify upper layer. 961 */ 962 963 ng_l2cap_command_untimeout(cmd); 964 error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS); 965 ng_l2cap_free_chan(cmd->ch); /* this will free commands too */ 966out: 967 return (error); 968} /* ng_l2cap_process_discon_rsp */ 969 970/* 971 * Process L2CAP_EchoReq command 972 */ 973 974static int 975ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident) 976{ 977 ng_l2cap_p l2cap = con->l2cap; 978 ng_l2cap_cmd_hdr_t *hdr = NULL; 979 ng_l2cap_cmd_p cmd = NULL; 980 981 con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr)); 982 if (con->rx_pkt == NULL) { 983 NG_L2CAP_ALERT( 984"%s: %s - ng_l2cap_prepend() failed, size=%d\n", 985 __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr)); 986 987 return (ENOBUFS); 988 } 989 990 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 991 hdr->code = NG_L2CAP_ECHO_RSP; 992 hdr->ident = ident; 993 hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 994 995 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0); 996 if (cmd == NULL) { 997 NG_FREE_M(con->rx_pkt); 998 999 return (ENOBUFS); 1000 } 1001 1002 /* Attach data and link command to the queue */ 1003 cmd->aux = con->rx_pkt; 1004 con->rx_pkt = NULL; 1005 ng_l2cap_link_cmd(con, cmd); 1006 ng_l2cap_lp_deliver(con); 1007 1008 return (0); 1009} /* ng_l2cap_process_echo_req */ 1010 1011/* 1012 * Process L2CAP_EchoRsp command 1013 */ 1014 1015static int 1016ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident) 1017{ 1018 ng_l2cap_p l2cap = con->l2cap; 1019 ng_l2cap_cmd_p cmd = NULL; 1020 int error = 0; 1021 1022 /* Check if we have this command */ 1023 cmd = ng_l2cap_cmd_by_ident(con, ident); 1024 if (cmd != NULL) { 1025 KASSERT((cmd->con == con), 1026("%s: %s - invalid connection pointer!\n", 1027 __func__, NG_NODE_NAME(l2cap->node))); 1028 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 1029("%s: %s - invalid command state, flags=%#x\n", 1030 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 1031 1032 ng_l2cap_command_untimeout(cmd); 1033 ng_l2cap_unlink_cmd(cmd); 1034 1035 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 1036 NG_L2CAP_SUCCESS, con->rx_pkt); 1037 1038 ng_l2cap_free_cmd(cmd); 1039 con->rx_pkt = NULL; 1040 } else { 1041 NG_L2CAP_ERR( 1042"%s: %s - unexpected L2CAP_EchoRsp command. " \ 1043"Requested ident does not exist, ident=%d\n", 1044 __func__, NG_NODE_NAME(l2cap->node), ident); 1045 NG_FREE_M(con->rx_pkt); 1046 } 1047 1048 return (error); 1049} /* ng_l2cap_process_echo_rsp */ 1050 1051/* 1052 * Process L2CAP_InfoReq command 1053 */ 1054 1055static int 1056ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident) 1057{ 1058 ng_l2cap_p l2cap = con->l2cap; 1059 ng_l2cap_cmd_p cmd = NULL; 1060 u_int16_t type; 1061 1062 /* Get command parameters */ 1063 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp)); 1064 if (con->rx_pkt == NULL) 1065 return (ENOBUFS); 1066 1067 type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type); 1068 NG_FREE_M(con->rx_pkt); 1069 1070 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0); 1071 if (cmd == NULL) 1072 return (ENOMEM); 1073 1074 switch (type) { 1075 case NG_L2CAP_CONNLESS_MTU: 1076 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU, 1077 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT); 1078 break; 1079 1080 default: 1081 _ng_l2cap_info_rsp(cmd->aux, ident, type, 1082 NG_L2CAP_NOT_SUPPORTED, 0); 1083 break; 1084 } 1085 1086 if (cmd->aux == NULL) { 1087 ng_l2cap_free_cmd(cmd); 1088 1089 return (ENOBUFS); 1090 } 1091 1092 /* Link command to the queue */ 1093 ng_l2cap_link_cmd(con, cmd); 1094 ng_l2cap_lp_deliver(con); 1095 1096 return (0); 1097} /* ng_l2cap_process_info_req */ 1098 1099/* 1100 * Process L2CAP_InfoRsp command 1101 */ 1102 1103static int 1104ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident) 1105{ 1106 ng_l2cap_p l2cap = con->l2cap; 1107 ng_l2cap_info_rsp_cp *cp = NULL; 1108 ng_l2cap_cmd_p cmd = NULL; 1109 int error = 0; 1110 1111 /* Get command parameters */ 1112 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1113 if (con->rx_pkt == NULL) 1114 return (ENOBUFS); 1115 1116 cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *); 1117 cp->type = le16toh(cp->type); 1118 cp->result = le16toh(cp->result); 1119 m_adj(con->rx_pkt, sizeof(*cp)); 1120 1121 /* Check if we have pending command descriptor */ 1122 cmd = ng_l2cap_cmd_by_ident(con, ident); 1123 if (cmd == NULL) { 1124 NG_L2CAP_ERR( 1125"%s: %s - unexpected L2CAP_InfoRsp command. " \ 1126"Requested ident does not exist, ident=%d\n", 1127 __func__, NG_NODE_NAME(l2cap->node), ident); 1128 NG_FREE_M(con->rx_pkt); 1129 1130 return (ENOENT); 1131 } 1132 1133 KASSERT((cmd->con == con), 1134("%s: %s - invalid connection pointer!\n", 1135 __func__, NG_NODE_NAME(l2cap->node))); 1136 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 1137("%s: %s - invalid command state, flags=%#x\n", 1138 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 1139 1140 ng_l2cap_command_untimeout(cmd); 1141 ng_l2cap_unlink_cmd(cmd); 1142 1143 if (cp->result == NG_L2CAP_SUCCESS) { 1144 switch (cp->type) { 1145 case NG_L2CAP_CONNLESS_MTU: 1146 if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t)) 1147 *mtod(con->rx_pkt, u_int16_t *) = 1148 le16toh(*mtod(con->rx_pkt,u_int16_t *)); 1149 else { 1150 cp->result = NG_L2CAP_UNKNOWN; /* XXX */ 1151 1152 NG_L2CAP_ERR( 1153"%s: %s - invalid L2CAP_InfoRsp command. " \ 1154"Bad connectionless MTU parameter, len=%d\n", 1155 __func__, NG_NODE_NAME(l2cap->node), 1156 con->rx_pkt->m_pkthdr.len); 1157 } 1158 break; 1159 1160 default: 1161 NG_L2CAP_WARN( 1162"%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n", 1163 __func__, NG_NODE_NAME(l2cap->node), cp->type); 1164 break; 1165 } 1166 } 1167 1168 error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 1169 cp->result, con->rx_pkt); 1170 1171 ng_l2cap_free_cmd(cmd); 1172 con->rx_pkt = NULL; 1173 1174 return (error); 1175} /* ng_l2cap_process_info_rsp */ 1176 1177/* 1178 * Send L2CAP reject 1179 */ 1180 1181static int 1182send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason, 1183 u_int16_t mtu, u_int16_t scid, u_int16_t dcid) 1184{ 1185 ng_l2cap_cmd_p cmd = NULL; 1186 1187 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0); 1188 if (cmd == NULL) 1189 return (ENOMEM); 1190 1191 _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid); 1192 if (cmd->aux == NULL) { 1193 ng_l2cap_free_cmd(cmd); 1194 1195 return (ENOBUFS); 1196 } 1197 1198 /* Link command to the queue */ 1199 ng_l2cap_link_cmd(con, cmd); 1200 ng_l2cap_lp_deliver(con); 1201 1202 return (0); 1203} /* send_l2cap_reject */ 1204 1205/* 1206 * Send L2CAP connection reject 1207 */ 1208 1209static int 1210send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1211 u_int16_t dcid, u_int16_t result) 1212{ 1213 ng_l2cap_cmd_p cmd = NULL; 1214 1215 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0); 1216 if (cmd == NULL) 1217 return (ENOMEM); 1218 1219 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0); 1220 if (cmd->aux == NULL) { 1221 ng_l2cap_free_cmd(cmd); 1222 1223 return (ENOBUFS); 1224 } 1225 1226 /* Link command to the queue */ 1227 ng_l2cap_link_cmd(con, cmd); 1228 ng_l2cap_lp_deliver(con); 1229 1230 return (0); 1231} /* send_l2cap_con_rej */ 1232 1233/* 1234 * Send L2CAP config response 1235 */ 1236 1237static int 1238send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1239 u_int16_t result, struct mbuf *opt) 1240{ 1241 ng_l2cap_cmd_p cmd = NULL; 1242 1243 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0); 1244 if (cmd == NULL) { 1245 NG_FREE_M(opt); 1246 1247 return (ENOMEM); 1248 } 1249 1250 _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt); 1251 if (cmd->aux == NULL) { 1252 ng_l2cap_free_cmd(cmd); 1253 1254 return (ENOBUFS); 1255 } 1256 1257 /* Link command to the queue */ 1258 ng_l2cap_link_cmd(con, cmd); 1259 ng_l2cap_lp_deliver(con); 1260 1261 return (0); 1262} /* send_l2cap_cfg_rsp */ 1263 1264/* 1265 * Get next L2CAP configuration option 1266 * 1267 * Return codes: 1268 * 0 no option 1269 * 1 we have got option 1270 * -1 header too short 1271 * -2 bad option value or length 1272 * -3 unknown option 1273 */ 1274 1275static int 1276get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr, 1277 ng_l2cap_cfg_opt_val_p val) 1278{ 1279 int hint, len = m->m_pkthdr.len - (*off); 1280 1281 if (len == 0) 1282 return (0); 1283 if (len < 0 || len < sizeof(*hdr)) 1284 return (-1); 1285 1286 m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr); 1287 *off += sizeof(*hdr); 1288 len -= sizeof(*hdr); 1289 1290 hint = NG_L2CAP_OPT_HINT(hdr->type); 1291 hdr->type &= NG_L2CAP_OPT_HINT_MASK; 1292 1293 switch (hdr->type) { 1294 case NG_L2CAP_OPT_MTU: 1295 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length) 1296 return (-2); 1297 1298 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val); 1299 val->mtu = le16toh(val->mtu); 1300 *off += NG_L2CAP_OPT_MTU_SIZE; 1301 break; 1302 1303 case NG_L2CAP_OPT_FLUSH_TIMO: 1304 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE || 1305 len < hdr->length) 1306 return (-2); 1307 1308 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val); 1309 val->flush_timo = le16toh(val->flush_timo); 1310 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE; 1311 break; 1312 1313 case NG_L2CAP_OPT_QOS: 1314 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length) 1315 return (-2); 1316 1317 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val); 1318 val->flow.token_rate = le32toh(val->flow.token_rate); 1319 val->flow.token_bucket_size = 1320 le32toh(val->flow.token_bucket_size); 1321 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); 1322 val->flow.latency = le32toh(val->flow.latency); 1323 val->flow.delay_variation = le32toh(val->flow.delay_variation); 1324 *off += NG_L2CAP_OPT_QOS_SIZE; 1325 break; 1326 1327 default: 1328 if (hint) 1329 *off += hdr->length; 1330 else 1331 return (-3); 1332 break; 1333 } 1334 1335 return (1); 1336} /* get_next_l2cap_opt */ 1337
|