1 /* 2 * ng_l2cap_evnt.c 3 */ 4 5 /*- 6 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com> 7 * All rights reserved. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 1. Redistributions of source code must retain the above copyright 13 * notice, this list of conditions and the following disclaimer. 14 * 2. Redistributions in binary form must reproduce the above copyright 15 * notice, this list of conditions and the following disclaimer in the 16 * documentation and/or other materials provided with the distribution. 17 * 18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 24 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 28 * SUCH DAMAGE. 29 * 30 * $Id: ng_l2cap_evnt.c,v 1.5 2003/09/08 19:11:45 max Exp $ 31 * $FreeBSD: src/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c,v 1.8 2005/01/07 01:45:43 imp Exp $ 32 * $DragonFly: src/sys/netgraph7/bluetooth/l2cap/ng_l2cap_evnt.c,v 1.2 2008/06/26 23:05:40 dillon Exp $ 33 */ 34 35 #include <sys/param.h> 36 #include <sys/systm.h> 37 #include <sys/kernel.h> 38 #include <sys/endian.h> 39 #include <sys/malloc.h> 40 #include <sys/mbuf.h> 41 #include <sys/queue.h> 42 #include "ng_message.h" 43 #include "netgraph.h" 44 #include "bluetooth/include/ng_bluetooth.h" 45 #include "bluetooth/include/ng_hci.h" 46 #include "bluetooth/include/ng_l2cap.h" 47 #include "bluetooth/l2cap/ng_l2cap_var.h" 48 #include "bluetooth/l2cap/ng_l2cap_cmds.h" 49 #include "bluetooth/l2cap/ng_l2cap_evnt.h" 50 #include "bluetooth/l2cap/ng_l2cap_llpi.h" 51 #include "bluetooth/l2cap/ng_l2cap_ulpi.h" 52 #include "bluetooth/l2cap/ng_l2cap_misc.h" 53 54 /****************************************************************************** 55 ****************************************************************************** 56 ** L2CAP events processing module 57 ****************************************************************************** 58 ******************************************************************************/ 59 60 static int ng_l2cap_process_signal_cmd (ng_l2cap_con_p); 61 static int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t); 62 static int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t); 63 static int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t); 64 static int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t); 65 static int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p, u_int8_t); 66 static int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t); 67 static int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t); 68 static int ng_l2cap_process_echo_req (ng_l2cap_con_p, u_int8_t); 69 static int ng_l2cap_process_echo_rsp (ng_l2cap_con_p, u_int8_t); 70 static int ng_l2cap_process_info_req (ng_l2cap_con_p, u_int8_t); 71 static int ng_l2cap_process_info_rsp (ng_l2cap_con_p, u_int8_t); 72 static int send_l2cap_reject 73 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t); 74 static int send_l2cap_con_rej 75 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t); 76 static int send_l2cap_cfg_rsp 77 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *); 78 static int get_next_l2cap_opt 79 (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p); 80 81 /* 82 * Receive L2CAP packet. First get L2CAP header and verify packet. Than 83 * get destination channel and process packet. 84 */ 85 86 int 87 ng_l2cap_receive(ng_l2cap_con_p con) 88 { 89 ng_l2cap_p l2cap = con->l2cap; 90 ng_l2cap_hdr_t *hdr = NULL; 91 int error = 0; 92 93 /* Check packet */ 94 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 95 NG_L2CAP_ERR( 96 "%s: %s - invalid L2CAP packet. Packet too small, len=%d\n", 97 __func__, NG_NODE_NAME(l2cap->node), 98 con->rx_pkt->m_pkthdr.len); 99 error = EMSGSIZE; 100 goto drop; 101 } 102 103 /* Get L2CAP header */ 104 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 105 if (con->rx_pkt == NULL) 106 return (ENOBUFS); 107 108 hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *); 109 hdr->length = le16toh(hdr->length); 110 hdr->dcid = le16toh(hdr->dcid); 111 112 /* Check payload size */ 113 if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) { 114 NG_L2CAP_ERR( 115 "%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%zd\n", 116 __func__, NG_NODE_NAME(l2cap->node), hdr->length, 117 con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 118 error = EMSGSIZE; 119 goto drop; 120 } 121 122 /* Process packet */ 123 switch (hdr->dcid) { 124 case NG_L2CAP_SIGNAL_CID: /* L2CAP command */ 125 m_adj(con->rx_pkt, sizeof(*hdr)); 126 error = ng_l2cap_process_signal_cmd(con); 127 break; 128 129 case NG_L2CAP_CLT_CID: /* Connectionless packet */ 130 error = ng_l2cap_l2ca_clt_receive(con); 131 break; 132 133 default: /* Data packet */ 134 error = ng_l2cap_l2ca_receive(con); 135 break; 136 } 137 138 return (error); 139 drop: 140 NG_FREE_M(con->rx_pkt); 141 142 return (error); 143 } /* ng_l2cap_receive */ 144 145 /* 146 * Process L2CAP signaling command. We already know that destination channel ID 147 * is 0x1 that means we have received signaling command from peer's L2CAP layer. 148 * So get command header, decode and process it. 149 * 150 * XXX do we need to check signaling MTU here? 151 */ 152 153 static int 154 ng_l2cap_process_signal_cmd(ng_l2cap_con_p con) 155 { 156 ng_l2cap_p l2cap = con->l2cap; 157 ng_l2cap_cmd_hdr_t *hdr = NULL; 158 struct mbuf *m = NULL; 159 160 while (con->rx_pkt != NULL) { 161 /* Verify packet length */ 162 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 163 NG_L2CAP_ERR( 164 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n", 165 __func__, NG_NODE_NAME(l2cap->node), 166 con->rx_pkt->m_pkthdr.len); 167 NG_FREE_M(con->rx_pkt); 168 169 return (EMSGSIZE); 170 } 171 172 /* Get signaling command */ 173 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 174 if (con->rx_pkt == NULL) 175 return (ENOBUFS); 176 177 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 178 hdr->length = le16toh(hdr->length); 179 m_adj(con->rx_pkt, sizeof(*hdr)); 180 181 /* Verify command length */ 182 if (con->rx_pkt->m_pkthdr.len < hdr->length) { 183 NG_L2CAP_ERR( 184 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \ 185 "Invalid command length=%d, m_pkthdr.len=%d\n", 186 __func__, NG_NODE_NAME(l2cap->node), 187 hdr->code, hdr->ident, hdr->length, 188 con->rx_pkt->m_pkthdr.len); 189 NG_FREE_M(con->rx_pkt); 190 191 return (EMSGSIZE); 192 } 193 194 /* Get the command, save the rest (if any) */ 195 if (con->rx_pkt->m_pkthdr.len > hdr->length) 196 m = m_split(con->rx_pkt, hdr->length, MB_DONTWAIT); 197 else 198 m = NULL; 199 200 /* Process command */ 201 switch (hdr->code) { 202 case NG_L2CAP_CMD_REJ: 203 ng_l2cap_process_cmd_rej(con, hdr->ident); 204 break; 205 206 case NG_L2CAP_CON_REQ: 207 ng_l2cap_process_con_req(con, hdr->ident); 208 break; 209 210 case NG_L2CAP_CON_RSP: 211 ng_l2cap_process_con_rsp(con, hdr->ident); 212 break; 213 214 case NG_L2CAP_CFG_REQ: 215 ng_l2cap_process_cfg_req(con, hdr->ident); 216 break; 217 218 case NG_L2CAP_CFG_RSP: 219 ng_l2cap_process_cfg_rsp(con, hdr->ident); 220 break; 221 222 case NG_L2CAP_DISCON_REQ: 223 ng_l2cap_process_discon_req(con, hdr->ident); 224 break; 225 226 case NG_L2CAP_DISCON_RSP: 227 ng_l2cap_process_discon_rsp(con, hdr->ident); 228 break; 229 230 case NG_L2CAP_ECHO_REQ: 231 ng_l2cap_process_echo_req(con, hdr->ident); 232 break; 233 234 case NG_L2CAP_ECHO_RSP: 235 ng_l2cap_process_echo_rsp(con, hdr->ident); 236 break; 237 238 case NG_L2CAP_INFO_REQ: 239 ng_l2cap_process_info_req(con, hdr->ident); 240 break; 241 242 case NG_L2CAP_INFO_RSP: 243 ng_l2cap_process_info_rsp(con, hdr->ident); 244 break; 245 246 default: 247 NG_L2CAP_ERR( 248 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", 249 __func__, NG_NODE_NAME(l2cap->node), 250 hdr->code, hdr->ident, hdr->length); 251 252 /* 253 * Send L2CAP_CommandRej. Do not really care 254 * about the result 255 */ 256 257 send_l2cap_reject(con, hdr->ident, 258 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); 259 NG_FREE_M(con->rx_pkt); 260 break; 261 } 262 263 con->rx_pkt = m; 264 } 265 266 return (0); 267 } /* ng_l2cap_process_signal_cmd */ 268 269 /* 270 * Process L2CAP_CommandRej command 271 */ 272 273 static int 274 ng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident) 275 { 276 ng_l2cap_p l2cap = con->l2cap; 277 ng_l2cap_cmd_rej_cp *cp = NULL; 278 ng_l2cap_cmd_p cmd = NULL; 279 280 /* Get command parameters */ 281 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 282 if (con->rx_pkt == NULL) 283 return (ENOBUFS); 284 285 cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *); 286 cp->reason = le16toh(cp->reason); 287 288 /* Check if we have pending command descriptor */ 289 cmd = ng_l2cap_cmd_by_ident(con, ident); 290 if (cmd != NULL) { 291 /* If command timeout already happened then ignore reject */ 292 if (ng_l2cap_command_untimeout(cmd) != 0) { 293 NG_FREE_M(con->rx_pkt); 294 return (ETIMEDOUT); 295 } 296 297 ng_l2cap_unlink_cmd(cmd); 298 299 switch (cmd->code) { 300 case NG_L2CAP_CON_REQ: 301 ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0); 302 ng_l2cap_free_chan(cmd->ch); 303 break; 304 305 case NG_L2CAP_CFG_REQ: 306 ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason); 307 break; 308 309 case NG_L2CAP_DISCON_REQ: 310 ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason); 311 ng_l2cap_free_chan(cmd->ch); /* XXX free channel */ 312 break; 313 314 case NG_L2CAP_ECHO_REQ: 315 ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 316 cp->reason, NULL); 317 break; 318 319 case NG_L2CAP_INFO_REQ: 320 ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 321 cp->reason, NULL); 322 break; 323 324 default: 325 NG_L2CAP_ALERT( 326 "%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n", 327 __func__, NG_NODE_NAME(l2cap->node), cmd->code); 328 break; 329 } 330 331 ng_l2cap_free_cmd(cmd); 332 } else 333 NG_L2CAP_ERR( 334 "%s: %s - unexpected L2CAP_CommandRej command. " \ 335 "Requested ident does not exist, ident=%d\n", 336 __func__, NG_NODE_NAME(l2cap->node), ident); 337 338 NG_FREE_M(con->rx_pkt); 339 340 return (0); 341 } /* ng_l2cap_process_cmd_rej */ 342 343 /* 344 * Process L2CAP_ConnectReq command 345 */ 346 347 static int 348 ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident) 349 { 350 ng_l2cap_p l2cap = con->l2cap; 351 struct mbuf *m = con->rx_pkt; 352 ng_l2cap_con_req_cp *cp = NULL; 353 ng_l2cap_chan_p ch = NULL; 354 int error = 0; 355 u_int16_t dcid, psm; 356 357 /* Get command parameters */ 358 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 359 if (m == NULL) 360 return (ENOBUFS); 361 362 cp = mtod(m, ng_l2cap_con_req_cp *); 363 psm = le16toh(cp->psm); 364 dcid = le16toh(cp->scid); 365 366 NG_FREE_M(m); 367 con->rx_pkt = NULL; 368 369 /* 370 * Create new channel and send L2CA_ConnectInd notification 371 * to the upper layer protocol. 372 */ 373 374 ch = ng_l2cap_new_chan(l2cap, con, psm); 375 if (ch == NULL) 376 return (send_l2cap_con_rej(con, ident, 0, dcid, 377 NG_L2CAP_NO_RESOURCES)); 378 379 /* Update channel IDs */ 380 ch->dcid = dcid; 381 382 /* Sent L2CA_ConnectInd notification to the upper layer */ 383 ch->ident = ident; 384 ch->state = NG_L2CAP_W4_L2CA_CON_RSP; 385 386 error = ng_l2cap_l2ca_con_ind(ch); 387 if (error != 0) { 388 send_l2cap_con_rej(con, ident, ch->scid, dcid, 389 (error == ENOMEM)? NG_L2CAP_NO_RESOURCES : 390 NG_L2CAP_PSM_NOT_SUPPORTED); 391 ng_l2cap_free_chan(ch); 392 } 393 394 return (error); 395 } /* ng_l2cap_process_con_req */ 396 397 /* 398 * Process L2CAP_ConnectRsp command 399 */ 400 401 static int 402 ng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident) 403 { 404 ng_l2cap_p l2cap = con->l2cap; 405 struct mbuf *m = con->rx_pkt; 406 ng_l2cap_con_rsp_cp *cp = NULL; 407 ng_l2cap_cmd_p cmd = NULL; 408 u_int16_t scid, dcid, result, status; 409 int error = 0; 410 411 /* Get command parameters */ 412 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 413 if (m == NULL) 414 return (ENOBUFS); 415 416 cp = mtod(m, ng_l2cap_con_rsp_cp *); 417 dcid = le16toh(cp->dcid); 418 scid = le16toh(cp->scid); 419 result = le16toh(cp->result); 420 status = le16toh(cp->status); 421 422 NG_FREE_M(m); 423 con->rx_pkt = NULL; 424 425 /* Check if we have pending command descriptor */ 426 cmd = ng_l2cap_cmd_by_ident(con, ident); 427 if (cmd == NULL) { 428 NG_L2CAP_ERR( 429 "%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n", 430 __func__, NG_NODE_NAME(l2cap->node), ident, 431 con->con_handle); 432 433 return (ENOENT); 434 } 435 436 /* Verify channel state, if invalid - do nothing */ 437 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) { 438 NG_L2CAP_ERR( 439 "%s: %s - unexpected L2CAP_ConnectRsp. " \ 440 "Invalid channel state, cid=%d, state=%d\n", 441 __func__, NG_NODE_NAME(l2cap->node), scid, 442 cmd->ch->state); 443 goto reject; 444 } 445 446 /* Verify CIDs and send reject if does not match */ 447 if (cmd->ch->scid != scid) { 448 NG_L2CAP_ERR( 449 "%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n", 450 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 451 scid); 452 goto reject; 453 } 454 455 /* 456 * Looks good. We got confirmation from our peer. Now process 457 * it. First disable RTX timer. Then check the result and send 458 * notification to the upper layer. If command timeout already 459 * happened then ignore response. 460 */ 461 462 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) 463 return (error); 464 465 if (result == NG_L2CAP_PENDING) { 466 /* 467 * Our peer wants more time to complete connection. We shall 468 * start ERTX timer and wait. Keep command in the list. 469 */ 470 471 cmd->ch->dcid = dcid; 472 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout()); 473 474 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 475 result, status); 476 if (error != 0) 477 ng_l2cap_free_chan(cmd->ch); 478 } else { 479 ng_l2cap_unlink_cmd(cmd); 480 481 if (result == NG_L2CAP_SUCCESS) { 482 /* 483 * Channel is open. Complete command and move to CONFIG 484 * state. Since we have sent positive confirmation we 485 * expect to receive L2CA_Config request from the upper 486 * layer protocol. 487 */ 488 489 cmd->ch->dcid = dcid; 490 cmd->ch->state = NG_L2CAP_CONFIG; 491 } else 492 /* There was an error, so close the channel */ 493 NG_L2CAP_INFO( 494 "%s: %s - failed to open L2CAP channel, result=%d, status=%d\n", 495 __func__, NG_NODE_NAME(l2cap->node), result, 496 status); 497 498 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 499 result, status); 500 501 /* XXX do we have to remove the channel on error? */ 502 if (error != 0 || result != NG_L2CAP_SUCCESS) 503 ng_l2cap_free_chan(cmd->ch); 504 505 ng_l2cap_free_cmd(cmd); 506 } 507 508 return (error); 509 510 reject: 511 /* Send reject. Do not really care about the result */ 512 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 513 514 return (0); 515 } /* ng_l2cap_process_con_rsp */ 516 517 /* 518 * Process L2CAP_ConfigReq command 519 */ 520 521 static int 522 ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident) 523 { 524 ng_l2cap_p l2cap = con->l2cap; 525 struct mbuf *m = con->rx_pkt; 526 ng_l2cap_cfg_req_cp *cp = NULL; 527 ng_l2cap_chan_p ch = NULL; 528 u_int16_t dcid, respond, result; 529 ng_l2cap_cfg_opt_t hdr; 530 ng_l2cap_cfg_opt_val_t val; 531 int off, error = 0; 532 533 /* Get command parameters */ 534 con->rx_pkt = NULL; 535 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 536 if (m == NULL) 537 return (ENOBUFS); 538 539 cp = mtod(m, ng_l2cap_cfg_req_cp *); 540 dcid = le16toh(cp->dcid); 541 respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 542 m_adj(m, sizeof(*cp)); 543 544 /* Check if we have this channel and it is in valid state */ 545 ch = ng_l2cap_chan_by_scid(l2cap, dcid); 546 if (ch == NULL) { 547 NG_L2CAP_ERR( 548 "%s: %s - unexpected L2CAP_ConfigReq command. " \ 549 "Channel does not exist, cid=%d\n", 550 __func__, NG_NODE_NAME(l2cap->node), dcid); 551 goto reject; 552 } 553 554 /* Verify channel state */ 555 if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) { 556 NG_L2CAP_ERR( 557 "%s: %s - unexpected L2CAP_ConfigReq. " \ 558 "Invalid channel state, cid=%d, state=%d\n", 559 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 560 goto reject; 561 } 562 563 if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */ 564 ch->cfg_state = 0; 565 ch->state = NG_L2CAP_CONFIG; 566 } 567 568 for (result = 0, off = 0; ; ) { 569 error = get_next_l2cap_opt(m, &off, &hdr, &val); 570 if (error == 0) { /* We done with this packet */ 571 NG_FREE_M(m); 572 break; 573 } else if (error > 0) { /* Got option */ 574 switch (hdr.type) { 575 case NG_L2CAP_OPT_MTU: 576 ch->omtu = val.mtu; 577 break; 578 579 case NG_L2CAP_OPT_FLUSH_TIMO: 580 ch->flush_timo = val.flush_timo; 581 break; 582 583 case NG_L2CAP_OPT_QOS: 584 bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow)); 585 break; 586 587 default: /* Ignore unknown hint option */ 588 break; 589 } 590 } else { /* Oops, something is wrong */ 591 respond = 1; 592 593 if (error == -3) { 594 595 /* 596 * Adjust mbuf so we can get to the start 597 * of the first option we did not like. 598 */ 599 600 m_adj(m, off - sizeof(hdr)); 601 m->m_pkthdr.len = sizeof(hdr) + hdr.length; 602 603 result = NG_L2CAP_UNKNOWN_OPTION; 604 } else { 605 /* XXX FIXME Send other reject codes? */ 606 NG_FREE_M(m); 607 result = NG_L2CAP_REJECT; 608 } 609 610 break; 611 } 612 } 613 614 /* 615 * Now check and see if we have to respond. If everything was OK then 616 * respond contain "C flag" and (if set) we will respond with empty 617 * packet and will wait for more options. 618 * 619 * Other case is that we did not like peer's options and will respond 620 * with L2CAP_Config response command with Reject error code. 621 * 622 * When "respond == 0" than we have received all options and we will 623 * sent L2CA_ConfigInd event to the upper layer protocol. 624 */ 625 626 if (respond) { 627 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m); 628 if (error != 0) { 629 ng_l2cap_l2ca_discon_ind(ch); 630 ng_l2cap_free_chan(ch); 631 } 632 } else { 633 /* Send L2CA_ConfigInd event to the upper layer protocol */ 634 ch->ident = ident; 635 error = ng_l2cap_l2ca_cfg_ind(ch); 636 if (error != 0) 637 ng_l2cap_free_chan(ch); 638 } 639 640 return (error); 641 642 reject: 643 /* Send reject. Do not really care about the result */ 644 NG_FREE_M(m); 645 646 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid); 647 648 return (0); 649 } /* ng_l2cap_process_cfg_req */ 650 651 /* 652 * Process L2CAP_ConfigRsp command 653 */ 654 655 static int 656 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident) 657 { 658 ng_l2cap_p l2cap = con->l2cap; 659 struct mbuf *m = con->rx_pkt; 660 ng_l2cap_cfg_rsp_cp *cp = NULL; 661 ng_l2cap_cmd_p cmd = NULL; 662 u_int16_t scid, cflag, result; 663 ng_l2cap_cfg_opt_t hdr; 664 ng_l2cap_cfg_opt_val_t val; 665 int off, error = 0; 666 667 /* Get command parameters */ 668 con->rx_pkt = NULL; 669 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 670 if (m == NULL) 671 return (ENOBUFS); 672 673 cp = mtod(m, ng_l2cap_cfg_rsp_cp *); 674 scid = le16toh(cp->scid); 675 cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 676 result = le16toh(cp->result); 677 m_adj(m, sizeof(*cp)); 678 679 /* Check if we have this command */ 680 cmd = ng_l2cap_cmd_by_ident(con, ident); 681 if (cmd == NULL) { 682 NG_L2CAP_ERR( 683 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", 684 __func__, NG_NODE_NAME(l2cap->node), ident, 685 con->con_handle); 686 NG_FREE_M(m); 687 688 return (ENOENT); 689 } 690 691 /* Verify CIDs and send reject if does not match */ 692 if (cmd->ch->scid != scid) { 693 NG_L2CAP_ERR( 694 "%s: %s - unexpected L2CAP_ConfigRsp. " \ 695 "Channel ID does not match, scid=%d(%d)\n", 696 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 697 scid); 698 goto reject; 699 } 700 701 /* Verify channel state and reject if invalid */ 702 if (cmd->ch->state != NG_L2CAP_CONFIG) { 703 NG_L2CAP_ERR( 704 "%s: %s - unexpected L2CAP_ConfigRsp. " \ 705 "Invalid channel state, scid=%d, state=%d\n", 706 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 707 cmd->ch->state); 708 goto reject; 709 } 710 711 /* 712 * Looks like it is our response, so process it. First parse options, 713 * then verify C flag. If it is set then we shall expect more 714 * configuration options from the peer and we will wait. Otherwise we 715 * have received all options and we will send L2CA_ConfigRsp event to 716 * the upper layer protocol. If command timeout already happened then 717 * ignore response. 718 */ 719 720 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 721 NG_FREE_M(m); 722 return (error); 723 } 724 725 for (off = 0; ; ) { 726 error = get_next_l2cap_opt(m, &off, &hdr, &val); 727 if (error == 0) /* We done with this packet */ 728 break; 729 else if (error > 0) { /* Got option */ 730 switch (hdr.type) { 731 case NG_L2CAP_OPT_MTU: 732 cmd->ch->imtu = val.mtu; 733 break; 734 735 case NG_L2CAP_OPT_FLUSH_TIMO: 736 cmd->ch->flush_timo = val.flush_timo; 737 break; 738 739 case NG_L2CAP_OPT_QOS: 740 bcopy(&val.flow, &cmd->ch->oflow, 741 sizeof(cmd->ch->oflow)); 742 break; 743 744 default: /* Ignore unknown hint option */ 745 break; 746 } 747 } else { 748 /* 749 * XXX FIXME What to do here? 750 * 751 * This is really BAD :( options packet was broken, or 752 * peer sent us option that we did not understand. Let 753 * upper layer know and do not wait for more options. 754 */ 755 756 NG_L2CAP_ALERT( 757 "%s: %s - failed to parse configuration options, error=%d\n", 758 __func__, NG_NODE_NAME(l2cap->node), error); 759 760 result = NG_L2CAP_UNKNOWN; 761 cflag = 0; 762 763 break; 764 } 765 } 766 767 NG_FREE_M(m); 768 769 if (cflag) /* Restart timer and wait for more options */ 770 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout()); 771 else { 772 ng_l2cap_unlink_cmd(cmd); 773 774 /* Send L2CA_Config response to the upper layer protocol */ 775 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result); 776 if (error != 0) { 777 /* 778 * XXX FIXME what to do here? we were not able to send 779 * response to the upper layer protocol, so for now 780 * just close the channel. Send L2CAP_Disconnect to 781 * remote peer? 782 */ 783 784 NG_L2CAP_ERR( 785 "%s: %s - failed to send L2CA_Config response, error=%d\n", 786 __func__, NG_NODE_NAME(l2cap->node), error); 787 788 ng_l2cap_free_chan(cmd->ch); 789 } 790 791 ng_l2cap_free_cmd(cmd); 792 } 793 794 return (error); 795 796 reject: 797 /* Send reject. Do not really care about the result */ 798 NG_FREE_M(m); 799 800 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0); 801 802 return (0); 803 } /* ng_l2cap_process_cfg_rsp */ 804 805 /* 806 * Process L2CAP_DisconnectReq command 807 */ 808 809 static int 810 ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident) 811 { 812 ng_l2cap_p l2cap = con->l2cap; 813 ng_l2cap_discon_req_cp *cp = NULL; 814 ng_l2cap_chan_p ch = NULL; 815 ng_l2cap_cmd_p cmd = NULL; 816 u_int16_t scid, dcid; 817 818 /* Get command parameters */ 819 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 820 if (con->rx_pkt == NULL) 821 return (ENOBUFS); 822 823 cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *); 824 dcid = le16toh(cp->dcid); 825 scid = le16toh(cp->scid); 826 827 NG_FREE_M(con->rx_pkt); 828 829 /* Check if we have this channel and it is in valid state */ 830 ch = ng_l2cap_chan_by_scid(l2cap, dcid); 831 if (ch == NULL) { 832 NG_L2CAP_ERR( 833 "%s: %s - unexpected L2CAP_DisconnectReq message. " \ 834 "Channel does not exist, cid=%d\n", 835 __func__, NG_NODE_NAME(l2cap->node), dcid); 836 goto reject; 837 } 838 839 /* XXX Verify channel state and reject if invalid -- is that true? */ 840 if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG && 841 ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 842 NG_L2CAP_ERR( 843 "%s: %s - unexpected L2CAP_DisconnectReq. " \ 844 "Invalid channel state, cid=%d, state=%d\n", 845 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 846 goto reject; 847 } 848 849 /* Match destination channel ID */ 850 if (ch->dcid != scid || ch->scid != dcid) { 851 NG_L2CAP_ERR( 852 "%s: %s - unexpected L2CAP_DisconnectReq. " \ 853 "Channel IDs does not match, channel: scid=%d, dcid=%d, " \ 854 "request: scid=%d, dcid=%d\n", 855 __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid, 856 scid, dcid); 857 goto reject; 858 } 859 860 /* 861 * Looks good, so notify upper layer protocol that channel is about 862 * to be disconnected and send L2CA_DisconnectInd message. Then respond 863 * with L2CAP_DisconnectRsp. 864 */ 865 866 if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 867 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */ 868 ng_l2cap_free_chan(ch); 869 } 870 871 /* Send L2CAP_DisconnectRsp */ 872 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0); 873 if (cmd == NULL) 874 return (ENOMEM); 875 876 _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid); 877 if (cmd->aux == NULL) { 878 ng_l2cap_free_cmd(cmd); 879 880 return (ENOBUFS); 881 } 882 883 /* Link command to the queue */ 884 ng_l2cap_link_cmd(con, cmd); 885 ng_l2cap_lp_deliver(con); 886 887 return (0); 888 889 reject: 890 /* Send reject. Do not really care about the result */ 891 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 892 893 return (0); 894 } /* ng_l2cap_process_discon_req */ 895 896 /* 897 * Process L2CAP_DisconnectRsp command 898 */ 899 900 static int 901 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident) 902 { 903 ng_l2cap_p l2cap = con->l2cap; 904 ng_l2cap_discon_rsp_cp *cp = NULL; 905 ng_l2cap_cmd_p cmd = NULL; 906 u_int16_t scid, dcid; 907 int error = 0; 908 909 /* Get command parameters */ 910 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 911 if (con->rx_pkt == NULL) 912 return (ENOBUFS); 913 914 cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *); 915 dcid = le16toh(cp->dcid); 916 scid = le16toh(cp->scid); 917 918 NG_FREE_M(con->rx_pkt); 919 920 /* Check if we have pending command descriptor */ 921 cmd = ng_l2cap_cmd_by_ident(con, ident); 922 if (cmd == NULL) { 923 NG_L2CAP_ERR( 924 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", 925 __func__, NG_NODE_NAME(l2cap->node), ident, 926 con->con_handle); 927 goto out; 928 } 929 930 /* Verify channel state, do nothing if invalid */ 931 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 932 NG_L2CAP_ERR( 933 "%s: %s - unexpected L2CAP_DisconnectRsp. " \ 934 "Invalid channel state, cid=%d, state=%d\n", 935 __func__, NG_NODE_NAME(l2cap->node), scid, 936 cmd->ch->state); 937 goto out; 938 } 939 940 /* Verify CIDs and send reject if does not match */ 941 if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) { 942 NG_L2CAP_ERR( 943 "%s: %s - unexpected L2CAP_DisconnectRsp. " \ 944 "Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n", 945 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 946 scid, cmd->ch->dcid, dcid); 947 goto out; 948 } 949 950 /* 951 * Looks like we have successfuly disconnected channel, so notify 952 * upper layer. If command timeout already happened then ignore 953 * response. 954 */ 955 956 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) 957 goto out; 958 959 error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS); 960 ng_l2cap_free_chan(cmd->ch); /* this will free commands too */ 961 out: 962 return (error); 963 } /* ng_l2cap_process_discon_rsp */ 964 965 /* 966 * Process L2CAP_EchoReq command 967 */ 968 969 static int 970 ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident) 971 { 972 ng_l2cap_p l2cap = con->l2cap; 973 ng_l2cap_cmd_hdr_t *hdr = NULL; 974 ng_l2cap_cmd_p cmd = NULL; 975 976 con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr)); 977 if (con->rx_pkt == NULL) { 978 NG_L2CAP_ALERT( 979 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n", 980 __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr)); 981 982 return (ENOBUFS); 983 } 984 985 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 986 hdr->code = NG_L2CAP_ECHO_RSP; 987 hdr->ident = ident; 988 hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 989 990 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0); 991 if (cmd == NULL) { 992 NG_FREE_M(con->rx_pkt); 993 994 return (ENOBUFS); 995 } 996 997 /* Attach data and link command to the queue */ 998 cmd->aux = con->rx_pkt; 999 con->rx_pkt = NULL; 1000 ng_l2cap_link_cmd(con, cmd); 1001 ng_l2cap_lp_deliver(con); 1002 1003 return (0); 1004 } /* ng_l2cap_process_echo_req */ 1005 1006 /* 1007 * Process L2CAP_EchoRsp command 1008 */ 1009 1010 static int 1011 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident) 1012 { 1013 ng_l2cap_p l2cap = con->l2cap; 1014 ng_l2cap_cmd_p cmd = NULL; 1015 int error = 0; 1016 1017 /* Check if we have this command */ 1018 cmd = ng_l2cap_cmd_by_ident(con, ident); 1019 if (cmd != NULL) { 1020 /* If command timeout already happened then ignore response */ 1021 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1022 NG_FREE_M(con->rx_pkt); 1023 return (error); 1024 } 1025 1026 ng_l2cap_unlink_cmd(cmd); 1027 1028 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 1029 NG_L2CAP_SUCCESS, con->rx_pkt); 1030 1031 ng_l2cap_free_cmd(cmd); 1032 con->rx_pkt = NULL; 1033 } else { 1034 NG_L2CAP_ERR( 1035 "%s: %s - unexpected L2CAP_EchoRsp command. " \ 1036 "Requested ident does not exist, ident=%d\n", 1037 __func__, NG_NODE_NAME(l2cap->node), ident); 1038 NG_FREE_M(con->rx_pkt); 1039 } 1040 1041 return (error); 1042 } /* ng_l2cap_process_echo_rsp */ 1043 1044 /* 1045 * Process L2CAP_InfoReq command 1046 */ 1047 1048 static int 1049 ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident) 1050 { 1051 ng_l2cap_p l2cap = con->l2cap; 1052 ng_l2cap_cmd_p cmd = NULL; 1053 u_int16_t type; 1054 1055 /* Get command parameters */ 1056 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp)); 1057 if (con->rx_pkt == NULL) 1058 return (ENOBUFS); 1059 1060 type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type); 1061 NG_FREE_M(con->rx_pkt); 1062 1063 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0); 1064 if (cmd == NULL) 1065 return (ENOMEM); 1066 1067 switch (type) { 1068 case NG_L2CAP_CONNLESS_MTU: 1069 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU, 1070 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT); 1071 break; 1072 1073 default: 1074 _ng_l2cap_info_rsp(cmd->aux, ident, type, 1075 NG_L2CAP_NOT_SUPPORTED, 0); 1076 break; 1077 } 1078 1079 if (cmd->aux == NULL) { 1080 ng_l2cap_free_cmd(cmd); 1081 1082 return (ENOBUFS); 1083 } 1084 1085 /* Link command to the queue */ 1086 ng_l2cap_link_cmd(con, cmd); 1087 ng_l2cap_lp_deliver(con); 1088 1089 return (0); 1090 } /* ng_l2cap_process_info_req */ 1091 1092 /* 1093 * Process L2CAP_InfoRsp command 1094 */ 1095 1096 static int 1097 ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident) 1098 { 1099 ng_l2cap_p l2cap = con->l2cap; 1100 ng_l2cap_info_rsp_cp *cp = NULL; 1101 ng_l2cap_cmd_p cmd = NULL; 1102 int error = 0; 1103 1104 /* Get command parameters */ 1105 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1106 if (con->rx_pkt == NULL) 1107 return (ENOBUFS); 1108 1109 cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *); 1110 cp->type = le16toh(cp->type); 1111 cp->result = le16toh(cp->result); 1112 m_adj(con->rx_pkt, sizeof(*cp)); 1113 1114 /* Check if we have pending command descriptor */ 1115 cmd = ng_l2cap_cmd_by_ident(con, ident); 1116 if (cmd == NULL) { 1117 NG_L2CAP_ERR( 1118 "%s: %s - unexpected L2CAP_InfoRsp command. " \ 1119 "Requested ident does not exist, ident=%d\n", 1120 __func__, NG_NODE_NAME(l2cap->node), ident); 1121 NG_FREE_M(con->rx_pkt); 1122 1123 return (ENOENT); 1124 } 1125 1126 /* If command timeout already happened then ignore response */ 1127 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1128 NG_FREE_M(con->rx_pkt); 1129 return (error); 1130 } 1131 1132 ng_l2cap_unlink_cmd(cmd); 1133 1134 if (cp->result == NG_L2CAP_SUCCESS) { 1135 switch (cp->type) { 1136 case NG_L2CAP_CONNLESS_MTU: 1137 if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t)) 1138 *mtod(con->rx_pkt, u_int16_t *) = 1139 le16toh(*mtod(con->rx_pkt,u_int16_t *)); 1140 else { 1141 cp->result = NG_L2CAP_UNKNOWN; /* XXX */ 1142 1143 NG_L2CAP_ERR( 1144 "%s: %s - invalid L2CAP_InfoRsp command. " \ 1145 "Bad connectionless MTU parameter, len=%d\n", 1146 __func__, NG_NODE_NAME(l2cap->node), 1147 con->rx_pkt->m_pkthdr.len); 1148 } 1149 break; 1150 1151 default: 1152 NG_L2CAP_WARN( 1153 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n", 1154 __func__, NG_NODE_NAME(l2cap->node), cp->type); 1155 break; 1156 } 1157 } 1158 1159 error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 1160 cp->result, con->rx_pkt); 1161 1162 ng_l2cap_free_cmd(cmd); 1163 con->rx_pkt = NULL; 1164 1165 return (error); 1166 } /* ng_l2cap_process_info_rsp */ 1167 1168 /* 1169 * Send L2CAP reject 1170 */ 1171 1172 static int 1173 send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason, 1174 u_int16_t mtu, u_int16_t scid, u_int16_t dcid) 1175 { 1176 ng_l2cap_cmd_p cmd = NULL; 1177 1178 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0); 1179 if (cmd == NULL) 1180 return (ENOMEM); 1181 1182 _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid); 1183 if (cmd->aux == NULL) { 1184 ng_l2cap_free_cmd(cmd); 1185 1186 return (ENOBUFS); 1187 } 1188 1189 /* Link command to the queue */ 1190 ng_l2cap_link_cmd(con, cmd); 1191 ng_l2cap_lp_deliver(con); 1192 1193 return (0); 1194 } /* send_l2cap_reject */ 1195 1196 /* 1197 * Send L2CAP connection reject 1198 */ 1199 1200 static int 1201 send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1202 u_int16_t dcid, u_int16_t result) 1203 { 1204 ng_l2cap_cmd_p cmd = NULL; 1205 1206 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0); 1207 if (cmd == NULL) 1208 return (ENOMEM); 1209 1210 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0); 1211 if (cmd->aux == NULL) { 1212 ng_l2cap_free_cmd(cmd); 1213 1214 return (ENOBUFS); 1215 } 1216 1217 /* Link command to the queue */ 1218 ng_l2cap_link_cmd(con, cmd); 1219 ng_l2cap_lp_deliver(con); 1220 1221 return (0); 1222 } /* send_l2cap_con_rej */ 1223 1224 /* 1225 * Send L2CAP config response 1226 */ 1227 1228 static int 1229 send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1230 u_int16_t result, struct mbuf *opt) 1231 { 1232 ng_l2cap_cmd_p cmd = NULL; 1233 1234 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0); 1235 if (cmd == NULL) { 1236 NG_FREE_M(opt); 1237 1238 return (ENOMEM); 1239 } 1240 1241 _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt); 1242 if (cmd->aux == NULL) { 1243 ng_l2cap_free_cmd(cmd); 1244 1245 return (ENOBUFS); 1246 } 1247 1248 /* Link command to the queue */ 1249 ng_l2cap_link_cmd(con, cmd); 1250 ng_l2cap_lp_deliver(con); 1251 1252 return (0); 1253 } /* send_l2cap_cfg_rsp */ 1254 1255 /* 1256 * Get next L2CAP configuration option 1257 * 1258 * Return codes: 1259 * 0 no option 1260 * 1 we have got option 1261 * -1 header too short 1262 * -2 bad option value or length 1263 * -3 unknown option 1264 */ 1265 1266 static int 1267 get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr, 1268 ng_l2cap_cfg_opt_val_p val) 1269 { 1270 int hint, len = m->m_pkthdr.len - (*off); 1271 1272 if (len == 0) 1273 return (0); 1274 if (len < 0 || len < sizeof(*hdr)) 1275 return (-1); 1276 1277 m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr); 1278 *off += sizeof(*hdr); 1279 len -= sizeof(*hdr); 1280 1281 hint = NG_L2CAP_OPT_HINT(hdr->type); 1282 hdr->type &= NG_L2CAP_OPT_HINT_MASK; 1283 1284 switch (hdr->type) { 1285 case NG_L2CAP_OPT_MTU: 1286 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length) 1287 return (-2); 1288 1289 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val); 1290 val->mtu = le16toh(val->mtu); 1291 *off += NG_L2CAP_OPT_MTU_SIZE; 1292 break; 1293 1294 case NG_L2CAP_OPT_FLUSH_TIMO: 1295 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE || 1296 len < hdr->length) 1297 return (-2); 1298 1299 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val); 1300 val->flush_timo = le16toh(val->flush_timo); 1301 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE; 1302 break; 1303 1304 case NG_L2CAP_OPT_QOS: 1305 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length) 1306 return (-2); 1307 1308 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val); 1309 val->flow.token_rate = le32toh(val->flow.token_rate); 1310 val->flow.token_bucket_size = 1311 le32toh(val->flow.token_bucket_size); 1312 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); 1313 val->flow.latency = le32toh(val->flow.latency); 1314 val->flow.delay_variation = le32toh(val->flow.delay_variation); 1315 *off += NG_L2CAP_OPT_QOS_SIZE; 1316 break; 1317 1318 default: 1319 if (hint) 1320 *off += hdr->length; 1321 else 1322 return (-3); 1323 break; 1324 } 1325 1326 return (1); 1327 } /* get_next_l2cap_opt */ 1328 1329