1 /* 2 * ng_l2cap_ulpi.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_ulpi.c,v 1.1 2002/11/24 19:47:06 max Exp $ 31 * $FreeBSD$ 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_hci.h> 44 #include <netgraph/bluetooth/include/ng_l2cap.h> 45 #include <netgraph/bluetooth/l2cap/ng_l2cap_var.h> 46 #include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h> 47 #include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h> 48 #include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h> 49 #include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h> 50 #include <netgraph/bluetooth/l2cap/ng_l2cap_misc.h> 51 52 /****************************************************************************** 53 ****************************************************************************** 54 ** Upper Layer Protocol Interface module 55 ****************************************************************************** 56 ******************************************************************************/ 57 58 /* 59 * Process L2CA_Connect request from the upper layer protocol. 60 */ 61 62 int 63 ng_l2cap_l2ca_con_req(ng_l2cap_p l2cap, struct ng_mesg *msg) 64 { 65 ng_l2cap_l2ca_con_ip *ip = NULL; 66 ng_l2cap_con_p con = NULL; 67 ng_l2cap_chan_p ch = NULL; 68 ng_l2cap_cmd_p cmd = NULL; 69 int error = 0; 70 71 /* Check message */ 72 if (msg->header.arglen != sizeof(*ip)) { 73 NG_L2CAP_ALERT( 74 "%s: %s - invalid L2CA_Connect request message size, size=%d\n", 75 __func__, NG_NODE_NAME(l2cap->node), 76 msg->header.arglen); 77 error = EMSGSIZE; 78 goto out; 79 } 80 81 ip = (ng_l2cap_l2ca_con_ip *)(msg->data); 82 83 /* Check if we have connection to the remote unit */ 84 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); 85 if (con == NULL) { 86 /* Submit LP_ConnectReq to the lower layer */ 87 error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr); 88 if (error != 0) { 89 NG_L2CAP_ERR( 90 "%s: %s - unable to send LP_ConnectReq message, error=%d\n", 91 __func__, NG_NODE_NAME(l2cap->node), error); 92 goto out; 93 } 94 95 /* This should not fail */ 96 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); 97 KASSERT((con != NULL), 98 ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node))); 99 } 100 101 /* 102 * Create new empty channel descriptor. In case of any failure do 103 * not touch connection descriptor. 104 */ 105 106 ch = ng_l2cap_new_chan(l2cap, con, ip->psm); 107 if (ch == NULL) { 108 error = ENOMEM; 109 goto out; 110 } 111 112 /* Now create L2CAP_ConnectReq command */ 113 cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(con), 114 NG_L2CAP_CON_REQ, msg->header.token); 115 if (cmd == NULL) { 116 ng_l2cap_free_chan(ch); 117 error = ENOMEM; 118 goto out; 119 } 120 121 if (cmd->ident == NG_L2CAP_NULL_IDENT) { 122 ng_l2cap_free_cmd(cmd); 123 ng_l2cap_free_chan(ch); 124 error = EIO; 125 goto out; 126 } 127 128 /* Create L2CAP command packet */ 129 _ng_l2cap_con_req(cmd->aux, cmd->ident, ch->psm, ch->scid); 130 if (cmd->aux == NULL) { 131 ng_l2cap_free_cmd(cmd); 132 ng_l2cap_free_chan(ch); 133 error = ENOBUFS; 134 goto out; 135 } 136 137 ch->state = NG_L2CAP_W4_L2CAP_CON_RSP; 138 139 /* Link command to the queue */ 140 ng_l2cap_link_cmd(ch->con, cmd); 141 ng_l2cap_lp_deliver(ch->con); 142 out: 143 return (error); 144 } /* ng_l2cap_l2ca_con_req */ 145 146 /* 147 * Send L2CA_Connect response to the upper layer protocol. 148 */ 149 150 int 151 ng_l2cap_l2ca_con_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result, 152 u_int16_t status) 153 { 154 ng_l2cap_p l2cap = ch->con->l2cap; 155 struct ng_mesg *msg = NULL; 156 ng_l2cap_l2ca_con_op *op = NULL; 157 int error = 0; 158 159 /* Check if upstream hook is connected and valid */ 160 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 161 NG_L2CAP_ERR( 162 "%s: %s - unable to send L2CA_Connect response message. " \ 163 "Hook is not connected or valid, psm=%d\n", 164 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 165 166 return (ENOTCONN); 167 } 168 169 /* Create and send L2CA_Connect response message */ 170 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON, 171 sizeof(*op), M_NOWAIT); 172 if (msg == NULL) 173 error = ENOMEM; 174 else { 175 msg->header.token = token; 176 msg->header.flags |= NGF_RESP; 177 178 op = (ng_l2cap_l2ca_con_op *)(msg->data); 179 180 /* 181 * XXX Spec. says we should only populate LCID when result == 0 182 * What about PENDING? What the heck, for now always populate 183 * LCID :) 184 */ 185 186 op->lcid = ch->scid; 187 op->result = result; 188 op->status = status; 189 190 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 191 } 192 193 return (error); 194 } /* ng_l2cap_l2ca_con_rsp */ 195 196 /* 197 * Process L2CA_ConnectRsp request from the upper layer protocol. 198 */ 199 200 int 201 ng_l2cap_l2ca_con_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg) 202 { 203 ng_l2cap_l2ca_con_rsp_ip *ip = NULL; 204 ng_l2cap_con_p con = NULL; 205 ng_l2cap_chan_p ch = NULL; 206 ng_l2cap_cmd_p cmd = NULL; 207 u_int16_t dcid; 208 int error = 0; 209 210 /* Check message */ 211 if (msg->header.arglen != sizeof(*ip)) { 212 NG_L2CAP_ALERT( 213 "%s: %s - invalid L2CA_ConnectRsp request message size, size=%d\n", 214 __func__, NG_NODE_NAME(l2cap->node), 215 msg->header.arglen); 216 error = EMSGSIZE; 217 goto out; 218 } 219 220 ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data); 221 222 /* Check if we have this channel */ 223 ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); 224 if (ch == NULL) { 225 NG_L2CAP_ALERT( 226 "%s: %s - unexpected L2CA_ConnectRsp request message. " \ 227 "Channel does not exist, lcid=%d\n", 228 __func__, NG_NODE_NAME(l2cap->node), ip->lcid); 229 error = ENOENT; 230 goto out; 231 } 232 233 /* Check channel state */ 234 if (ch->state != NG_L2CAP_W4_L2CA_CON_RSP) { 235 NG_L2CAP_ERR( 236 "%s: %s - unexpected L2CA_ConnectRsp request message. " \ 237 "Invalid channel state, state=%d, lcid=%d\n", 238 __func__, NG_NODE_NAME(l2cap->node), ch->state, 239 ip->lcid); 240 error = EINVAL; 241 goto out; 242 } 243 244 dcid = ch->dcid; 245 con = ch->con; 246 247 /* 248 * Now we are pretty much sure it is our response. So create and send 249 * L2CAP_ConnectRsp message to our peer. 250 */ 251 252 if (ch->ident != ip->ident) 253 NG_L2CAP_WARN( 254 "%s: %s - channel ident and response ident do not match, scid=%d, ident=%d. " \ 255 "Will use response ident=%d\n", 256 __func__, NG_NODE_NAME(l2cap->node), ch->scid, 257 ch->ident, ip->ident); 258 259 /* Check result */ 260 switch (ip->result) { 261 case NG_L2CAP_SUCCESS: 262 ch->state = NG_L2CAP_CONFIG; 263 ch->cfg_state = 0; 264 break; 265 266 case NG_L2CAP_PENDING: 267 break; 268 269 default: 270 ng_l2cap_free_chan(ch); 271 ch = NULL; 272 break; 273 } 274 275 /* Create L2CAP command */ 276 cmd = ng_l2cap_new_cmd(con, ch, ip->ident, NG_L2CAP_CON_RSP, 277 msg->header.token); 278 if (cmd == NULL) { 279 if (ch != NULL) 280 ng_l2cap_free_chan(ch); 281 282 error = ENOMEM; 283 goto out; 284 } 285 286 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, ip->lcid, dcid, 287 ip->result, ip->status); 288 if (cmd->aux == NULL) { 289 if (ch != NULL) 290 ng_l2cap_free_chan(ch); 291 292 ng_l2cap_free_cmd(cmd); 293 error = ENOBUFS; 294 goto out; 295 } 296 297 /* Link command to the queue */ 298 ng_l2cap_link_cmd(con, cmd); 299 ng_l2cap_lp_deliver(con); 300 out: 301 return (error); 302 } /* ng_l2cap_l2ca_con_rsp_req */ 303 304 /* 305 * Send L2CAP_ConnectRsp response to the upper layer 306 */ 307 308 int 309 ng_l2cap_l2ca_con_rsp_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result) 310 { 311 ng_l2cap_p l2cap = ch->con->l2cap; 312 struct ng_mesg *msg = NULL; 313 ng_l2cap_l2ca_con_rsp_op *op = NULL; 314 int error = 0; 315 316 /* Check if upstream hook is connected and valid */ 317 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 318 NG_L2CAP_ERR( 319 "%s: %s - unable to send L2CA_ConnectRsp response message. " \ 320 "Hook is not connected or valid, psm=%d\n", 321 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 322 323 return (ENOTCONN); 324 } 325 326 /* Create and send L2CA_ConnectRsp response message */ 327 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_RSP, 328 sizeof(*op), M_NOWAIT); 329 if (msg == NULL) 330 error = ENOMEM; 331 else { 332 msg->header.token = token; 333 msg->header.flags |= NGF_RESP; 334 335 op = (ng_l2cap_l2ca_con_rsp_op *)(msg->data); 336 op->result = result; 337 338 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 339 } 340 341 return (error); 342 } /* ng_l2cap_l2ca_con_rsp_rsp */ 343 344 /* 345 * Send L2CA_ConnectInd message to the upper layer protocol. 346 */ 347 348 int 349 ng_l2cap_l2ca_con_ind(ng_l2cap_chan_p ch) 350 { 351 ng_l2cap_p l2cap = ch->con->l2cap; 352 struct ng_mesg *msg = NULL; 353 ng_l2cap_l2ca_con_ind_ip *ip = NULL; 354 int error = 0; 355 356 /* Check if upstream hook is connected and valid */ 357 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 358 NG_L2CAP_ERR( 359 "%s: %s - unable to send L2CA_ConnectInd message. " \ 360 "Hook is not connected or valid, psm=%d\n", 361 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 362 363 return (ENOTCONN); 364 } 365 366 /* Create and send L2CA_ConnectInd message */ 367 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_IND, 368 sizeof(*ip), M_NOWAIT); 369 if (msg == NULL) 370 error = ENOMEM; 371 else { 372 ip = (ng_l2cap_l2ca_con_ind_ip *)(msg->data); 373 374 bcopy(&ch->con->remote, &ip->bdaddr, sizeof(ip->bdaddr)); 375 ip->lcid = ch->scid; 376 ip->psm = ch->psm; 377 ip->ident = ch->ident; 378 379 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 380 } 381 382 return (error); 383 } /* ng_l2cap_l2ca_con_ind */ 384 385 /* 386 * Process L2CA_Config request from the upper layer protocol 387 */ 388 389 int 390 ng_l2cap_l2ca_cfg_req(ng_l2cap_p l2cap, struct ng_mesg *msg) 391 { 392 ng_l2cap_l2ca_cfg_ip *ip = NULL; 393 ng_l2cap_chan_p ch = NULL; 394 ng_l2cap_cmd_p cmd = NULL; 395 struct mbuf *opt = NULL; 396 u_int16_t *mtu = NULL, *flush_timo = NULL; 397 ng_l2cap_flow_p flow = NULL; 398 int error = 0; 399 400 /* Check message */ 401 if (msg->header.arglen != sizeof(*ip)) { 402 NG_L2CAP_ALERT( 403 "%s: %s - Invalid L2CA_Config request message size, size=%d\n", 404 __func__, NG_NODE_NAME(l2cap->node), 405 msg->header.arglen); 406 error = EMSGSIZE; 407 goto out; 408 } 409 410 ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data); 411 412 /* Check if we have this channel */ 413 ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); 414 if (ch == NULL) { 415 NG_L2CAP_ERR( 416 "%s: %s - unexpected L2CA_Config request message. " \ 417 "Channel does not exist, lcid=%d\n", 418 __func__, NG_NODE_NAME(l2cap->node), ip->lcid); 419 error = ENOENT; 420 goto out; 421 } 422 423 /* Check channel state */ 424 if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) { 425 NG_L2CAP_ERR( 426 "%s: %s - unexpected L2CA_Config request message. " \ 427 "Invalid channel state, state=%d, lcid=%d\n", 428 __func__, NG_NODE_NAME(l2cap->node), ch->state, 429 ch->scid); 430 error = EINVAL; 431 goto out; 432 } 433 434 /* Set requested channel configuration options */ 435 ch->imtu = ip->imtu; 436 bcopy(&ip->oflow, &ch->oflow, sizeof(ch->oflow)); 437 ch->flush_timo = ip->flush_timo; 438 ch->link_timo = ip->link_timo; 439 440 /* Compare channel settings with defaults */ 441 if (ch->imtu != NG_L2CAP_MTU_DEFAULT) 442 mtu = &ch->imtu; 443 if (ch->flush_timo != NG_L2CAP_FLUSH_TIMO_DEFAULT) 444 flush_timo = &ch->flush_timo; 445 if (bcmp(ng_l2cap_default_flow(), &ch->oflow, sizeof(ch->oflow)) != 0) 446 flow = &ch->oflow; 447 448 /* Create configuration options */ 449 _ng_l2cap_build_cfg_options(opt, mtu, flush_timo, flow); 450 if (opt == NULL) { 451 error = ENOBUFS; 452 goto out; 453 } 454 455 /* Create L2CAP command descriptor */ 456 cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con), 457 NG_L2CAP_CFG_REQ, msg->header.token); 458 if (cmd == NULL) { 459 NG_FREE_M(opt); 460 error = ENOMEM; 461 goto out; 462 } 463 464 if (cmd->ident == NG_L2CAP_NULL_IDENT) { 465 ng_l2cap_free_cmd(cmd); 466 NG_FREE_M(opt); 467 error = EIO; 468 goto out; 469 } 470 471 /* Create L2CAP command packet */ 472 _ng_l2cap_cfg_req(cmd->aux, cmd->ident, ch->dcid, 0, opt); 473 if (cmd->aux == NULL) { 474 ng_l2cap_free_cmd(cmd); 475 error = ENOBUFS; 476 goto out; 477 } 478 479 /* Adjust channel state for re-configuration */ 480 if (ch->state == NG_L2CAP_OPEN) { 481 ch->state = NG_L2CAP_CONFIG; 482 ch->cfg_state = 0; 483 } 484 485 /* Link command to the queue */ 486 ng_l2cap_link_cmd(ch->con, cmd); 487 ng_l2cap_lp_deliver(ch->con); 488 out: 489 return (error); 490 } /* ng_l2cap_l2ca_cfg_req */ 491 492 /* 493 * Send L2CA_Config response to the upper layer protocol 494 */ 495 496 int 497 ng_l2cap_l2ca_cfg_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result) 498 { 499 ng_l2cap_p l2cap = ch->con->l2cap; 500 struct ng_mesg *msg = NULL; 501 ng_l2cap_l2ca_cfg_op *op = NULL; 502 int error = 0; 503 504 /* Check if upstream hook is connected and valid */ 505 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 506 NG_L2CAP_ERR( 507 "%s: %s - unable to send L2CA_Config response message. " \ 508 "Hook is not connected or valid, psm=%d\n", 509 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 510 511 return (ENOTCONN); 512 } 513 514 /* Create and send L2CA_Config response message */ 515 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG, 516 sizeof(*op), M_NOWAIT); 517 if (msg == NULL) 518 error = ENOMEM; 519 else { 520 msg->header.token = token; 521 msg->header.flags |= NGF_RESP; 522 523 op = (ng_l2cap_l2ca_cfg_op *)(msg->data); 524 op->result = result; 525 op->imtu = ch->imtu; 526 bcopy(&ch->oflow, &op->oflow, sizeof(op->oflow)); 527 op->flush_timo = ch->flush_timo; 528 529 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 530 531 if (error == 0 && result == NG_L2CAP_SUCCESS) { 532 ch->cfg_state |= NG_L2CAP_CFG_IN; 533 534 if (ch->cfg_state == NG_L2CAP_CFG_BOTH) 535 ch->state = NG_L2CAP_OPEN; 536 } 537 } 538 539 return (error); 540 } /* ng_l2cap_l2ca_cfg_rsp */ 541 542 /* 543 * Process L2CA_ConfigRsp request from the upper layer protocol 544 * 545 * XXX XXX XXX 546 * 547 * NOTE: The Bluetooth specification says that Configuration_Response 548 * (L2CA_ConfigRsp) should be used to issue response to configuration request 549 * indication. The minor problem here is L2CAP command ident. We should use 550 * ident from original L2CAP request to make sure our peer can match request 551 * and response. For some reason Bluetooth specification does not include 552 * ident field into L2CA_ConfigInd and L2CA_ConfigRsp messages. This seems 553 * strange to me, because L2CA_ConnectInd and L2CA_ConnectRsp do have ident 554 * field. So we should store last known L2CAP request command ident in channel. 555 * Also it seems that upper layer can not reject configuration request, as 556 * Configuration_Response message does not have status/reason field. 557 */ 558 559 int 560 ng_l2cap_l2ca_cfg_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg) 561 { 562 ng_l2cap_l2ca_cfg_rsp_ip *ip = NULL; 563 ng_l2cap_chan_p ch = NULL; 564 ng_l2cap_cmd_p cmd = NULL; 565 struct mbuf *opt = NULL; 566 u_int16_t *mtu = NULL; 567 ng_l2cap_flow_p flow = NULL; 568 int error = 0; 569 570 /* Check message */ 571 if (msg->header.arglen != sizeof(*ip)) { 572 NG_L2CAP_ALERT( 573 "%s: %s - invalid L2CA_ConfigRsp request message size, size=%d\n", 574 __func__, NG_NODE_NAME(l2cap->node), 575 msg->header.arglen); 576 error = EMSGSIZE; 577 goto out; 578 } 579 580 ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data); 581 582 /* Check if we have this channel */ 583 ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); 584 if (ch == NULL) { 585 NG_L2CAP_ERR( 586 "%s: %s - unexpected L2CA_ConfigRsp request message. " \ 587 "Channel does not exist, lcid=%d\n", 588 __func__, NG_NODE_NAME(l2cap->node), ip->lcid); 589 error = ENOENT; 590 goto out; 591 } 592 593 /* Check channel state */ 594 if (ch->state != NG_L2CAP_CONFIG) { 595 NG_L2CAP_ERR( 596 "%s: %s - unexpected L2CA_ConfigRsp request message. " \ 597 "Invalid channel state, state=%d, lcid=%d\n", 598 __func__, NG_NODE_NAME(l2cap->node), ch->state, 599 ch->scid); 600 error = EINVAL; 601 goto out; 602 } 603 604 /* Set channel settings */ 605 if (ip->omtu != ch->omtu) { 606 ch->omtu = ip->omtu; 607 mtu = &ch->omtu; 608 } 609 610 if (bcmp(&ip->iflow, &ch->iflow, sizeof(ch->iflow)) != 0) { 611 bcopy(&ip->iflow, &ch->iflow, sizeof(ch->iflow)); 612 flow = &ch->iflow; 613 } 614 615 if (mtu != NULL || flow != NULL) { 616 _ng_l2cap_build_cfg_options(opt, mtu, NULL, flow); 617 if (opt == NULL) { 618 error = ENOBUFS; 619 goto out; 620 } 621 } 622 623 /* Create L2CAP command */ 624 cmd = ng_l2cap_new_cmd(ch->con, ch, ch->ident, NG_L2CAP_CFG_RSP, 625 msg->header.token); 626 if (cmd == NULL) { 627 NG_FREE_M(opt); 628 error = ENOMEM; 629 goto out; 630 } 631 632 _ng_l2cap_cfg_rsp(cmd->aux,cmd->ident,ch->dcid,0,NG_L2CAP_SUCCESS,opt); 633 if (cmd->aux == NULL) { 634 ng_l2cap_free_cmd(cmd); 635 error = ENOBUFS; 636 goto out; 637 } 638 639 /* XXX FIXME - not here ??? */ 640 ch->cfg_state |= NG_L2CAP_CFG_OUT; 641 if (ch->cfg_state == NG_L2CAP_CFG_BOTH) 642 ch->state = NG_L2CAP_OPEN; 643 644 /* Link command to the queue */ 645 ng_l2cap_link_cmd(ch->con, cmd); 646 ng_l2cap_lp_deliver(ch->con); 647 out: 648 return (error); 649 } /* ng_l2cap_l2ca_cfg_rsp_req */ 650 651 /* 652 * Send L2CA_ConfigRsp response to the upper layer protocol 653 */ 654 655 int 656 ng_l2cap_l2ca_cfg_rsp_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result) 657 { 658 ng_l2cap_p l2cap = ch->con->l2cap; 659 struct ng_mesg *msg = NULL; 660 ng_l2cap_l2ca_cfg_rsp_op *op = NULL; 661 int error = 0; 662 663 /* Check if upstream hook is connected and valid */ 664 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 665 NG_L2CAP_ERR( 666 "%s: %s - unable to send L2CA_ConfigRsp response message. " \ 667 "Hook is not connected or valid, psm=%d\n", 668 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 669 670 return (ENOTCONN); 671 } 672 673 /* Create and send L2CA_ConfigRsp response message */ 674 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG_RSP, 675 sizeof(*op), M_NOWAIT); 676 if (msg == NULL) 677 error = ENOMEM; 678 else { 679 msg->header.token = token; 680 msg->header.flags |= NGF_RESP; 681 682 op = (ng_l2cap_l2ca_cfg_rsp_op *)(msg->data); 683 op->result = result; 684 685 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 686 } 687 688 return (error); 689 } /* ng_l2cap_l2ca_cfg_rsp_rsp */ 690 691 /* 692 * Send L2CA_ConfigInd message to the upper layer protocol 693 * 694 * XXX XXX XXX 695 * 696 * NOTE: The Bluetooth specification says that Configuration_Response 697 * (L2CA_ConfigRsp) should be used to issue response to configuration request 698 * indication. The minor problem here is L2CAP command ident. We should use 699 * ident from original L2CAP request to make sure our peer can match request 700 * and response. For some reason Bluetooth specification does not include 701 * ident field into L2CA_ConfigInd and L2CA_ConfigRsp messages. This seems 702 * strange to me, because L2CA_ConnectInd and L2CA_ConnectRsp do have ident 703 * field. So we should store last known L2CAP request command ident in channel. 704 * Also it seems that upper layer can not reject configuration request, as 705 * Configuration_Response message does not have status/reason field. 706 */ 707 708 int 709 ng_l2cap_l2ca_cfg_ind(ng_l2cap_chan_p ch) 710 { 711 ng_l2cap_p l2cap = ch->con->l2cap; 712 struct ng_mesg *msg = NULL; 713 ng_l2cap_l2ca_cfg_ind_ip *ip = NULL; 714 int error = 0; 715 716 /* Check if upstream hook is connected and valid */ 717 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 718 NG_L2CAP_ERR( 719 "%s: %s - Unable to send L2CA_ConfigInd message. " \ 720 "Hook is not connected or valid, psm=%d\n", 721 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 722 723 return (ENOTCONN); 724 } 725 726 /* Create and send L2CA_ConnectInd message */ 727 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG_IND, 728 sizeof(*ip), M_NOWAIT); 729 if (msg == NULL) 730 error = ENOMEM; 731 else { 732 ip = (ng_l2cap_l2ca_cfg_ind_ip *)(msg->data); 733 ip->lcid = ch->scid; 734 ip->omtu = ch->omtu; 735 bcopy(&ch->iflow, &ip->iflow, sizeof(ip->iflow)); 736 ip->flush_timo = ch->flush_timo; 737 738 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 739 } 740 741 return (error); 742 } /* ng_l2cap_l2ca_cfg_ind */ 743 744 /* 745 * Process L2CA_Write event 746 */ 747 748 int 749 ng_l2cap_l2ca_write_req(ng_l2cap_p l2cap, struct mbuf *m) 750 { 751 ng_l2cap_l2ca_hdr_t *l2ca_hdr = NULL; 752 ng_l2cap_chan_p ch = NULL; 753 ng_l2cap_cmd_p cmd = NULL; 754 int error = 0; 755 u_int32_t token = 0; 756 757 /* Make sure we can access L2CA data packet header */ 758 if (m->m_pkthdr.len < sizeof(*l2ca_hdr)) { 759 NG_L2CAP_ERR( 760 "%s: %s - L2CA Data packet too small, len=%d\n", 761 __func__,NG_NODE_NAME(l2cap->node),m->m_pkthdr.len); 762 error = EMSGSIZE; 763 goto drop; 764 } 765 766 /* Get L2CA data packet header */ 767 NG_L2CAP_M_PULLUP(m, sizeof(*l2ca_hdr)); 768 if (m == NULL) 769 return (ENOBUFS); 770 771 l2ca_hdr = mtod(m, ng_l2cap_l2ca_hdr_t *); 772 token = l2ca_hdr->token; 773 m_adj(m, sizeof(*l2ca_hdr)); 774 775 /* Verify payload size */ 776 if (l2ca_hdr->length != m->m_pkthdr.len) { 777 NG_L2CAP_ERR( 778 "%s: %s - invalid L2CA Data packet. " \ 779 "Payload length does not match, length=%d, len=%d\n", 780 __func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->length, 781 m->m_pkthdr.len); 782 error = EMSGSIZE; 783 goto drop; 784 } 785 786 /* Check channel ID */ 787 if (l2ca_hdr->lcid < NG_L2CAP_FIRST_CID) { 788 NG_L2CAP_ERR( 789 "%s: %s - invalid L2CA Data packet. Inavlid channel ID, cid=%d\n", 790 __func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid); 791 error = EINVAL; 792 goto drop; 793 } 794 795 /* Verify that we have the channel and make sure it is open */ 796 ch = ng_l2cap_chan_by_scid(l2cap, l2ca_hdr->lcid); 797 if (ch == NULL) { 798 NG_L2CAP_ERR( 799 "%s: %s - invalid L2CA Data packet. Channel does not exist, cid=%d\n", 800 __func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid); 801 error = ENOENT; 802 goto drop; 803 } 804 805 if (ch->state != NG_L2CAP_OPEN) { 806 NG_L2CAP_ERR( 807 "%s: %s - invalid L2CA Data packet. Invalid channel state, scid=%d, state=%d\n", 808 __func__, NG_NODE_NAME(l2cap->node), ch->scid, 809 ch->state); 810 error = EHOSTDOWN; 811 goto drop; /* XXX not always - re-configure */ 812 } 813 814 /* Create L2CAP command descriptor */ 815 cmd = ng_l2cap_new_cmd(ch->con, ch, 0, NGM_L2CAP_L2CA_WRITE, token); 816 if (cmd == NULL) { 817 error = ENOMEM; 818 goto drop; 819 } 820 821 /* Attach data packet and link command to the queue */ 822 cmd->aux = m; 823 ng_l2cap_link_cmd(ch->con, cmd); 824 ng_l2cap_lp_deliver(ch->con); 825 826 return (error); 827 drop: 828 NG_FREE_M(m); 829 830 return (error); 831 } /* ng_l2cap_l2ca_write_req */ 832 833 /* 834 * Send L2CA_Write response 835 */ 836 837 int 838 ng_l2cap_l2ca_write_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result, 839 u_int16_t length) 840 { 841 ng_l2cap_p l2cap = ch->con->l2cap; 842 struct ng_mesg *msg = NULL; 843 ng_l2cap_l2ca_write_op *op = NULL; 844 int error = 0; 845 846 /* Check if upstream hook is connected and valid */ 847 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 848 NG_L2CAP_ERR( 849 "%s: %s - unable to send L2CA_WriteRsp message. " \ 850 "Hook is not connected or valid, psm=%d\n", 851 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 852 853 return (ENOTCONN); 854 } 855 856 /* Create and send L2CA_WriteRsp message */ 857 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_WRITE, 858 sizeof(*op), M_NOWAIT); 859 if (msg == NULL) 860 error = ENOMEM; 861 else { 862 msg->header.token = token; 863 msg->header.flags |= NGF_RESP; 864 865 op = (ng_l2cap_l2ca_write_op *)(msg->data); 866 op->result = result; 867 op->length = length; 868 op->lcid = ch->scid; 869 870 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 871 } 872 873 return (error); 874 } /* ng_l2cap_l2ca_write_rsp */ 875 876 /* 877 * Receive packet from the lower layer protocol and send it to the upper 878 * layer protocol (L2CAP_Read) 879 */ 880 881 int 882 ng_l2cap_l2ca_receive(ng_l2cap_con_p con) 883 { 884 ng_l2cap_p l2cap = con->l2cap; 885 ng_l2cap_hdr_t *hdr = NULL; 886 ng_l2cap_chan_p ch = NULL; 887 int error = 0; 888 889 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 890 if (con->rx_pkt == NULL) 891 return (ENOBUFS); 892 893 hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *); 894 895 /* Check channel */ 896 ch = ng_l2cap_chan_by_scid(l2cap, hdr->dcid); 897 if (ch == NULL) { 898 NG_L2CAP_ERR( 899 "%s: %s - unexpected L2CAP data packet. Channel does not exist, cid=%d\n", 900 __func__, NG_NODE_NAME(l2cap->node), hdr->dcid); 901 error = ENOENT; 902 goto drop; 903 } 904 905 /* Check channel state */ 906 if (ch->state != NG_L2CAP_OPEN) { 907 NG_L2CAP_WARN( 908 "%s: %s - unexpected L2CAP data packet. " \ 909 "Invalid channel state, cid=%d, state=%d\n", 910 __func__, NG_NODE_NAME(l2cap->node), ch->scid, 911 ch->state); 912 error = EHOSTDOWN; /* XXX not always - re-configuration */ 913 goto drop; 914 } 915 916 /* Check payload size and channel's MTU */ 917 if (hdr->length > ch->imtu) { 918 NG_L2CAP_ERR( 919 "%s: %s - invalid L2CAP data packet. " \ 920 "Packet too big, length=%d, imtu=%d, cid=%d\n", 921 __func__, NG_NODE_NAME(l2cap->node), hdr->length, 922 ch->imtu, ch->scid); 923 error = EMSGSIZE; 924 goto drop; 925 } 926 927 /* 928 * If we got here then everything looks good and we can sent packet 929 * to the upper layer protocol. 930 */ 931 932 /* Check if upstream hook is connected and valid */ 933 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 934 NG_L2CAP_ERR( 935 "%s: %s - unable to send L2CAP data packet. " \ 936 "Hook is not connected or valid, psm=%d\n", 937 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 938 error = ENOTCONN; 939 goto drop; 940 } 941 942 NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt); 943 con->rx_pkt = NULL; 944 drop: 945 NG_FREE_M(con->rx_pkt); /* checks for != NULL */ 946 947 return (error); 948 } /* ng_l2cap_receive */ 949 950 /* 951 * Receive connectioless (multicast) packet from the lower layer protocol and 952 * send it to the upper layer protocol 953 */ 954 955 int 956 ng_l2cap_l2ca_clt_receive(ng_l2cap_con_p con) 957 { 958 struct _clt_pkt { 959 ng_l2cap_hdr_t h; 960 ng_l2cap_clt_hdr_t c_h; 961 } __attribute__ ((packed)) *hdr = NULL; 962 ng_l2cap_p l2cap = con->l2cap; 963 int length, error = 0; 964 965 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 966 if (con->rx_pkt == NULL) 967 return (ENOBUFS); 968 969 hdr = mtod(con->rx_pkt, struct _clt_pkt *); 970 971 /* Check packet */ 972 length = con->rx_pkt->m_pkthdr.len - sizeof(*hdr); 973 if (length < 0) { 974 NG_L2CAP_ERR( 975 "%s: %s - invalid L2CAP CLT data packet. Packet too small, length=%d\n", 976 __func__, NG_NODE_NAME(l2cap->node), length); 977 error = EMSGSIZE; 978 goto drop; 979 } 980 981 /* Check payload size against CLT MTU */ 982 if (length > NG_L2CAP_MTU_DEFAULT) { 983 NG_L2CAP_ERR( 984 "%s: %s - invalid L2CAP CLT data packet. Packet too big, length=%d, mtu=%d\n", 985 __func__, NG_NODE_NAME(l2cap->node), length, 986 NG_L2CAP_MTU_DEFAULT); 987 error = EMSGSIZE; 988 goto drop; 989 } 990 991 hdr->c_h.psm = le16toh(hdr->c_h.psm); 992 993 /* 994 * If we got here then everything looks good and we can sent packet 995 * to the upper layer protocol. 996 */ 997 998 /* Select upstream hook based on PSM */ 999 switch (hdr->c_h.psm) { 1000 case NG_L2CAP_PSM_SDP: 1001 if (l2cap->flags & NG_L2CAP_CLT_SDP_DISABLED) 1002 goto drop; 1003 break; 1004 1005 case NG_L2CAP_PSM_RFCOMM: 1006 if (l2cap->flags & NG_L2CAP_CLT_RFCOMM_DISABLED) 1007 goto drop; 1008 break; 1009 1010 case NG_L2CAP_PSM_TCP: 1011 if (l2cap->flags & NG_L2CAP_CLT_TCP_DISABLED) 1012 goto drop; 1013 break; 1014 } 1015 1016 /* Check if upstream hook is connected and valid */ 1017 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 1018 NG_L2CAP_ERR( 1019 "%s: %s - unable to send L2CAP CLT data packet. " \ 1020 "Hook is not connected or valid, psm=%d\n", 1021 __func__, NG_NODE_NAME(l2cap->node), hdr->c_h.psm); 1022 error = ENOTCONN; 1023 goto drop; 1024 } 1025 1026 NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt); 1027 con->rx_pkt = NULL; 1028 drop: 1029 NG_FREE_M(con->rx_pkt); /* checks for != NULL */ 1030 1031 return (error); 1032 } /* ng_l2cap_l2ca_clt_receive */ 1033 1034 /* 1035 * Send L2CA_QoSViolationInd to the upper layer protocol 1036 */ 1037 1038 int 1039 ng_l2cap_l2ca_qos_ind(ng_l2cap_chan_p ch) 1040 { 1041 ng_l2cap_p l2cap = ch->con->l2cap; 1042 struct ng_mesg *msg = NULL; 1043 ng_l2cap_l2ca_qos_ind_ip *ip = NULL; 1044 int error = 0; 1045 1046 /* Check if upstream hook is connected and valid */ 1047 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 1048 NG_L2CAP_ERR( 1049 "%s: %s - unable to send L2CA_QoSViolationInd message. " \ 1050 "Hook is not connected or valid, psm=%d\n", 1051 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 1052 1053 return (ENOTCONN); 1054 } 1055 1056 /* Create and send L2CA_QoSViolationInd message */ 1057 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_QOS_IND, 1058 sizeof(*ip), M_NOWAIT); 1059 if (msg == NULL) 1060 error = ENOMEM; 1061 else { 1062 ip = (ng_l2cap_l2ca_qos_ind_ip *)(msg->data); 1063 bcopy(&ch->con->remote, &ip->bdaddr, sizeof(ip->bdaddr)); 1064 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 1065 } 1066 1067 return (error); 1068 } /* ng_l2cap_l2ca_qos_ind */ 1069 1070 /* 1071 * Process L2CA_Disconnect request from the upper layer protocol. 1072 */ 1073 1074 int 1075 ng_l2cap_l2ca_discon_req(ng_l2cap_p l2cap, struct ng_mesg *msg) 1076 { 1077 ng_l2cap_l2ca_discon_ip *ip = NULL; 1078 ng_l2cap_chan_p ch = NULL; 1079 ng_l2cap_cmd_p cmd = NULL; 1080 int error = 0; 1081 1082 /* Check message */ 1083 if (msg->header.arglen != sizeof(*ip)) { 1084 NG_L2CAP_ALERT( 1085 "%s: %s - invalid L2CA_Disconnect request message size, size=%d\n", 1086 __func__, NG_NODE_NAME(l2cap->node), 1087 msg->header.arglen); 1088 error = EMSGSIZE; 1089 goto out; 1090 } 1091 1092 ip = (ng_l2cap_l2ca_discon_ip *)(msg->data); 1093 1094 /* Check if we have this channel */ 1095 ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); 1096 if (ch == NULL) { 1097 NG_L2CAP_ERR( 1098 "%s: %s - unexpected L2CA_Disconnect request message. " \ 1099 "Channel does not exist, lcid=%d\n", 1100 __func__, NG_NODE_NAME(l2cap->node), ip->lcid); 1101 error = ENOENT; 1102 goto out; 1103 } 1104 1105 /* Check channel state */ 1106 if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN && 1107 ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 1108 NG_L2CAP_ERR( 1109 "%s: %s - unexpected L2CA_Disconnect request message. " \ 1110 "Invalid channel state, state=%d, lcid=%d\n", 1111 __func__, NG_NODE_NAME(l2cap->node), ch->state, 1112 ch->scid); 1113 error = EINVAL; 1114 goto out; 1115 } 1116 1117 /* Create and send L2CAP_DisconReq message */ 1118 cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con), 1119 NG_L2CAP_DISCON_REQ, msg->header.token); 1120 if (cmd == NULL) { 1121 ng_l2cap_free_chan(ch); 1122 error = ENOMEM; 1123 goto out; 1124 } 1125 1126 if (cmd->ident == NG_L2CAP_NULL_IDENT) { 1127 ng_l2cap_free_chan(ch); 1128 ng_l2cap_free_cmd(cmd); 1129 error = EIO; 1130 goto out; 1131 } 1132 1133 _ng_l2cap_discon_req(cmd->aux, cmd->ident, ch->dcid, ch->scid); 1134 if (cmd->aux == NULL) { 1135 ng_l2cap_free_chan(ch); 1136 ng_l2cap_free_cmd(cmd); 1137 error = ENOBUFS; 1138 goto out; 1139 } 1140 1141 ch->state = NG_L2CAP_W4_L2CAP_DISCON_RSP; 1142 1143 /* Link command to the queue */ 1144 ng_l2cap_link_cmd(ch->con, cmd); 1145 ng_l2cap_lp_deliver(ch->con); 1146 out: 1147 return (error); 1148 } /* ng_l2cap_l2ca_discon_req */ 1149 1150 /* 1151 * Send L2CA_Disconnect response to the upper layer protocol 1152 */ 1153 1154 int 1155 ng_l2cap_l2ca_discon_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result) 1156 { 1157 ng_l2cap_p l2cap = ch->con->l2cap; 1158 struct ng_mesg *msg = NULL; 1159 ng_l2cap_l2ca_discon_op *op = NULL; 1160 int error = 0; 1161 1162 /* Check if upstream hook is connected and valid */ 1163 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 1164 NG_L2CAP_ERR( 1165 "%s: %s - unable to send L2CA_Disconnect response message. " \ 1166 "Hook is not connected or valid, psm=%d\n", 1167 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 1168 1169 return (ENOTCONN); 1170 } 1171 1172 /* Create and send L2CA_Disconnect response message */ 1173 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON, 1174 sizeof(*op), M_NOWAIT); 1175 if (msg == NULL) 1176 error = ENOMEM; 1177 else { 1178 msg->header.token = token; 1179 msg->header.flags |= NGF_RESP; 1180 1181 op = (ng_l2cap_l2ca_discon_op *)(msg->data); 1182 op->result = result; 1183 1184 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 1185 } 1186 1187 return (error); 1188 } /* ng_l2cap_l2ca_discon_rsp */ 1189 1190 /* 1191 * Send L2CA_DisconnectInd message to the upper layer protocol. 1192 */ 1193 1194 int 1195 ng_l2cap_l2ca_discon_ind(ng_l2cap_chan_p ch) 1196 { 1197 ng_l2cap_p l2cap = ch->con->l2cap; 1198 struct ng_mesg *msg = NULL; 1199 ng_l2cap_l2ca_discon_ind_ip *ip = NULL; 1200 int error = 0; 1201 1202 /* Check if upstream hook is connected and valid */ 1203 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { 1204 NG_L2CAP_ERR( 1205 "%s: %s - unable to send L2CA_DisconnectInd message. " \ 1206 "Hook is not connected or valid, psm=%d\n", 1207 __func__, NG_NODE_NAME(l2cap->node), ch->psm); 1208 1209 return (ENOTCONN); 1210 } 1211 1212 /* Create and send L2CA_DisconnectInd message */ 1213 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON_IND, 1214 sizeof(*ip), M_NOWAIT); 1215 if (msg == NULL) 1216 error = ENOMEM; 1217 else { 1218 ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data); 1219 ip->lcid = ch->scid; 1220 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); 1221 } 1222 1223 return (error); 1224 } /* ng_l2cap_l2ca_discon_ind */ 1225 1226 /* 1227 * Process L2CA_GroupCreate request from the upper layer protocol. 1228 * XXX FIXME 1229 */ 1230 1231 int 1232 ng_l2cap_l2ca_grp_create(ng_l2cap_p l2cap, struct ng_mesg *msg) 1233 { 1234 return (ENOTSUP); 1235 } /* ng_l2cap_l2ca_grp_create */ 1236 1237 /* 1238 * Process L2CA_GroupClose request from the upper layer protocol 1239 * XXX FIXME 1240 */ 1241 1242 int 1243 ng_l2cap_l2ca_grp_close(ng_l2cap_p l2cap, struct ng_mesg *msg) 1244 { 1245 return (ENOTSUP); 1246 } /* ng_l2cap_l2ca_grp_close */ 1247 1248 /* 1249 * Process L2CA_GroupAddMember request from the upper layer protocol. 1250 * XXX FIXME 1251 */ 1252 1253 int 1254 ng_l2cap_l2ca_grp_add_member_req(ng_l2cap_p l2cap, struct ng_mesg *msg) 1255 { 1256 return (ENOTSUP); 1257 } /* ng_l2cap_l2ca_grp_add_member_req */ 1258 1259 /* 1260 * Send L2CA_GroupAddMember response to the upper layer protocol. 1261 * XXX FIXME 1262 */ 1263 1264 int 1265 ng_l2cap_l2ca_grp_add_member_rsp(ng_l2cap_chan_p ch, u_int32_t token, 1266 u_int16_t result) 1267 { 1268 return (0); 1269 } /* ng_l2cap_l2ca_grp_add_member_rsp */ 1270 1271 /* 1272 * Process L2CA_GroupDeleteMember request from the upper layer protocol 1273 * XXX FIXME 1274 */ 1275 1276 int 1277 ng_l2cap_l2ca_grp_rem_member(ng_l2cap_p l2cap, struct ng_mesg *msg) 1278 { 1279 return (ENOTSUP); 1280 } /* ng_l2cap_l2ca_grp_rem_member */ 1281 1282 /* 1283 * Process L2CA_GroupGetMembers request from the upper layer protocol 1284 * XXX FIXME 1285 */ 1286 1287 int 1288 ng_l2cap_l2ca_grp_get_members(ng_l2cap_p l2cap, struct ng_mesg *msg) 1289 { 1290 return (ENOTSUP); 1291 } /* ng_l2cap_l2ca_grp_get_members */ 1292 1293 /* 1294 * Process L2CA_Ping request from the upper layer protocol 1295 */ 1296 1297 int 1298 ng_l2cap_l2ca_ping_req(ng_l2cap_p l2cap, struct ng_mesg *msg) 1299 { 1300 ng_l2cap_l2ca_ping_ip *ip = NULL; 1301 ng_l2cap_con_p con = NULL; 1302 ng_l2cap_cmd_p cmd = NULL; 1303 int error = 0; 1304 1305 /* Verify message */ 1306 if (msg->header.arglen < sizeof(*ip)) { 1307 NG_L2CAP_ALERT( 1308 "%s: %s - invalid L2CA_Ping request message size, size=%d\n", 1309 __func__, NG_NODE_NAME(l2cap->node), 1310 msg->header.arglen); 1311 error = EMSGSIZE; 1312 goto out; 1313 } 1314 1315 ip = (ng_l2cap_l2ca_ping_ip *)(msg->data); 1316 if (ip->echo_size > NG_L2CAP_MAX_ECHO_SIZE) { 1317 NG_L2CAP_WARN( 1318 "%s: %s - invalid L2CA_Ping request. Echo size is too big, echo_size=%d\n", 1319 __func__, NG_NODE_NAME(l2cap->node), ip->echo_size); 1320 error = EMSGSIZE; 1321 goto out; 1322 } 1323 1324 /* Check if we have connection to the unit */ 1325 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); 1326 if (con == NULL) { 1327 /* Submit LP_ConnectReq to the lower layer */ 1328 error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr); 1329 if (error != 0) { 1330 NG_L2CAP_ERR( 1331 "%s: %s - unable to send LP_ConnectReq message, error=%d\n", 1332 __func__, NG_NODE_NAME(l2cap->node), error); 1333 goto out; 1334 } 1335 1336 /* This should not fail */ 1337 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); 1338 KASSERT((con != NULL), 1339 ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node))); 1340 } 1341 1342 /* Create L2CAP command descriptor */ 1343 cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con), 1344 NG_L2CAP_ECHO_REQ, msg->header.token); 1345 if (cmd == NULL) { 1346 error = ENOMEM; 1347 goto out; 1348 } 1349 1350 if (cmd->ident == NG_L2CAP_NULL_IDENT) { 1351 ng_l2cap_free_cmd(cmd); 1352 error = EIO; 1353 goto out; 1354 } 1355 1356 /* Create L2CAP command packet */ 1357 _ng_l2cap_echo_req(cmd->aux, cmd->ident, 1358 msg->data + sizeof(*ip), ip->echo_size); 1359 if (cmd->aux == NULL) { 1360 ng_l2cap_free_cmd(cmd); 1361 error = ENOBUFS; 1362 goto out; 1363 } 1364 1365 /* Link command to the queue */ 1366 ng_l2cap_link_cmd(con, cmd); 1367 ng_l2cap_lp_deliver(con); 1368 out: 1369 return (error); 1370 } /* ng_l2cap_l2ca_ping_req */ 1371 1372 /* 1373 * Send L2CA_Ping response to the upper layer protocol 1374 */ 1375 1376 int 1377 ng_l2cap_l2ca_ping_rsp(ng_l2cap_con_p con, u_int32_t token, u_int16_t result, 1378 struct mbuf *data) 1379 { 1380 ng_l2cap_p l2cap = con->l2cap; 1381 struct ng_mesg *msg = NULL; 1382 ng_l2cap_l2ca_ping_op *op = NULL; 1383 int error = 0, size = 0; 1384 1385 /* Check if control hook is connected and valid */ 1386 if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) { 1387 NG_L2CAP_WARN( 1388 "%s: %s - unable to send L2CA_Ping response message. " \ 1389 "Hook is not connected or valid\n", 1390 __func__, NG_NODE_NAME(l2cap->node)); 1391 error = ENOTCONN; 1392 goto out; 1393 } 1394 1395 size = (data == NULL)? 0 : data->m_pkthdr.len; 1396 1397 /* Create and send L2CA_Ping response message */ 1398 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_PING, 1399 sizeof(*op) + size, M_NOWAIT); 1400 if (msg == NULL) 1401 error = ENOMEM; 1402 else { 1403 msg->header.token = token; 1404 msg->header.flags |= NGF_RESP; 1405 1406 op = (ng_l2cap_l2ca_ping_op *)(msg->data); 1407 op->result = result; 1408 bcopy(&con->remote, &op->bdaddr, sizeof(op->bdaddr)); 1409 if (data != NULL && size > 0) { 1410 op->echo_size = size; 1411 m_copydata(data, 0, size, (caddr_t) op + sizeof(*op)); 1412 } 1413 1414 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0); 1415 } 1416 out: 1417 NG_FREE_M(data); 1418 1419 return (error); 1420 } /* ng_l2cap_l2ca_ping_rsp */ 1421 1422 /* 1423 * Process L2CA_GetInfo request from the upper layer protocol 1424 */ 1425 1426 int 1427 ng_l2cap_l2ca_get_info_req(ng_l2cap_p l2cap, struct ng_mesg *msg) 1428 { 1429 ng_l2cap_l2ca_get_info_ip *ip = NULL; 1430 ng_l2cap_con_p con = NULL; 1431 ng_l2cap_cmd_p cmd = NULL; 1432 int error = 0; 1433 1434 /* Verify message */ 1435 if (msg->header.arglen != sizeof(*ip)) { 1436 NG_L2CAP_ALERT( 1437 "%s: %s - invalid L2CA_GetInfo request message size, size=%d\n", 1438 __func__, NG_NODE_NAME(l2cap->node), 1439 msg->header.arglen); 1440 error = EMSGSIZE; 1441 goto out; 1442 } 1443 1444 ip = (ng_l2cap_l2ca_get_info_ip *)(msg->data); 1445 1446 /* Check if we have connection to the unit */ 1447 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); 1448 if (con == NULL) { 1449 /* Submit LP_ConnectReq to the lower layer */ 1450 error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr); 1451 if (error != 0) { 1452 NG_L2CAP_ERR( 1453 "%s: %s - unable to send LP_ConnectReq message, error=%d\n", 1454 __func__, NG_NODE_NAME(l2cap->node), error); 1455 goto out; 1456 } 1457 1458 /* This should not fail */ 1459 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); 1460 KASSERT((con != NULL), 1461 ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node))); 1462 } 1463 1464 /* Create L2CAP command descriptor */ 1465 cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con), 1466 NG_L2CAP_INFO_REQ, msg->header.token); 1467 if (cmd == NULL) { 1468 error = ENOMEM; 1469 goto out; 1470 } 1471 1472 if (cmd->ident == NG_L2CAP_NULL_IDENT) { 1473 ng_l2cap_free_cmd(cmd); 1474 error = EIO; 1475 goto out; 1476 } 1477 1478 /* Create L2CAP command packet */ 1479 _ng_l2cap_info_req(cmd->aux, cmd->ident, ip->info_type); 1480 if (cmd->aux == NULL) { 1481 ng_l2cap_free_cmd(cmd); 1482 error = ENOBUFS; 1483 goto out; 1484 } 1485 1486 /* Link command to the queue */ 1487 ng_l2cap_link_cmd(con, cmd); 1488 ng_l2cap_lp_deliver(con); 1489 out: 1490 return (error); 1491 } /* ng_l2cap_l2ca_get_info_req */ 1492 1493 /* 1494 * Send L2CA_GetInfo response to the upper layer protocol 1495 */ 1496 1497 int 1498 ng_l2cap_l2ca_get_info_rsp(ng_l2cap_con_p con, u_int32_t token, 1499 u_int16_t result, struct mbuf *data) 1500 { 1501 ng_l2cap_p l2cap = con->l2cap; 1502 struct ng_mesg *msg = NULL; 1503 ng_l2cap_l2ca_get_info_op *op = NULL; 1504 int error = 0, size; 1505 1506 /* Check if control hook is connected and valid */ 1507 if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) { 1508 NG_L2CAP_WARN( 1509 "%s: %s - unable to send L2CA_GetInfo response message. " \ 1510 "Hook is not connected or valid\n", 1511 __func__, NG_NODE_NAME(l2cap->node)); 1512 error = ENOTCONN; 1513 goto out; 1514 } 1515 1516 size = (data == NULL)? 0 : data->m_pkthdr.len; 1517 1518 /* Create and send L2CA_GetInfo response message */ 1519 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_GET_INFO, 1520 sizeof(*op) + size, M_NOWAIT); 1521 if (msg == NULL) 1522 error = ENOMEM; 1523 else { 1524 msg->header.token = token; 1525 msg->header.flags |= NGF_RESP; 1526 1527 op = (ng_l2cap_l2ca_get_info_op *)(msg->data); 1528 op->result = result; 1529 if (data != NULL && size > 0) { 1530 op->info_size = size; 1531 m_copydata(data, 0, size, (caddr_t) op + sizeof(*op)); 1532 } 1533 1534 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0); 1535 } 1536 out: 1537 NG_FREE_M(data); 1538 1539 return (error); 1540 } /* ng_l2cap_l2ca_get_info_rsp */ 1541 1542 /* 1543 * Process L2CA_EnableCLT message from the upper layer protocol 1544 * XXX convert to NGN_L2CAP_NODE_SET_FLAGS? 1545 */ 1546 1547 int 1548 ng_l2cap_l2ca_enable_clt(ng_l2cap_p l2cap, struct ng_mesg *msg) 1549 { 1550 ng_l2cap_l2ca_enable_clt_ip *ip = NULL; 1551 int error = 0; 1552 #if 0 1553 * ng_l2cap_l2ca_enable_clt_op *op = NULL; 1554 * u_int16_t result; 1555 * u_int32_t token; 1556 #endif 1557 1558 /* Check message */ 1559 if (msg->header.arglen != sizeof(*ip)) { 1560 NG_L2CAP_ALERT( 1561 "%s: %s - invalid L2CA_EnableCLT message size, size=%d\n", 1562 __func__, NG_NODE_NAME(l2cap->node), 1563 msg->header.arglen); 1564 1565 return (EMSGSIZE); 1566 } 1567 1568 /* Process request */ 1569 ip = (ng_l2cap_l2ca_enable_clt_ip *) (msg->data); 1570 #if 0 1571 * result = NG_L2CAP_SUCCESS; 1572 #endif 1573 1574 switch (ip->psm) 1575 { 1576 case 0: 1577 /* Special case: disable/enable all PSM */ 1578 if (ip->enable) 1579 l2cap->flags &= ~(NG_L2CAP_CLT_SDP_DISABLED | 1580 NG_L2CAP_CLT_RFCOMM_DISABLED | 1581 NG_L2CAP_CLT_TCP_DISABLED); 1582 else 1583 l2cap->flags |= (NG_L2CAP_CLT_SDP_DISABLED | 1584 NG_L2CAP_CLT_RFCOMM_DISABLED | 1585 NG_L2CAP_CLT_TCP_DISABLED); 1586 break; 1587 1588 case NG_L2CAP_PSM_SDP: 1589 if (ip->enable) 1590 l2cap->flags &= ~NG_L2CAP_CLT_SDP_DISABLED; 1591 else 1592 l2cap->flags |= NG_L2CAP_CLT_SDP_DISABLED; 1593 break; 1594 1595 case NG_L2CAP_PSM_RFCOMM: 1596 if (ip->enable) 1597 l2cap->flags &= ~NG_L2CAP_CLT_RFCOMM_DISABLED; 1598 else 1599 l2cap->flags |= NG_L2CAP_CLT_RFCOMM_DISABLED; 1600 break; 1601 1602 case NG_L2CAP_PSM_TCP: 1603 if (ip->enable) 1604 l2cap->flags &= ~NG_L2CAP_CLT_TCP_DISABLED; 1605 else 1606 l2cap->flags |= NG_L2CAP_CLT_TCP_DISABLED; 1607 break; 1608 1609 default: 1610 NG_L2CAP_ERR( 1611 "%s: %s - unsupported PSM=%d\n", __func__, NG_NODE_NAME(l2cap->node), ip->psm); 1612 #if 0 1613 * result = NG_L2CAP_PSM_NOT_SUPPORTED; 1614 #endif 1615 error = ENOTSUP; 1616 break; 1617 } 1618 1619 #if 0 1620 * /* Create and send response message */ 1621 * token = msg->header.token; 1622 * NG_FREE_MSG(msg); 1623 * NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_ENABLE_CLT, 1624 * sizeof(*op), M_NOWAIT); 1625 * if (msg == NULL) 1626 * error = ENOMEM; 1627 * else { 1628 * msg->header.token = token; 1629 * msg->header.flags |= NGF_RESP; 1630 * 1631 * op = (ng_l2cap_l2ca_enable_clt_op *)(msg->data); 1632 * op->result = result; 1633 * } 1634 * 1635 * /* Send response to control hook */ 1636 * if (l2cap->ctl != NULL && NG_HOOK_IS_VALID(l2cap->ctl)) 1637 * NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0); 1638 #endif 1639 1640 return (error); 1641 } /* ng_l2cap_l2ca_enable_clt */ 1642 1643