ng_l2cap_evnt.c revision 114878
1/* 2 * ng_l2cap_evnt.c 3 * 4 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com> 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26 * SUCH DAMAGE. 27 * 28 * $Id: ng_l2cap_evnt.c,v 1.4 2003/04/01 18:15:26 max Exp $ 29 * $FreeBSD: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c 114878 2003-05-10 21:44:42Z julian $ 30 */ 31 32#include <sys/param.h> 33#include <sys/systm.h> 34#include <sys/kernel.h> 35#include <sys/endian.h> 36#include <sys/malloc.h> 37#include <sys/mbuf.h> 38#include <sys/queue.h> 39#include <netgraph/ng_message.h> 40#include <netgraph/netgraph.h> 41#include "ng_bluetooth.h" 42#include "ng_hci.h" 43#include "ng_l2cap.h" 44#include "ng_l2cap_var.h" 45#include "ng_l2cap_cmds.h" 46#include "ng_l2cap_evnt.h" 47#include "ng_l2cap_llpi.h" 48#include "ng_l2cap_ulpi.h" 49#include "ng_l2cap_misc.h" 50 51/****************************************************************************** 52 ****************************************************************************** 53 ** L2CAP events processing module 54 ****************************************************************************** 55 ******************************************************************************/ 56 57static int ng_l2cap_process_signal_cmd (ng_l2cap_con_p); 58static int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t); 59static int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t); 60static int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t); 61static int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t); 62static int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p, u_int8_t); 63static int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t); 64static int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t); 65static int ng_l2cap_process_echo_req (ng_l2cap_con_p, u_int8_t); 66static int ng_l2cap_process_echo_rsp (ng_l2cap_con_p, u_int8_t); 67static int ng_l2cap_process_info_req (ng_l2cap_con_p, u_int8_t); 68static int ng_l2cap_process_info_rsp (ng_l2cap_con_p, u_int8_t); 69static int send_l2cap_reject 70 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t); 71static int send_l2cap_con_rej 72 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t); 73static int send_l2cap_cfg_rsp 74 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *); 75static int get_next_l2cap_opt 76 (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p); 77 78/* 79 * Receive L2CAP packet. First get L2CAP header and verify packet. Than 80 * get destination channel and process packet. 81 */ 82 83int 84ng_l2cap_receive(ng_l2cap_con_p con) 85{ 86 ng_l2cap_p l2cap = con->l2cap; 87 ng_l2cap_hdr_t *hdr = NULL; 88 int error = 0; 89 90 /* Check packet */ 91 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 92 NG_L2CAP_ERR( 93"%s: %s - invalid L2CAP packet. Packet too small, len=%d\n", 94 __func__, NG_NODE_NAME(l2cap->node), 95 con->rx_pkt->m_pkthdr.len); 96 error = EMSGSIZE; 97 goto drop; 98 } 99 100 /* Get L2CAP header */ 101 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 102 if (con->rx_pkt == NULL) 103 return (ENOBUFS); 104 105 hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *); 106 hdr->length = le16toh(hdr->length); 107 hdr->dcid = le16toh(hdr->dcid); 108 109 /* Check payload size */ 110 if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) { 111 NG_L2CAP_ERR( 112"%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%d\n", 113 __func__, NG_NODE_NAME(l2cap->node), hdr->length, 114 con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 115 error = EMSGSIZE; 116 goto drop; 117 } 118 119 /* Process packet */ 120 switch (hdr->dcid) { 121 case NG_L2CAP_SIGNAL_CID: /* L2CAP command */ 122 m_adj(con->rx_pkt, sizeof(*hdr)); 123 error = ng_l2cap_process_signal_cmd(con); 124 break; 125 126 case NG_L2CAP_CLT_CID: /* Connectionless packet */ 127 error = ng_l2cap_l2ca_clt_receive(con); 128 break; 129 130 default: /* Data packet */ 131 error = ng_l2cap_l2ca_receive(con); 132 break; 133 } 134 135 return (error); 136drop: 137 NG_FREE_M(con->rx_pkt); 138 139 return (error); 140} /* ng_l2cap_receive */ 141 142/* 143 * Process L2CAP signaling command. We already know that destination channel ID 144 * is 0x1 that means we have received signaling command from peer's L2CAP layer. 145 * So get command header, decode and process it. 146 * 147 * XXX do we need to check signaling MTU here? 148 */ 149 150static int 151ng_l2cap_process_signal_cmd(ng_l2cap_con_p con) 152{ 153 ng_l2cap_p l2cap = con->l2cap; 154 ng_l2cap_cmd_hdr_t *hdr = NULL; 155 struct mbuf *m = NULL; 156 157 while (con->rx_pkt != NULL) { 158 /* Verify packet length */ 159 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 160 NG_L2CAP_ERR( 161"%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n", 162 __func__, NG_NODE_NAME(l2cap->node), 163 con->rx_pkt->m_pkthdr.len); 164 NG_FREE_M(con->rx_pkt); 165 166 return (EMSGSIZE); 167 } 168 169 /* Get signaling command */ 170 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 171 if (con->rx_pkt == NULL) 172 return (ENOBUFS); 173 174 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 175 hdr->length = le16toh(hdr->length); 176 m_adj(con->rx_pkt, sizeof(*hdr)); 177 178 /* Verify command length */ 179 if (con->rx_pkt->m_pkthdr.len < hdr->length) { 180 NG_L2CAP_ERR( 181"%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \ 182"Invalid command length=%d, m_pkthdr.len=%d\n", 183 __func__, NG_NODE_NAME(l2cap->node), 184 hdr->code, hdr->ident, hdr->length, 185 con->rx_pkt->m_pkthdr.len); 186 NG_FREE_M(con->rx_pkt); 187 188 return (EMSGSIZE); 189 } 190 191 /* Get the command, save the rest (if any) */ 192 if (con->rx_pkt->m_pkthdr.len > hdr->length) 193 m = m_split(con->rx_pkt, hdr->length, M_DONTWAIT); 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 ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 849 NG_L2CAP_ERR( 850"%s: %s - unexpected L2CAP_DisconnectReq. " \ 851"Invalid channel state, cid=%d, state=%d\n", 852 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 853 goto reject; 854 } 855 856 /* Match destination channel ID */ 857 if (ch->dcid != scid || ch->scid != dcid) { 858 NG_L2CAP_ERR( 859"%s: %s - unexpected L2CAP_DisconnectReq. " \ 860"Channel IDs does not match, channel: scid=%d, dcid=%d, " \ 861"request: scid=%d, dcid=%d\n", 862 __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid, 863 scid, dcid); 864 goto reject; 865 } 866 867 /* 868 * Looks good, so notify upper layer protocol that channel is about 869 * to be disconnected and send L2CA_DisconnectInd message. Then respond 870 * with L2CAP_DisconnectRsp. 871 */ 872 873 if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 874 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */ 875 ng_l2cap_free_chan(ch); 876 } 877 878 /* Send L2CAP_DisconnectRsp */ 879 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0); 880 if (cmd == NULL) 881 return (ENOMEM); 882 883 _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid); 884 if (cmd->aux == NULL) { 885 ng_l2cap_free_cmd(cmd); 886 887 return (ENOBUFS); 888 } 889 890 /* Link command to the queue */ 891 ng_l2cap_link_cmd(con, cmd); 892 ng_l2cap_lp_deliver(con); 893 894 return (0); 895 896reject: 897 /* Send reject. Do not really care about the result */ 898 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 899 900 return (0); 901} /* ng_l2cap_process_discon_req */ 902 903/* 904 * Process L2CAP_DisconnectRsp command 905 */ 906 907static int 908ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident) 909{ 910 ng_l2cap_p l2cap = con->l2cap; 911 ng_l2cap_discon_rsp_cp *cp = NULL; 912 ng_l2cap_cmd_p cmd = NULL; 913 u_int16_t scid, dcid; 914 int error = 0; 915 916 /* Get command parameters */ 917 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 918 if (con->rx_pkt == NULL) 919 return (ENOBUFS); 920 921 cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *); 922 dcid = le16toh(cp->dcid); 923 scid = le16toh(cp->scid); 924 925 NG_FREE_M(con->rx_pkt); 926 927 /* Check if we have pending command descriptor */ 928 cmd = ng_l2cap_cmd_by_ident(con, ident); 929 if (cmd == NULL) { 930 NG_L2CAP_ERR( 931"%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", 932 __func__, NG_NODE_NAME(l2cap->node), ident, 933 con->con_handle); 934 goto out; 935 } 936 937 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 938("%s: %s - invalid command state, flags=%#x\n", 939 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 940 941 /* Verify channel state, do nothing if invalid */ 942 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 943 NG_L2CAP_ERR( 944"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 945"Invalid channel state, cid=%d, state=%d\n", 946 __func__, NG_NODE_NAME(l2cap->node), scid, 947 cmd->ch->state); 948 goto out; 949 } 950 951 /* Verify CIDs and send reject if does not match */ 952 if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) { 953 NG_L2CAP_ERR( 954"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 955"Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n", 956 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 957 scid, cmd->ch->dcid, dcid); 958 goto out; 959 } 960 961 /* 962 * Looks like we have successfuly disconnected channel, 963 * so notify upper layer. 964 */ 965 966 ng_l2cap_command_untimeout(cmd); 967 error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS); 968 ng_l2cap_free_chan(cmd->ch); /* this will free commands too */ 969out: 970 return (error); 971} /* ng_l2cap_process_discon_rsp */ 972 973/* 974 * Process L2CAP_EchoReq command 975 */ 976 977static int 978ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident) 979{ 980 ng_l2cap_p l2cap = con->l2cap; 981 ng_l2cap_cmd_hdr_t *hdr = NULL; 982 ng_l2cap_cmd_p cmd = NULL; 983 984 con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr)); 985 if (con->rx_pkt == NULL) { 986 NG_L2CAP_ALERT( 987"%s: %s - ng_l2cap_prepend() failed, size=%d\n", 988 __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr)); 989 990 return (ENOBUFS); 991 } 992 993 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 994 hdr->code = NG_L2CAP_ECHO_RSP; 995 hdr->ident = ident; 996 hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 997 998 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0); 999 if (cmd == NULL) { 1000 NG_FREE_M(con->rx_pkt); 1001 1002 return (ENOBUFS); 1003 } 1004 1005 /* Attach data and link command to the queue */ 1006 cmd->aux = con->rx_pkt; 1007 con->rx_pkt = NULL; 1008 ng_l2cap_link_cmd(con, cmd); 1009 ng_l2cap_lp_deliver(con); 1010 1011 return (0); 1012} /* ng_l2cap_process_echo_req */ 1013 1014/* 1015 * Process L2CAP_EchoRsp command 1016 */ 1017 1018static int 1019ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident) 1020{ 1021 ng_l2cap_p l2cap = con->l2cap; 1022 ng_l2cap_cmd_p cmd = NULL; 1023 int error = 0; 1024 1025 /* Check if we have this command */ 1026 cmd = ng_l2cap_cmd_by_ident(con, ident); 1027 if (cmd != NULL) { 1028 KASSERT((cmd->con == con), 1029("%s: %s - invalid connection pointer!\n", 1030 __func__, NG_NODE_NAME(l2cap->node))); 1031 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 1032("%s: %s - invalid command state, flags=%#x\n", 1033 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 1034 1035 ng_l2cap_command_untimeout(cmd); 1036 ng_l2cap_unlink_cmd(cmd); 1037 1038 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 1039 NG_L2CAP_SUCCESS, con->rx_pkt); 1040 1041 ng_l2cap_free_cmd(cmd); 1042 con->rx_pkt = NULL; 1043 } else { 1044 NG_L2CAP_ERR( 1045"%s: %s - unexpected L2CAP_EchoRsp command. " \ 1046"Requested ident does not exist, ident=%d\n", 1047 __func__, NG_NODE_NAME(l2cap->node), ident); 1048 NG_FREE_M(con->rx_pkt); 1049 } 1050 1051 return (error); 1052} /* ng_l2cap_process_echo_rsp */ 1053 1054/* 1055 * Process L2CAP_InfoReq command 1056 */ 1057 1058static int 1059ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident) 1060{ 1061 ng_l2cap_p l2cap = con->l2cap; 1062 ng_l2cap_cmd_p cmd = NULL; 1063 u_int16_t type; 1064 1065 /* Get command parameters */ 1066 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp)); 1067 if (con->rx_pkt == NULL) 1068 return (ENOBUFS); 1069 1070 type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type); 1071 NG_FREE_M(con->rx_pkt); 1072 1073 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0); 1074 if (cmd == NULL) 1075 return (ENOMEM); 1076 1077 switch (type) { 1078 case NG_L2CAP_CONNLESS_MTU: 1079 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU, 1080 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT); 1081 break; 1082 1083 default: 1084 _ng_l2cap_info_rsp(cmd->aux, ident, type, 1085 NG_L2CAP_NOT_SUPPORTED, 0); 1086 break; 1087 } 1088 1089 if (cmd->aux == NULL) { 1090 ng_l2cap_free_cmd(cmd); 1091 1092 return (ENOBUFS); 1093 } 1094 1095 /* Link command to the queue */ 1096 ng_l2cap_link_cmd(con, cmd); 1097 ng_l2cap_lp_deliver(con); 1098 1099 return (0); 1100} /* ng_l2cap_process_info_req */ 1101 1102/* 1103 * Process L2CAP_InfoRsp command 1104 */ 1105 1106static int 1107ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident) 1108{ 1109 ng_l2cap_p l2cap = con->l2cap; 1110 ng_l2cap_info_rsp_cp *cp = NULL; 1111 ng_l2cap_cmd_p cmd = NULL; 1112 int error = 0; 1113 1114 /* Get command parameters */ 1115 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1116 if (con->rx_pkt == NULL) 1117 return (ENOBUFS); 1118 1119 cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *); 1120 cp->type = le16toh(cp->type); 1121 cp->result = le16toh(cp->result); 1122 m_adj(con->rx_pkt, sizeof(*cp)); 1123 1124 /* Check if we have pending command descriptor */ 1125 cmd = ng_l2cap_cmd_by_ident(con, ident); 1126 if (cmd == NULL) { 1127 NG_L2CAP_ERR( 1128"%s: %s - unexpected L2CAP_InfoRsp command. " \ 1129"Requested ident does not exist, ident=%d\n", 1130 __func__, NG_NODE_NAME(l2cap->node), ident); 1131 NG_FREE_M(con->rx_pkt); 1132 1133 return (ENOENT); 1134 } 1135 1136 KASSERT((cmd->con == con), 1137("%s: %s - invalid connection pointer!\n", 1138 __func__, NG_NODE_NAME(l2cap->node))); 1139 KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), 1140("%s: %s - invalid command state, flags=%#x\n", 1141 __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); 1142 1143 ng_l2cap_command_untimeout(cmd); 1144 ng_l2cap_unlink_cmd(cmd); 1145 1146 if (cp->result == NG_L2CAP_SUCCESS) { 1147 switch (cp->type) { 1148 case NG_L2CAP_CONNLESS_MTU: 1149 if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t)) 1150 *mtod(con->rx_pkt, u_int16_t *) = 1151 le16toh(*mtod(con->rx_pkt,u_int16_t *)); 1152 else { 1153 cp->result = NG_L2CAP_UNKNOWN; /* XXX */ 1154 1155 NG_L2CAP_ERR( 1156"%s: %s - invalid L2CAP_InfoRsp command. " \ 1157"Bad connectionless MTU parameter, len=%d\n", 1158 __func__, NG_NODE_NAME(l2cap->node), 1159 con->rx_pkt->m_pkthdr.len); 1160 } 1161 break; 1162 1163 default: 1164 NG_L2CAP_WARN( 1165"%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n", 1166 __func__, NG_NODE_NAME(l2cap->node), cp->type); 1167 break; 1168 } 1169 } 1170 1171 error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 1172 cp->result, con->rx_pkt); 1173 1174 ng_l2cap_free_cmd(cmd); 1175 con->rx_pkt = NULL; 1176 1177 return (error); 1178} /* ng_l2cap_process_info_rsp */ 1179 1180/* 1181 * Send L2CAP reject 1182 */ 1183 1184static int 1185send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason, 1186 u_int16_t mtu, u_int16_t scid, u_int16_t dcid) 1187{ 1188 ng_l2cap_cmd_p cmd = NULL; 1189 1190 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0); 1191 if (cmd == NULL) 1192 return (ENOMEM); 1193 1194 _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid); 1195 if (cmd->aux == NULL) { 1196 ng_l2cap_free_cmd(cmd); 1197 1198 return (ENOBUFS); 1199 } 1200 1201 /* Link command to the queue */ 1202 ng_l2cap_link_cmd(con, cmd); 1203 ng_l2cap_lp_deliver(con); 1204 1205 return (0); 1206} /* send_l2cap_reject */ 1207 1208/* 1209 * Send L2CAP connection reject 1210 */ 1211 1212static int 1213send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1214 u_int16_t dcid, u_int16_t result) 1215{ 1216 ng_l2cap_cmd_p cmd = NULL; 1217 1218 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0); 1219 if (cmd == NULL) 1220 return (ENOMEM); 1221 1222 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0); 1223 if (cmd->aux == NULL) { 1224 ng_l2cap_free_cmd(cmd); 1225 1226 return (ENOBUFS); 1227 } 1228 1229 /* Link command to the queue */ 1230 ng_l2cap_link_cmd(con, cmd); 1231 ng_l2cap_lp_deliver(con); 1232 1233 return (0); 1234} /* send_l2cap_con_rej */ 1235 1236/* 1237 * Send L2CAP config response 1238 */ 1239 1240static int 1241send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1242 u_int16_t result, struct mbuf *opt) 1243{ 1244 ng_l2cap_cmd_p cmd = NULL; 1245 1246 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0); 1247 if (cmd == NULL) { 1248 NG_FREE_M(opt); 1249 1250 return (ENOMEM); 1251 } 1252 1253 _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt); 1254 if (cmd->aux == NULL) { 1255 ng_l2cap_free_cmd(cmd); 1256 1257 return (ENOBUFS); 1258 } 1259 1260 /* Link command to the queue */ 1261 ng_l2cap_link_cmd(con, cmd); 1262 ng_l2cap_lp_deliver(con); 1263 1264 return (0); 1265} /* send_l2cap_cfg_rsp */ 1266 1267/* 1268 * Get next L2CAP configuration option 1269 * 1270 * Return codes: 1271 * 0 no option 1272 * 1 we have got option 1273 * -1 header too short 1274 * -2 bad option value or length 1275 * -3 unknown option 1276 */ 1277 1278static int 1279get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr, 1280 ng_l2cap_cfg_opt_val_p val) 1281{ 1282 int hint, len = m->m_pkthdr.len - (*off); 1283 1284 if (len == 0) 1285 return (0); 1286 if (len < 0 || len < sizeof(*hdr)) 1287 return (-1); 1288 1289 m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr); 1290 *off += sizeof(*hdr); 1291 len -= sizeof(*hdr); 1292 1293 hint = NG_L2CAP_OPT_HINT(hdr->type); 1294 hdr->type &= NG_L2CAP_OPT_HINT_MASK; 1295 1296 switch (hdr->type) { 1297 case NG_L2CAP_OPT_MTU: 1298 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length) 1299 return (-2); 1300 1301 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val); 1302 val->mtu = le16toh(val->mtu); 1303 *off += NG_L2CAP_OPT_MTU_SIZE; 1304 break; 1305 1306 case NG_L2CAP_OPT_FLUSH_TIMO: 1307 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE || 1308 len < hdr->length) 1309 return (-2); 1310 1311 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val); 1312 val->flush_timo = le16toh(val->flush_timo); 1313 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE; 1314 break; 1315 1316 case NG_L2CAP_OPT_QOS: 1317 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length) 1318 return (-2); 1319 1320 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val); 1321 val->flow.token_rate = le32toh(val->flow.token_rate); 1322 val->flow.token_bucket_size = 1323 le32toh(val->flow.token_bucket_size); 1324 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); 1325 val->flow.latency = le32toh(val->flow.latency); 1326 val->flow.delay_variation = le32toh(val->flow.delay_variation); 1327 *off += NG_L2CAP_OPT_QOS_SIZE; 1328 break; 1329 1330 default: 1331 if (hint) 1332 *off += hdr->length; 1333 else 1334 return (-3); 1335 break; 1336 } 1337 1338 return (1); 1339} /* get_next_l2cap_opt */ 1340 1341