sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c

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