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