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