sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c

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