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
1237         if(ip->idtype == NG_L2CAP_L2CA_IDTYPE_ATT){
1238                 /* Don't send Disconnect request on L2CAP Layer*/
1239                 ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_ATT_CID,
1240                         ip->lcid);
1241
1242                 if(ch != NULL){
1243                         ng_l2cap_free_chan(ch);
1244                 }else{
1245                 NG_L2CAP_ERR(
1246 "%s: %s - unexpected L2CA_Disconnect request message. " \
1247 "Channel does not exist, conhandle=%d\n",
1248                         __func__, NG_NODE_NAME(l2cap->node), ip->lcid);
1249                         error = EINVAL;
1250                 }
1251                 goto out;
1252         }else if(ip->idtype == NG_L2CAP_L2CA_IDTYPE_SMP){
1253                 /* Don't send Disconnect request on L2CAP Layer*/
1254                 ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_SMP_CID,
1255                         ip->lcid);
1256
1257                 if(ch != NULL){
1258                         ng_l2cap_free_chan(ch);
1259                 }else{
1260                 NG_L2CAP_ERR(
1261 "%s: %s - unexpected L2CA_Disconnect request message. " \
1262 "Channel does not exist, conhandle=%d\n",
1263                         __func__, NG_NODE_NAME(l2cap->node), ip->lcid);
1264                         error = EINVAL;
1265                 }
1266                 goto out;
1267         }else{
1268                 /* Check if we have this channel */
1269                 ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid, ip->idtype);
1270         }
1271         if (ch == NULL) {
1272                 NG_L2CAP_ERR(
1273 "%s: %s - unexpected L2CA_Disconnect request message. " \
1274 "Channel does not exist, lcid=%d\n",
1275                         __func__, NG_NODE_NAME(l2cap->node), ip->lcid);
1276                 error = ENOENT;
1277                 goto out;
1278         }
1279
1280         /* Check channel state */
1281         if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN &&
1282             ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
1283                 NG_L2CAP_ERR(
1284 "%s: %s - unexpected L2CA_Disconnect request message. " \
1285 "Invalid channel state, state=%d, lcid=%d\n",
1286                         __func__, NG_NODE_NAME(l2cap->node), ch->state,
1287                         ch->scid);
1288                 error = EINVAL;
1289                 goto out;
1290         }
1291
1292         /* Create and send L2CAP_DisconReq message */
1293         cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con),
1294                         NG_L2CAP_DISCON_REQ, msg->header.token);
1295         if (cmd == NULL) {
1296                 ng_l2cap_free_chan(ch);
1297                 error = ENOMEM;
1298                 goto out;
1299         }
1300
1301         if (cmd->ident == NG_L2CAP_NULL_IDENT) {
1302                 ng_l2cap_free_chan(ch);
1303                 ng_l2cap_free_cmd(cmd);
1304                 error = EIO;
1305                 goto out;
1306         }
1307
1308         _ng_l2cap_discon_req(cmd->aux, cmd->ident, ch->dcid, ch->scid);
1309         if (cmd->aux == NULL) {
1310                 ng_l2cap_free_chan(ch);
1311                 ng_l2cap_free_cmd(cmd);
1312                 error = ENOBUFS;
1313                 goto out;
1314         }
1315
1316         ch->state = NG_L2CAP_W4_L2CAP_DISCON_RSP;
1317
1318         /* Link command to the queue */
1319         ng_l2cap_link_cmd(ch->con, cmd);
1320         ng_l2cap_lp_deliver(ch->con);
1321 out:
1322         return (error);
1323 } /* ng_l2cap_l2ca_discon_req */
1324
1325 /*
1326  * Send L2CA_Disconnect response to the upper layer protocol
1327  */
1328
1329 int
1330 ng_l2cap_l2ca_discon_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result)
1331 {
1332         ng_l2cap_p               l2cap = ch->con->l2cap;
1333         struct ng_mesg          *msg = NULL;
1334         ng_l2cap_l2ca_discon_op *op = NULL;
1335         int                      error = 0;
1336
1337         /* Check if upstream hook is connected and valid */
1338         if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
1339                 NG_L2CAP_ERR(
1340 "%s: %s - unable to send L2CA_Disconnect response message. " \
1341 "Hook is not connected or valid, psm=%d\n",
1342                         __func__, NG_NODE_NAME(l2cap->node), ch->psm);
1343
1344                 return (ENOTCONN);
1345         }
1346
1347         /* Create and send L2CA_Disconnect response message */
1348         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON,
1349                 sizeof(*op), M_NOWAIT);
1350         if (msg == NULL)
1351                 error = ENOMEM;
1352         else {
1353                 msg->header.token = token;
1354                 msg->header.flags |= NGF_RESP;
1355
1356                 op = (ng_l2cap_l2ca_discon_op *)(msg->data);
1357                 op->result = result;
1358
1359                 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
1360         }
1361
1362         return (error);
1363 } /* ng_l2cap_l2ca_discon_rsp */
1364
1365 /*
1366  * Send L2CA_DisconnectInd message to the upper layer protocol.
1367  */
1368
1369 int
1370 ng_l2cap_l2ca_discon_ind(ng_l2cap_chan_p ch)
1371 {
1372         ng_l2cap_p                       l2cap = ch->con->l2cap;
1373         struct ng_mesg                  *msg = NULL;
1374         ng_l2cap_l2ca_discon_ind_ip     *ip = NULL;
1375         int                              error = 0;
1376
1377         /* Check if upstream hook is connected and valid */
1378         if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
1379                 NG_L2CAP_ERR(
1380 "%s: %s - unable to send L2CA_DisconnectInd message. " \
1381 "Hook is not connected or valid, psm=%d\n",
1382                         __func__, NG_NODE_NAME(l2cap->node), ch->psm);
1383
1384                 return (ENOTCONN);
1385         }
1386
1387         /* Create and send L2CA_DisconnectInd message */
1388         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON_IND,
1389                 sizeof(*ip), M_NOWAIT);
1390         if (msg == NULL)
1391                 error = ENOMEM;
1392         else {
1393                 ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data);
1394                 ip->idtype = ch->idtype;
1395                 if(ch->idtype == NG_L2CAP_L2CA_IDTYPE_ATT||
1396                    ch->idtype == NG_L2CAP_L2CA_IDTYPE_SMP)
1397                         ip->lcid = ch->con->con_handle;
1398                 else
1399                         ip->lcid = ch->scid;
1400
1401                 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
1402         }
1403
1404         return (error);
1405 } /* ng_l2cap_l2ca_discon_ind */
1406
1407 /*
1408  * Process L2CA_GroupCreate request from the upper layer protocol.
1409  * XXX FIXME
1410  */
1411
1412 int
1413 ng_l2cap_l2ca_grp_create(ng_l2cap_p l2cap, struct ng_mesg *msg)
1414 {
1415         return (ENOTSUP);
1416 } /* ng_l2cap_l2ca_grp_create */
1417
1418 /*
1419  * Process L2CA_GroupClose request from the upper layer protocol
1420  * XXX FIXME
1421  */
1422
1423 int
1424 ng_l2cap_l2ca_grp_close(ng_l2cap_p l2cap, struct ng_mesg *msg)
1425 {
1426         return (ENOTSUP);
1427 } /* ng_l2cap_l2ca_grp_close */
1428
1429 /*
1430  * Process L2CA_GroupAddMember request from the upper layer protocol.
1431  * XXX FIXME
1432  */
1433
1434 int
1435 ng_l2cap_l2ca_grp_add_member_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
1436 {
1437         return (ENOTSUP);
1438 } /* ng_l2cap_l2ca_grp_add_member_req */
1439
1440 /*
1441  * Send L2CA_GroupAddMember response to the upper layer protocol.
1442  * XXX FIXME
1443  */
1444
1445 int
1446 ng_l2cap_l2ca_grp_add_member_rsp(ng_l2cap_chan_p ch, u_int32_t token,
1447                 u_int16_t result)
1448 {
1449         return (0);
1450 } /* ng_l2cap_l2ca_grp_add_member_rsp */
1451
1452 /*
1453  * Process L2CA_GroupDeleteMember request from the upper layer protocol
1454  * XXX FIXME
1455  */
1456
1457 int
1458 ng_l2cap_l2ca_grp_rem_member(ng_l2cap_p l2cap, struct ng_mesg *msg)
1459 {
1460         return (ENOTSUP);
1461 } /* ng_l2cap_l2ca_grp_rem_member */
1462
1463 /*
1464  * Process L2CA_GroupGetMembers request from the upper layer protocol
1465  * XXX FIXME
1466  */
1467
1468 int
1469 ng_l2cap_l2ca_grp_get_members(ng_l2cap_p l2cap, struct ng_mesg *msg)
1470 {
1471         return (ENOTSUP);
1472 } /* ng_l2cap_l2ca_grp_get_members */
1473
1474 /*
1475  * Process L2CA_Ping request from the upper layer protocol
1476  */
1477
1478 int
1479 ng_l2cap_l2ca_ping_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
1480 {
1481         ng_l2cap_l2ca_ping_ip   *ip = NULL;
1482         ng_l2cap_con_p           con = NULL;
1483         ng_l2cap_cmd_p           cmd = NULL;
1484         int                      error = 0;
1485
1486         /* Verify message */
1487         if (msg->header.arglen < sizeof(*ip)) {
1488                 NG_L2CAP_ALERT(
1489 "%s: %s - invalid L2CA_Ping request message size, size=%d\n",
1490                         __func__, NG_NODE_NAME(l2cap->node),
1491                         msg->header.arglen);
1492                 error = EMSGSIZE;
1493                 goto out;
1494         }
1495
1496         ip = (ng_l2cap_l2ca_ping_ip *)(msg->data);
1497         if (ip->echo_size > NG_L2CAP_MAX_ECHO_SIZE) {
1498                 NG_L2CAP_WARN(
1499 "%s: %s - invalid L2CA_Ping request. Echo size is too big, echo_size=%d\n",
1500                         __func__, NG_NODE_NAME(l2cap->node), ip->echo_size);
1501                 error = EMSGSIZE;
1502                 goto out;
1503         }
1504
1505         /* Check if we have connection to the unit */
1506         con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr, NG_HCI_LINK_ACL);
1507         if (con == NULL) {
1508                 /* Submit LP_ConnectReq to the lower layer */
1509           error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr, NG_HCI_LINK_ACL);
1510                 if (error != 0) {
1511                         NG_L2CAP_ERR(
1512 "%s: %s - unable to send LP_ConnectReq message, error=%d\n",
1513                                 __func__, NG_NODE_NAME(l2cap->node), error);
1514                         goto out;
1515                 }
1516
1517                 /* This should not fail */
1518                 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr, NG_HCI_LINK_ACL);
1519                 KASSERT((con != NULL),
1520 ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
1521         }
1522
1523         /* Create L2CAP command descriptor */
1524         cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con),
1525                         NG_L2CAP_ECHO_REQ, msg->header.token);
1526         if (cmd == NULL) {
1527                 error = ENOMEM;
1528                 goto out;
1529         }
1530
1531         if (cmd->ident == NG_L2CAP_NULL_IDENT) {
1532                 ng_l2cap_free_cmd(cmd);
1533                 error = EIO;
1534                 goto out;
1535         }
1536
1537         /* Create L2CAP command packet */
1538         _ng_l2cap_echo_req(cmd->aux, cmd->ident,
1539                         msg->data + sizeof(*ip), ip->echo_size);
1540         if (cmd->aux == NULL) {
1541                 ng_l2cap_free_cmd(cmd);
1542                 error = ENOBUFS;
1543                 goto out;
1544         }
1545
1546         /* Link command to the queue */
1547         ng_l2cap_link_cmd(con, cmd);
1548         ng_l2cap_lp_deliver(con);
1549 out:
1550         return (error);
1551 } /* ng_l2cap_l2ca_ping_req */
1552
1553 /*
1554  * Send L2CA_Ping response to the upper layer protocol
1555  */
1556
1557 int
1558 ng_l2cap_l2ca_ping_rsp(ng_l2cap_con_p con, u_int32_t token, u_int16_t result,
1559                 struct mbuf *data)
1560 {
1561         ng_l2cap_p               l2cap = con->l2cap;
1562         struct ng_mesg          *msg = NULL;
1563         ng_l2cap_l2ca_ping_op   *op = NULL;
1564         int                      error = 0, size = 0;
1565
1566         /* Check if control hook is connected and valid */
1567         if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) {
1568                 NG_L2CAP_WARN(
1569 "%s: %s - unable to send L2CA_Ping response message. " \
1570 "Hook is not connected or valid\n",
1571                         __func__, NG_NODE_NAME(l2cap->node));
1572                 error = ENOTCONN;
1573                 goto out;
1574         }
1575
1576         size = (data == NULL)? 0 : data->m_pkthdr.len;
1577
1578         /* Create and send L2CA_Ping response message */
1579         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_PING,
1580                 sizeof(*op) + size, M_NOWAIT);
1581         if (msg == NULL)
1582                 error = ENOMEM;
1583         else {
1584                 msg->header.token = token;
1585                 msg->header.flags |= NGF_RESP;
1586
1587                 op = (ng_l2cap_l2ca_ping_op *)(msg->data);
1588                 op->result = result;
1589                 bcopy(&con->remote, &op->bdaddr, sizeof(op->bdaddr));
1590                 if (data != NULL && size > 0) {
1591                         op->echo_size = size;
1592                         m_copydata(data, 0, size, (caddr_t) op + sizeof(*op));
1593                 }
1594
1595                 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0);
1596         }
1597 out:
1598         NG_FREE_M(data);
1599
1600         return (error);
1601 } /* ng_l2cap_l2ca_ping_rsp */
1602
1603 /*
1604  * Process L2CA_GetInfo request from the upper layer protocol
1605  */
1606
1607 int
1608 ng_l2cap_l2ca_get_info_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
1609 {
1610         ng_l2cap_l2ca_get_info_ip       *ip = NULL;
1611         ng_l2cap_con_p                   con = NULL;
1612         ng_l2cap_cmd_p                   cmd = NULL;
1613         int                              error = 0;
1614
1615         /* Verify message */
1616         if (msg->header.arglen != sizeof(*ip)) {
1617                 NG_L2CAP_ALERT(
1618 "%s: %s - invalid L2CA_GetInfo request message size, size=%d\n",
1619                         __func__, NG_NODE_NAME(l2cap->node),
1620                         msg->header.arglen);
1621                 error = EMSGSIZE;
1622                 goto out;
1623         }
1624
1625         ip = (ng_l2cap_l2ca_get_info_ip *)(msg->data);
1626
1627         /* Check if we have connection to the unit */
1628         con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr,ip->linktype);
1629         if (con == NULL) {
1630                 /* Submit LP_ConnectReq to the lower layer */
1631                 error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr,ip->linktype);
1632                 if (error != 0) {
1633                         NG_L2CAP_ERR(
1634 "%s: %s - unable to send LP_ConnectReq message, error=%d\n",
1635                                 __func__, NG_NODE_NAME(l2cap->node), error);
1636                         goto out;
1637                 }
1638
1639                 /* This should not fail */
1640                 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr, ip->linktype);
1641                 KASSERT((con != NULL),
1642 ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
1643         }
1644
1645         /* Create L2CAP command descriptor */
1646         cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con),
1647                         NG_L2CAP_INFO_REQ, msg->header.token);
1648         if (cmd == NULL) {
1649                 error = ENOMEM;
1650                 goto out;
1651         }
1652
1653         if (cmd->ident == NG_L2CAP_NULL_IDENT) {
1654                 ng_l2cap_free_cmd(cmd);
1655                 error = EIO;
1656                 goto out;
1657         }
1658
1659         /* Create L2CAP command packet */
1660         _ng_l2cap_info_req(cmd->aux, cmd->ident, ip->info_type);
1661         if (cmd->aux == NULL) {
1662                 ng_l2cap_free_cmd(cmd);
1663                 error = ENOBUFS;
1664                 goto out;
1665         }
1666
1667         /* Link command to the queue */
1668         ng_l2cap_link_cmd(con, cmd);
1669         ng_l2cap_lp_deliver(con);
1670 out:
1671         return (error);
1672 } /* ng_l2cap_l2ca_get_info_req */
1673
1674 /*
1675  * Send L2CA_GetInfo response to the upper layer protocol
1676  */
1677
1678 int
1679 ng_l2cap_l2ca_get_info_rsp(ng_l2cap_con_p con, u_int32_t token,
1680                 u_int16_t result, struct mbuf *data)
1681 {
1682         ng_l2cap_p                       l2cap = con->l2cap;
1683         struct ng_mesg                  *msg = NULL;
1684         ng_l2cap_l2ca_get_info_op       *op = NULL;
1685         int                              error = 0, size;
1686
1687         /* Check if control hook is connected and valid */
1688         if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) {
1689                 NG_L2CAP_WARN(
1690 "%s: %s - unable to send L2CA_GetInfo response message. " \
1691 "Hook is not connected or valid\n",
1692                         __func__, NG_NODE_NAME(l2cap->node));
1693                 error = ENOTCONN;
1694                 goto out;
1695         }
1696
1697         size = (data == NULL)? 0 : data->m_pkthdr.len;
1698
1699         /* Create and send L2CA_GetInfo response message */
1700         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_GET_INFO,
1701                 sizeof(*op) + size, M_NOWAIT);
1702         if (msg == NULL)
1703                 error = ENOMEM;
1704         else {
1705                 msg->header.token = token;
1706                 msg->header.flags |= NGF_RESP;
1707
1708                 op = (ng_l2cap_l2ca_get_info_op *)(msg->data);
1709                 op->result = result;
1710                 if (data != NULL && size > 0) {
1711                         op->info_size = size;
1712                         m_copydata(data, 0, size, (caddr_t) op + sizeof(*op));
1713                 }
1714
1715                 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0);
1716         }
1717 out:
1718         NG_FREE_M(data);
1719
1720         return (error);
1721 } /* ng_l2cap_l2ca_get_info_rsp */
1722
1723 /*
1724  * Process L2CA_EnableCLT message from the upper layer protocol
1725  * XXX convert to NGN_L2CAP_NODE_SET_FLAGS?
1726  */
1727
1728 int
1729 ng_l2cap_l2ca_enable_clt(ng_l2cap_p l2cap, struct ng_mesg *msg)
1730 {
1731         ng_l2cap_l2ca_enable_clt_ip     *ip = NULL;
1732         int                              error = 0;
1733 #if 0
1734  *      ng_l2cap_l2ca_enable_clt_op     *op = NULL;
1735  *      u_int16_t                        result;
1736  *      u_int32_t                        token;
1737 #endif
1738
1739         /* Check message */
1740         if (msg->header.arglen != sizeof(*ip)) {
1741                 NG_L2CAP_ALERT(
1742 "%s: %s - invalid L2CA_EnableCLT message size, size=%d\n",
1743                         __func__, NG_NODE_NAME(l2cap->node),
1744                         msg->header.arglen);
1745
1746                 return (EMSGSIZE);
1747         }
1748
1749         /* Process request */
1750         ip = (ng_l2cap_l2ca_enable_clt_ip *) (msg->data);
1751 #if 0
1752  *      result = NG_L2CAP_SUCCESS;
1753 #endif
1754
1755         switch (ip->psm)
1756         {
1757         case 0:
1758                 /* Special case: disable/enable all PSM */
1759                 if (ip->enable)
1760                         l2cap->flags &= ~(NG_L2CAP_CLT_SDP_DISABLED    |
1761                                           NG_L2CAP_CLT_RFCOMM_DISABLED |
1762                                           NG_L2CAP_CLT_TCP_DISABLED);
1763                 else
1764                         l2cap->flags |= (NG_L2CAP_CLT_SDP_DISABLED    |
1765                                          NG_L2CAP_CLT_RFCOMM_DISABLED |
1766                                          NG_L2CAP_CLT_TCP_DISABLED);
1767                 break;
1768
1769         case NG_L2CAP_PSM_SDP:
1770                 if (ip->enable)
1771                         l2cap->flags &= ~NG_L2CAP_CLT_SDP_DISABLED;
1772                 else
1773                         l2cap->flags |= NG_L2CAP_CLT_SDP_DISABLED;
1774                 break;
1775
1776         case NG_L2CAP_PSM_RFCOMM:
1777                 if (ip->enable)
1778                         l2cap->flags &= ~NG_L2CAP_CLT_RFCOMM_DISABLED;
1779                 else
1780                         l2cap->flags |= NG_L2CAP_CLT_RFCOMM_DISABLED;
1781                 break;
1782
1783         case NG_L2CAP_PSM_TCP:
1784                 if (ip->enable)
1785                         l2cap->flags &= ~NG_L2CAP_CLT_TCP_DISABLED;
1786                 else
1787                         l2cap->flags |= NG_L2CAP_CLT_TCP_DISABLED;
1788                 break;
1789
1790         default:
1791                 NG_L2CAP_ERR(
1792 "%s: %s - unsupported PSM=%d\n", __func__, NG_NODE_NAME(l2cap->node), ip->psm);
1793 #if 0
1794  *              result = NG_L2CAP_PSM_NOT_SUPPORTED;
1795 #endif
1796                 error = ENOTSUP;
1797                 break;
1798         }
1799
1800 #if 0
1801  *      /* Create and send response message */
1802  *      token = msg->header.token;
1803  *      NG_FREE_MSG(msg);
1804  *      NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_ENABLE_CLT,
1805  *              sizeof(*op), M_NOWAIT);
1806  *      if (msg == NULL)
1807  *              error = ENOMEM;
1808  *      else {
1809  *              msg->header.token = token;
1810  *              msg->header.flags |= NGF_RESP;
1811  *
1812  *              op = (ng_l2cap_l2ca_enable_clt_op *)(msg->data);
1813  *              op->result = result;
1814  *      }
1815  *
1816  *      /* Send response to control hook */
1817  *      if (l2cap->ctl != NULL && NG_HOOK_IS_VALID(l2cap->ctl))
1818  *              NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0);
1819 #endif
1820
1821         return (error);
1822 } /* ng_l2cap_l2ca_enable_clt */
1823