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