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