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