sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c

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