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