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