sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c

   1 /*
   2  * ng_l2cap_evnt.c
   3  */
   4
   5 /*-
   6  * SPDX-License-Identifier: BSD-2-Clause
   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_evnt.c,v 1.5 2003/09/08 19:11:45 max Exp $
  33  */
  34
  35 #include <sys/param.h>
  36 #include <sys/systm.h>
  37 #include <sys/kernel.h>
  38 #include <sys/endian.h>
  39 #include <sys/malloc.h>
  40 #include <sys/mbuf.h>
  41 #include <sys/queue.h>
  42 #include <netgraph/ng_message.h>
  43 #include <netgraph/netgraph.h>
  44 #include <netgraph/bluetooth/include/ng_bluetooth.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  **                    L2CAP events processing module
  57  ******************************************************************************
  58  ******************************************************************************/
  59
  60 static int ng_l2cap_process_signal_cmd (ng_l2cap_con_p);
  61 static int ng_l2cap_process_lesignal_cmd (ng_l2cap_con_p);
  62 static int ng_l2cap_process_cmd_rej    (ng_l2cap_con_p, u_int8_t);
  63 static int ng_l2cap_process_cmd_urq    (ng_l2cap_con_p, u_int8_t);
  64 static int ng_l2cap_process_cmd_urs    (ng_l2cap_con_p, u_int8_t);
  65 static int ng_l2cap_process_con_req    (ng_l2cap_con_p, u_int8_t);
  66 static int ng_l2cap_process_con_rsp    (ng_l2cap_con_p, u_int8_t);
  67 static int ng_l2cap_process_cfg_req    (ng_l2cap_con_p, u_int8_t);
  68 static int ng_l2cap_process_cfg_rsp    (ng_l2cap_con_p, u_int8_t);
  69 static int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t);
  70 static int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t);
  71 static int ng_l2cap_process_echo_req   (ng_l2cap_con_p, u_int8_t);
  72 static int ng_l2cap_process_echo_rsp   (ng_l2cap_con_p, u_int8_t);
  73 static int ng_l2cap_process_info_req   (ng_l2cap_con_p, u_int8_t);
  74 static int ng_l2cap_process_info_rsp   (ng_l2cap_con_p, u_int8_t);
  75 static int send_l2cap_reject
  76         (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t);
  77 static int send_l2cap_con_rej
  78         (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t);
  79 static int send_l2cap_cfg_rsp
  80         (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *);
  81 static int send_l2cap_param_urs
  82        (ng_l2cap_con_p , u_int8_t , u_int16_t);
  83
  84 static int get_next_l2cap_opt
  85         (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p);
  86
  87 /*
  88  * Receive L2CAP packet. First get L2CAP header and verify packet. Than
  89  * get destination channel and process packet.
  90  */
  91
  92 int
  93 ng_l2cap_receive(ng_l2cap_con_p con)
  94 {
  95         ng_l2cap_p       l2cap = con->l2cap;
  96         ng_l2cap_hdr_t  *hdr = NULL;
  97         int              error = 0;
  98
  99         /* Check packet */
 100         if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
 101                 NG_L2CAP_ERR(
 102 "%s: %s - invalid L2CAP packet. Packet too small, len=%d\n",
 103                         __func__, NG_NODE_NAME(l2cap->node),
 104                         con->rx_pkt->m_pkthdr.len);
 105                 error = EMSGSIZE;
 106                 goto drop;
 107         }
 108
 109         /* Get L2CAP header */
 110         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
 111         if (con->rx_pkt == NULL)
 112                 return (ENOBUFS);
 113
 114         hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *);
 115         hdr->length = le16toh(hdr->length);
 116         hdr->dcid = le16toh(hdr->dcid);
 117
 118         /* Check payload size */
 119         if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) {
 120                 NG_L2CAP_ERR(
 121 "%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%zd\n",
 122                         __func__, NG_NODE_NAME(l2cap->node), hdr->length,
 123                         con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
 124                 error = EMSGSIZE;
 125                 goto drop;
 126         }
 127
 128         /* Process packet */
 129         switch (hdr->dcid) {
 130         case NG_L2CAP_SIGNAL_CID: /* L2CAP command */
 131                 m_adj(con->rx_pkt, sizeof(*hdr));
 132                 error = ng_l2cap_process_signal_cmd(con);
 133                 break;
 134         case NG_L2CAP_LESIGNAL_CID:
 135                 m_adj(con->rx_pkt, sizeof(*hdr));
 136                 error = ng_l2cap_process_lesignal_cmd(con);
 137                 break;
 138         case NG_L2CAP_CLT_CID: /* Connectionless packet */
 139                 error = ng_l2cap_l2ca_clt_receive(con);
 140                 break;
 141
 142         default: /* Data packet */
 143                 error = ng_l2cap_l2ca_receive(con);
 144                 break;
 145         }
 146
 147         return (error);
 148 drop:
 149         NG_FREE_M(con->rx_pkt);
 150
 151         return (error);
 152 } /* ng_l2cap_receive */
 153
 154 /*
 155  * Process L2CAP signaling command. We already know that destination channel ID
 156  * is 0x1 that means we have received signaling command from peer's L2CAP layer.
 157  * So get command header, decode and process it.
 158  *
 159  * XXX do we need to check signaling MTU here?
 160  */
 161
 162 static int
 163 ng_l2cap_process_signal_cmd(ng_l2cap_con_p con)
 164 {
 165         ng_l2cap_p               l2cap = con->l2cap;
 166         ng_l2cap_cmd_hdr_t      *hdr = NULL;
 167         struct mbuf             *m = NULL;
 168
 169         while (con->rx_pkt != NULL) {
 170                 /* Verify packet length */
 171                 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
 172                         NG_L2CAP_ERR(
 173 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n",
 174                                 __func__, NG_NODE_NAME(l2cap->node),
 175                                 con->rx_pkt->m_pkthdr.len);
 176                         NG_FREE_M(con->rx_pkt);
 177
 178                         return (EMSGSIZE);
 179                 }
 180
 181                 /* Get signaling command */
 182                 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
 183                 if (con->rx_pkt == NULL)
 184                         return (ENOBUFS);
 185
 186                 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
 187                 hdr->length = le16toh(hdr->length);
 188                 m_adj(con->rx_pkt, sizeof(*hdr));
 189
 190                 /* Verify command length */
 191                 if (con->rx_pkt->m_pkthdr.len < hdr->length) {
 192                         NG_L2CAP_ERR(
 193 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \
 194 "Invalid command length=%d, m_pkthdr.len=%d\n",
 195                                 __func__, NG_NODE_NAME(l2cap->node),
 196                                 hdr->code, hdr->ident, hdr->length,
 197                                 con->rx_pkt->m_pkthdr.len);
 198                         NG_FREE_M(con->rx_pkt);
 199
 200                         return (EMSGSIZE);
 201                 }
 202
 203                 /* Get the command, save the rest (if any) */
 204                 if (con->rx_pkt->m_pkthdr.len > hdr->length)
 205                         m = m_split(con->rx_pkt, hdr->length, M_NOWAIT);
 206                 else
 207                         m = NULL;
 208
 209                 /* Process command */
 210                 switch (hdr->code) {
 211                 case NG_L2CAP_CMD_REJ:
 212                         ng_l2cap_process_cmd_rej(con, hdr->ident);
 213                         break;
 214
 215                 case NG_L2CAP_CON_REQ:
 216                         ng_l2cap_process_con_req(con, hdr->ident);
 217                         break;
 218
 219                 case NG_L2CAP_CON_RSP:
 220                         ng_l2cap_process_con_rsp(con, hdr->ident);
 221                         break;
 222
 223                 case NG_L2CAP_CFG_REQ:
 224                         ng_l2cap_process_cfg_req(con, hdr->ident);
 225                         break;
 226
 227                 case NG_L2CAP_CFG_RSP:
 228                         ng_l2cap_process_cfg_rsp(con, hdr->ident);
 229                         break;
 230
 231                 case NG_L2CAP_DISCON_REQ:
 232                         ng_l2cap_process_discon_req(con, hdr->ident);
 233                         break;
 234
 235                 case NG_L2CAP_DISCON_RSP:
 236                         ng_l2cap_process_discon_rsp(con, hdr->ident);
 237                         break;
 238
 239                 case NG_L2CAP_ECHO_REQ:
 240                         ng_l2cap_process_echo_req(con, hdr->ident);
 241                         break;
 242
 243                 case NG_L2CAP_ECHO_RSP:
 244                         ng_l2cap_process_echo_rsp(con, hdr->ident);
 245                         break;
 246
 247                 case NG_L2CAP_INFO_REQ:
 248                         ng_l2cap_process_info_req(con, hdr->ident);
 249                         break;
 250
 251                 case NG_L2CAP_INFO_RSP:
 252                         ng_l2cap_process_info_rsp(con, hdr->ident);
 253                         break;
 254
 255                 default:
 256                         NG_L2CAP_ERR(
 257 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n",
 258                                 __func__, NG_NODE_NAME(l2cap->node),
 259                                 hdr->code, hdr->ident, hdr->length);
 260
 261                         /*
 262                          * Send L2CAP_CommandRej. Do not really care
 263                          * about the result
 264                          */
 265
 266                         send_l2cap_reject(con, hdr->ident,
 267                                 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0);
 268                         NG_FREE_M(con->rx_pkt);
 269                         break;
 270                 }
 271
 272                 con->rx_pkt = m;
 273         }
 274
 275         return (0);
 276 } /* ng_l2cap_process_signal_cmd */
 277 static int
 278 ng_l2cap_process_lesignal_cmd(ng_l2cap_con_p con)
 279 {
 280         ng_l2cap_p               l2cap = con->l2cap;
 281         ng_l2cap_cmd_hdr_t      *hdr = NULL;
 282         struct mbuf             *m = NULL;
 283
 284         while (con->rx_pkt != NULL) {
 285                 /* Verify packet length */
 286                 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
 287                         NG_L2CAP_ERR(
 288 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n",
 289                                 __func__, NG_NODE_NAME(l2cap->node),
 290                                 con->rx_pkt->m_pkthdr.len);
 291                         NG_FREE_M(con->rx_pkt);
 292
 293                         return (EMSGSIZE);
 294                 }
 295
 296                 /* Get signaling command */
 297                 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
 298                 if (con->rx_pkt == NULL)
 299                         return (ENOBUFS);
 300
 301                 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
 302                 hdr->length = le16toh(hdr->length);
 303                 m_adj(con->rx_pkt, sizeof(*hdr));
 304
 305                 /* Verify command length */
 306                 if (con->rx_pkt->m_pkthdr.len < hdr->length) {
 307                         NG_L2CAP_ERR(
 308 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \
 309 "Invalid command length=%d, m_pkthdr.len=%d\n",
 310                                 __func__, NG_NODE_NAME(l2cap->node),
 311                                 hdr->code, hdr->ident, hdr->length,
 312                                 con->rx_pkt->m_pkthdr.len);
 313                         NG_FREE_M(con->rx_pkt);
 314
 315                         return (EMSGSIZE);
 316                 }
 317
 318                 /* Get the command, save the rest (if any) */
 319                 if (con->rx_pkt->m_pkthdr.len > hdr->length)
 320                         m = m_split(con->rx_pkt, hdr->length, M_NOWAIT);
 321                 else
 322                         m = NULL;
 323
 324                 /* Process command */
 325                 switch (hdr->code) {
 326                 case NG_L2CAP_CMD_REJ:
 327                         ng_l2cap_process_cmd_rej(con, hdr->ident);
 328                         break;
 329                 case NG_L2CAP_CMD_PARAM_UPDATE_REQUEST:
 330                         ng_l2cap_process_cmd_urq(con, hdr->ident);
 331                         break;
 332                 case NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE:
 333                         ng_l2cap_process_cmd_urs(con, hdr->ident);
 334                         break;
 335
 336
 337                 default:
 338                         NG_L2CAP_ERR(
 339 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n",
 340                                 __func__, NG_NODE_NAME(l2cap->node),
 341                                 hdr->code, hdr->ident, hdr->length);
 342
 343                         /*
 344                          * Send L2CAP_CommandRej. Do not really care
 345                          * about the result
 346                          */
 347
 348                         send_l2cap_reject(con, hdr->ident,
 349                                 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0);
 350                         NG_FREE_M(con->rx_pkt);
 351                         break;
 352                 }
 353
 354                 con->rx_pkt = m;
 355         }
 356
 357         return (0);
 358 } /* ng_l2cap_process_signal_cmd */
 359 /*Update Paramater Request*/
 360 static int ng_l2cap_process_cmd_urq(ng_l2cap_con_p con, uint8_t ident)
 361 {
 362         /* We do not implement parameter negotiation for now. */
 363         send_l2cap_param_urs(con, ident, NG_L2CAP_UPDATE_PARAM_ACCEPT);
 364         NG_FREE_M(con->rx_pkt);
 365         return 0;
 366 }
 367
 368 static int ng_l2cap_process_cmd_urs(ng_l2cap_con_p con, uint8_t ident)
 369 {
 370         /* We only support master side yet .*/
 371         //send_l2cap_reject(con,ident ... );
 372
 373         NG_FREE_M(con->rx_pkt);
 374         return 0;
 375 }
 376
 377 /*
 378  * Process L2CAP_CommandRej command
 379  */
 380
 381 static int
 382 ng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident)
 383 {
 384         ng_l2cap_p               l2cap = con->l2cap;
 385         ng_l2cap_cmd_rej_cp     *cp = NULL;
 386         ng_l2cap_cmd_p           cmd = NULL;
 387
 388         /* Get command parameters */
 389         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
 390         if (con->rx_pkt == NULL)
 391                 return (ENOBUFS);
 392
 393         cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *);
 394         cp->reason = le16toh(cp->reason);
 395
 396         /* Check if we have pending command descriptor */
 397         cmd = ng_l2cap_cmd_by_ident(con, ident);
 398         if (cmd != NULL) {
 399                 /* If command timeout already happened then ignore reject */
 400                 if (ng_l2cap_command_untimeout(cmd) != 0) {
 401                         NG_FREE_M(con->rx_pkt);
 402                         return (ETIMEDOUT);
 403                 }
 404
 405                 ng_l2cap_unlink_cmd(cmd);
 406
 407                 switch (cmd->code) {
 408                 case NG_L2CAP_CON_REQ:
 409                         ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0);
 410                         ng_l2cap_free_chan(cmd->ch);
 411                         break;
 412
 413                 case NG_L2CAP_CFG_REQ:
 414                         ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason);
 415                         break;
 416
 417                 case NG_L2CAP_DISCON_REQ:
 418                         ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason);
 419                         ng_l2cap_free_chan(cmd->ch); /* XXX free channel */
 420                         break;
 421
 422                 case NG_L2CAP_ECHO_REQ:
 423                         ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
 424                                 cp->reason, NULL);
 425                         break;
 426
 427                 case NG_L2CAP_INFO_REQ:
 428                         ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
 429                                 cp->reason, NULL);
 430                         break;
 431
 432                 default:
 433                         NG_L2CAP_ALERT(
 434 "%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n",
 435                                 __func__, NG_NODE_NAME(l2cap->node), cmd->code);
 436                         break;
 437                 }
 438
 439                 ng_l2cap_free_cmd(cmd);
 440         } else
 441                 NG_L2CAP_ERR(
 442 "%s: %s - unexpected L2CAP_CommandRej command. " \
 443 "Requested ident does not exist, ident=%d\n",
 444                         __func__, NG_NODE_NAME(l2cap->node), ident);
 445
 446         NG_FREE_M(con->rx_pkt);
 447
 448         return (0);
 449 } /* ng_l2cap_process_cmd_rej */
 450
 451 /*
 452  * Process L2CAP_ConnectReq command
 453  */
 454
 455 static int
 456 ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident)
 457 {
 458         ng_l2cap_p               l2cap = con->l2cap;
 459         struct mbuf             *m = con->rx_pkt;
 460         ng_l2cap_con_req_cp     *cp = NULL;
 461         ng_l2cap_chan_p          ch = NULL;
 462         int                      error = 0;
 463         u_int16_t                dcid, psm;
 464         int idtype;
 465
 466         /* Get command parameters */
 467         NG_L2CAP_M_PULLUP(m, sizeof(*cp));
 468         if (m == NULL)
 469                 return (ENOBUFS);
 470
 471         cp = mtod(m, ng_l2cap_con_req_cp *);
 472         psm = le16toh(cp->psm);
 473         dcid = le16toh(cp->scid);
 474
 475         NG_FREE_M(m);
 476         con->rx_pkt = NULL;
 477         if(dcid == NG_L2CAP_ATT_CID)
 478                 idtype = NG_L2CAP_L2CA_IDTYPE_ATT;
 479         else if(dcid == NG_L2CAP_SMP_CID)
 480                 idtype = NG_L2CAP_L2CA_IDTYPE_SMP;
 481         else if( con->linktype != NG_HCI_LINK_ACL)
 482                 idtype = NG_L2CAP_L2CA_IDTYPE_LE;
 483         else
 484                 idtype = NG_L2CAP_L2CA_IDTYPE_BREDR;
 485
 486         /*
 487          * Create new channel and send L2CA_ConnectInd notification
 488          * to the upper layer protocol.
 489          */
 490
 491         ch = ng_l2cap_new_chan(l2cap, con, psm, idtype);
 492
 493         if (ch == NULL)
 494                 return (send_l2cap_con_rej(con, ident, 0, dcid,
 495                                 NG_L2CAP_NO_RESOURCES));
 496
 497         /* Update channel IDs */
 498         ch->dcid = dcid;
 499
 500         /* Sent L2CA_ConnectInd notification to the upper layer */
 501         ch->ident = ident;
 502         ch->state = NG_L2CAP_W4_L2CA_CON_RSP;
 503
 504         error = ng_l2cap_l2ca_con_ind(ch);
 505         if (error != 0) {
 506                 send_l2cap_con_rej(con, ident, ch->scid, dcid,
 507                         (error == ENOMEM)? NG_L2CAP_NO_RESOURCES :
 508                                 NG_L2CAP_PSM_NOT_SUPPORTED);
 509                 ng_l2cap_free_chan(ch);
 510         }
 511
 512         return (error);
 513 } /* ng_l2cap_process_con_req */
 514
 515 /*
 516  * Process L2CAP_ConnectRsp command
 517  */
 518
 519 static int
 520 ng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident)
 521 {
 522         ng_l2cap_p               l2cap = con->l2cap;
 523         struct mbuf             *m = con->rx_pkt;
 524         ng_l2cap_con_rsp_cp     *cp = NULL;
 525         ng_l2cap_cmd_p           cmd = NULL;
 526         u_int16_t                scid, dcid, result, status;
 527         int                      error = 0;
 528
 529         /* Get command parameters */
 530         NG_L2CAP_M_PULLUP(m, sizeof(*cp));
 531         if (m == NULL)
 532                 return (ENOBUFS);
 533
 534         cp = mtod(m, ng_l2cap_con_rsp_cp *);
 535         dcid = le16toh(cp->dcid);
 536         scid = le16toh(cp->scid);
 537         result = le16toh(cp->result);
 538         status = le16toh(cp->status);
 539
 540         NG_FREE_M(m);
 541         con->rx_pkt = NULL;
 542
 543         /* Check if we have pending command descriptor */
 544         cmd = ng_l2cap_cmd_by_ident(con, ident);
 545         if (cmd == NULL) {
 546                 NG_L2CAP_ERR(
 547 "%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n",
 548                         __func__, NG_NODE_NAME(l2cap->node), ident,
 549                         con->con_handle);
 550
 551                 return (ENOENT);
 552         }
 553
 554         /* Verify channel state, if invalid - do nothing */
 555         if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) {
 556                 NG_L2CAP_ERR(
 557 "%s: %s - unexpected L2CAP_ConnectRsp. " \
 558 "Invalid channel state, cid=%d, state=%d\n",
 559                         __func__, NG_NODE_NAME(l2cap->node), scid,
 560                         cmd->ch->state);
 561                 goto reject;
 562         }
 563
 564         /* Verify CIDs and send reject if does not match */
 565         if (cmd->ch->scid != scid) {
 566                 NG_L2CAP_ERR(
 567 "%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n",
 568                          __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
 569                         scid);
 570                 goto reject;
 571         }
 572
 573         /*
 574          * Looks good. We got confirmation from our peer. Now process
 575          * it. First disable RTX timer. Then check the result and send
 576          * notification to the upper layer. If command timeout already
 577          * happened then ignore response.
 578          */
 579
 580         if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
 581                 return (error);
 582
 583         if (result == NG_L2CAP_PENDING) {
 584                 /*
 585                  * Our peer wants more time to complete connection. We shall
 586                  * start ERTX timer and wait. Keep command in the list.
 587                  */
 588
 589                 cmd->ch->dcid = dcid;
 590                 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout());
 591
 592                 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
 593                                 result, status);
 594                 if (error != 0)
 595                         ng_l2cap_free_chan(cmd->ch);
 596         } else {
 597                 ng_l2cap_unlink_cmd(cmd);
 598
 599                 if (result == NG_L2CAP_SUCCESS) {
 600                         /*
 601                          * Channel is open. Complete command and move to CONFIG
 602                          * state. Since we have sent positive confirmation we
 603                          * expect to receive L2CA_Config request from the upper
 604                          * layer protocol.
 605                          */
 606
 607                         cmd->ch->dcid = dcid;
 608                         cmd->ch->state = ((cmd->ch->scid == NG_L2CAP_ATT_CID)||
 609                                           (cmd->ch->scid == NG_L2CAP_SMP_CID))
 610                                           ?
 611                           NG_L2CAP_OPEN : NG_L2CAP_CONFIG;
 612                 } else
 613                         /* There was an error, so close the channel */
 614                         NG_L2CAP_INFO(
 615 "%s: %s - failed to open L2CAP channel, result=%d, status=%d\n",
 616                                 __func__, NG_NODE_NAME(l2cap->node), result,
 617                                 status);
 618
 619                 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
 620                                 result, status);
 621
 622                 /* XXX do we have to remove the channel on error? */
 623                 if (error != 0 || result != NG_L2CAP_SUCCESS)
 624                         ng_l2cap_free_chan(cmd->ch);
 625
 626                 ng_l2cap_free_cmd(cmd);
 627         }
 628
 629         return (error);
 630
 631 reject:
 632         /* Send reject. Do not really care about the result */
 633         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
 634
 635         return (0);
 636 } /* ng_l2cap_process_con_rsp */
 637
 638 /*
 639  * Process L2CAP_ConfigReq command
 640  */
 641
 642 static int
 643 ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident)
 644 {
 645         ng_l2cap_p               l2cap = con->l2cap;
 646         struct mbuf             *m = con->rx_pkt;
 647         ng_l2cap_cfg_req_cp     *cp = NULL;
 648         ng_l2cap_chan_p          ch = NULL;
 649         u_int16_t                dcid, respond, result;
 650         ng_l2cap_cfg_opt_t       hdr;
 651         ng_l2cap_cfg_opt_val_t   val;
 652         int                      off, error = 0;
 653
 654         /* Get command parameters */
 655         con->rx_pkt = NULL;
 656         NG_L2CAP_M_PULLUP(m, sizeof(*cp));
 657         if (m == NULL)
 658                 return (ENOBUFS);
 659
 660         cp = mtod(m, ng_l2cap_cfg_req_cp *);
 661         dcid = le16toh(cp->dcid);
 662         respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
 663         m_adj(m, sizeof(*cp));
 664
 665         /* Check if we have this channel and it is in valid state */
 666         ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
 667         if (ch == NULL) {
 668                 NG_L2CAP_ERR(
 669 "%s: %s - unexpected L2CAP_ConfigReq command. " \
 670 "Channel does not exist, cid=%d\n",
 671                         __func__, NG_NODE_NAME(l2cap->node), dcid);
 672                 goto reject;
 673         }
 674
 675         /* Verify channel state */
 676         if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) {
 677                 NG_L2CAP_ERR(
 678 "%s: %s - unexpected L2CAP_ConfigReq. " \
 679 "Invalid channel state, cid=%d, state=%d\n",
 680                         __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
 681                 goto reject;
 682         }
 683
 684         if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */
 685                 ch->cfg_state = 0;
 686                 ch->state = NG_L2CAP_CONFIG;
 687         }
 688
 689         for (result = 0, off = 0; ; ) {
 690                 error = get_next_l2cap_opt(m, &off, &hdr, &val);
 691                 if (error == 0) { /* We done with this packet */
 692                         NG_FREE_M(m);
 693                         break;
 694                 } else if (error > 0) { /* Got option */
 695                         switch (hdr.type) {
 696                         case NG_L2CAP_OPT_MTU:
 697                                 ch->omtu = val.mtu;
 698                                 break;
 699
 700                         case NG_L2CAP_OPT_FLUSH_TIMO:
 701                                 ch->flush_timo = val.flush_timo;
 702                                 break;
 703
 704                         case NG_L2CAP_OPT_QOS:
 705                                 bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow));
 706                                 break;
 707
 708                         default: /* Ignore unknown hint option */
 709                                 break;
 710                         }
 711                 } else { /* Oops, something is wrong */
 712                         respond = 1;
 713
 714                         if (error == -3) {
 715                                 /*
 716                                  * Adjust mbuf so we can get to the start
 717                                  * of the first option we did not like.
 718                                  */
 719
 720                                 m_adj(m, off - sizeof(hdr));
 721                                 m->m_pkthdr.len = sizeof(hdr) + hdr.length;
 722
 723                                 result = NG_L2CAP_UNKNOWN_OPTION;
 724                         } else {
 725                                 /* XXX FIXME Send other reject codes? */
 726                                 NG_FREE_M(m);
 727                                 result = NG_L2CAP_REJECT;
 728                         }
 729
 730                         break;
 731                 }
 732         }
 733
 734         /*
 735          * Now check and see if we have to respond. If everything was OK then
 736          * respond contain "C flag" and (if set) we will respond with empty
 737          * packet and will wait for more options.
 738          *
 739          * Other case is that we did not like peer's options and will respond
 740          * with L2CAP_Config response command with Reject error code.
 741          *
 742          * When "respond == 0" than we have received all options and we will
 743          * sent L2CA_ConfigInd event to the upper layer protocol.
 744          */
 745
 746         if (respond) {
 747                 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m);
 748                 if (error != 0) {
 749                         ng_l2cap_l2ca_discon_ind(ch);
 750                         ng_l2cap_free_chan(ch);
 751                 }
 752         } else {
 753                 /* Send L2CA_ConfigInd event to the upper layer protocol */
 754                 ch->ident = ident;
 755                 error = ng_l2cap_l2ca_cfg_ind(ch);
 756                 if (error != 0)
 757                         ng_l2cap_free_chan(ch);
 758         }
 759
 760         return (error);
 761
 762 reject:
 763         /* Send reject. Do not really care about the result */
 764         NG_FREE_M(m);
 765
 766         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid);
 767
 768         return (0);
 769 } /* ng_l2cap_process_cfg_req */
 770
 771 /*
 772  * Process L2CAP_ConfigRsp command
 773  */
 774
 775 static int
 776 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident)
 777 {
 778         ng_l2cap_p               l2cap = con->l2cap;
 779         struct mbuf             *m = con->rx_pkt;
 780         ng_l2cap_cfg_rsp_cp     *cp = NULL;
 781         ng_l2cap_cmd_p           cmd = NULL;
 782         u_int16_t                scid, cflag, result;
 783         ng_l2cap_cfg_opt_t       hdr;
 784         ng_l2cap_cfg_opt_val_t   val;
 785         int                      off, error = 0;
 786
 787         /* Get command parameters */
 788         con->rx_pkt = NULL;
 789         NG_L2CAP_M_PULLUP(m, sizeof(*cp));
 790         if (m == NULL)
 791                 return (ENOBUFS);
 792
 793         cp = mtod(m, ng_l2cap_cfg_rsp_cp *);
 794         scid = le16toh(cp->scid);
 795         cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
 796         result = le16toh(cp->result);
 797         m_adj(m, sizeof(*cp));
 798
 799         /* Check if we have this command */
 800         cmd = ng_l2cap_cmd_by_ident(con, ident);
 801         if (cmd == NULL) {
 802                 NG_L2CAP_ERR(
 803 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n",
 804                         __func__, NG_NODE_NAME(l2cap->node), ident,
 805                         con->con_handle);
 806                 NG_FREE_M(m);
 807
 808                 return (ENOENT);
 809         }
 810
 811         /* Verify CIDs and send reject if does not match */
 812         if (cmd->ch->scid != scid) {
 813                 NG_L2CAP_ERR(
 814 "%s: %s - unexpected L2CAP_ConfigRsp. " \
 815 "Channel ID does not match, scid=%d(%d)\n",
 816                         __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
 817                         scid);
 818                 goto reject;
 819         }
 820
 821         /* Verify channel state and reject if invalid */
 822         if (cmd->ch->state != NG_L2CAP_CONFIG) {
 823                 NG_L2CAP_ERR(
 824 "%s: %s - unexpected L2CAP_ConfigRsp. " \
 825 "Invalid channel state, scid=%d, state=%d\n",
 826                         __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
 827                         cmd->ch->state);
 828                 goto reject;
 829         }
 830
 831         /*
 832          * Looks like it is our response, so process it. First parse options,
 833          * then verify C flag. If it is set then we shall expect more
 834          * configuration options from the peer and we will wait. Otherwise we
 835          * have received all options and we will send L2CA_ConfigRsp event to
 836          * the upper layer protocol. If command timeout already happened then
 837          * ignore response.
 838          */
 839
 840         if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
 841                 NG_FREE_M(m);
 842                 return (error);
 843         }
 844
 845         for (off = 0; ; ) {
 846                 error = get_next_l2cap_opt(m, &off, &hdr, &val);
 847                 if (error == 0) /* We done with this packet */
 848                         break;
 849                 else if (error > 0) { /* Got option */
 850                         switch (hdr.type) {
 851                         case NG_L2CAP_OPT_MTU:
 852                                 cmd->ch->imtu = val.mtu;
 853                         break;
 854
 855                         case NG_L2CAP_OPT_FLUSH_TIMO:
 856                                 cmd->ch->flush_timo = val.flush_timo;
 857                                 break;
 858
 859                         case NG_L2CAP_OPT_QOS:
 860                                 bcopy(&val.flow, &cmd->ch->oflow,
 861                                         sizeof(cmd->ch->oflow));
 862                         break;
 863
 864                         default: /* Ignore unknown hint option */
 865                                 break;
 866                         }
 867                 } else {
 868                         /*
 869                          * XXX FIXME What to do here?
 870                          *
 871                          * This is really BAD :( options packet was broken, or
 872                          * peer sent us option that we did not understand. Let
 873                          * upper layer know and do not wait for more options.
 874                          */
 875
 876                         NG_L2CAP_ALERT(
 877 "%s: %s - failed to parse configuration options, error=%d\n",
 878                                 __func__, NG_NODE_NAME(l2cap->node), error);
 879
 880                         result = NG_L2CAP_UNKNOWN;
 881                         cflag = 0;
 882
 883                         break;
 884                 }
 885         }
 886
 887         NG_FREE_M(m);
 888
 889         if (cflag) /* Restart timer and wait for more options */
 890                 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout());
 891         else {
 892                 ng_l2cap_unlink_cmd(cmd);
 893
 894                 /* Send L2CA_Config response to the upper layer protocol */
 895                 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result);
 896                 if (error != 0) {
 897                         /*
 898                          * XXX FIXME what to do here? we were not able to send
 899                          * response to the upper layer protocol, so for now
 900                          * just close the channel. Send L2CAP_Disconnect to
 901                          * remote peer?
 902                          */
 903
 904                         NG_L2CAP_ERR(
 905 "%s: %s - failed to send L2CA_Config response, error=%d\n",
 906                         __func__, NG_NODE_NAME(l2cap->node), error);
 907
 908                         ng_l2cap_free_chan(cmd->ch);
 909                 }
 910
 911                 ng_l2cap_free_cmd(cmd);
 912         }
 913
 914         return (error);
 915
 916 reject:
 917         /* Send reject. Do not really care about the result */
 918         NG_FREE_M(m);
 919
 920         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0);
 921
 922         return (0);
 923 } /* ng_l2cap_process_cfg_rsp */
 924
 925 /*
 926  * Process L2CAP_DisconnectReq command
 927  */
 928
 929 static int
 930 ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident)
 931 {
 932         ng_l2cap_p               l2cap = con->l2cap;
 933         ng_l2cap_discon_req_cp  *cp = NULL;
 934         ng_l2cap_chan_p          ch = NULL;
 935         ng_l2cap_cmd_p           cmd = NULL;
 936         u_int16_t                scid, dcid;
 937
 938         /* Get command parameters */
 939         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
 940         if (con->rx_pkt == NULL)
 941                 return (ENOBUFS);
 942
 943         cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *);
 944         dcid = le16toh(cp->dcid);
 945         scid = le16toh(cp->scid);
 946
 947         NG_FREE_M(con->rx_pkt);
 948
 949         /* Check if we have this channel and it is in valid state */
 950         ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
 951         if (ch == NULL) {
 952                 NG_L2CAP_ERR(
 953 "%s: %s - unexpected L2CAP_DisconnectReq message. " \
 954 "Channel does not exist, cid=%d\n",
 955                         __func__, NG_NODE_NAME(l2cap->node), dcid);
 956                 goto reject;
 957         }
 958
 959         /* XXX Verify channel state and reject if invalid -- is that true? */
 960         if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG &&
 961             ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
 962                 NG_L2CAP_ERR(
 963 "%s: %s - unexpected L2CAP_DisconnectReq. " \
 964 "Invalid channel state, cid=%d, state=%d\n",
 965                         __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
 966                 goto reject;
 967         }
 968
 969         /* Match destination channel ID */
 970         if (ch->dcid != scid || ch->scid != dcid) {
 971                 NG_L2CAP_ERR(
 972 "%s: %s - unexpected L2CAP_DisconnectReq. " \
 973 "Channel IDs does not match, channel: scid=%d, dcid=%d, " \
 974 "request: scid=%d, dcid=%d\n",
 975                         __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid,
 976                         scid, dcid);
 977                 goto reject;
 978         }
 979
 980         /*
 981          * Looks good, so notify upper layer protocol that channel is about
 982          * to be disconnected and send L2CA_DisconnectInd message. Then respond
 983          * with L2CAP_DisconnectRsp.
 984          */
 985
 986         if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
 987                 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */
 988                 ng_l2cap_free_chan(ch);
 989         }
 990
 991         /* Send L2CAP_DisconnectRsp */
 992         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0);
 993         if (cmd == NULL)
 994                 return (ENOMEM);
 995
 996         _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid);
 997         if (cmd->aux == NULL) {
 998                 ng_l2cap_free_cmd(cmd);
 999
1000                 return (ENOBUFS);
1001         }
1002
1003         /* Link command to the queue */
1004         ng_l2cap_link_cmd(con, cmd);
1005         ng_l2cap_lp_deliver(con);
1006
1007         return (0);
1008
1009 reject:
1010         /* Send reject. Do not really care about the result */
1011         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
1012
1013         return (0);
1014 } /* ng_l2cap_process_discon_req */
1015
1016 /*
1017  * Process L2CAP_DisconnectRsp command
1018  */
1019
1020 static int
1021 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident)
1022 {
1023         ng_l2cap_p               l2cap = con->l2cap;
1024         ng_l2cap_discon_rsp_cp  *cp = NULL;
1025         ng_l2cap_cmd_p           cmd = NULL;
1026         u_int16_t                scid, dcid;
1027         int                      error = 0;
1028
1029         /* Get command parameters */
1030         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1031         if (con->rx_pkt == NULL)
1032                 return (ENOBUFS);
1033
1034         cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *);
1035         dcid = le16toh(cp->dcid);
1036         scid = le16toh(cp->scid);
1037
1038         NG_FREE_M(con->rx_pkt);
1039
1040         /* Check if we have pending command descriptor */
1041         cmd = ng_l2cap_cmd_by_ident(con, ident);
1042         if (cmd == NULL) {
1043                 NG_L2CAP_ERR(
1044 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n",
1045                         __func__, NG_NODE_NAME(l2cap->node), ident,
1046                         con->con_handle);
1047                 goto out;
1048         }
1049
1050         /* Verify channel state, do nothing if invalid */
1051         if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
1052                 NG_L2CAP_ERR(
1053 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1054 "Invalid channel state, cid=%d, state=%d\n",
1055                         __func__, NG_NODE_NAME(l2cap->node), scid,
1056                         cmd->ch->state);
1057                 goto out;
1058         }
1059
1060         /* Verify CIDs and send reject if does not match */
1061         if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) {
1062                 NG_L2CAP_ERR(
1063 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1064 "Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n",
1065                         __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
1066                         scid, cmd->ch->dcid, dcid);
1067                 goto out;
1068         }
1069
1070         /*
1071          * Looks like we have successfully disconnected channel, so notify
1072          * upper layer. If command timeout already happened then ignore
1073          * response.
1074          */
1075
1076         if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
1077                 goto out;
1078
1079         error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS);
1080         ng_l2cap_free_chan(cmd->ch); /* this will free commands too */
1081 out:
1082         return (error);
1083 } /* ng_l2cap_process_discon_rsp */
1084
1085 /*
1086  * Process L2CAP_EchoReq command
1087  */
1088
1089 static int
1090 ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident)
1091 {
1092         ng_l2cap_p               l2cap = con->l2cap;
1093         ng_l2cap_cmd_hdr_t      *hdr = NULL;
1094         ng_l2cap_cmd_p           cmd = NULL;
1095
1096         con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr));
1097         if (con->rx_pkt == NULL) {
1098                 NG_L2CAP_ALERT(
1099 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n",
1100                         __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr));
1101
1102                 return (ENOBUFS);
1103         }
1104
1105         hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
1106         hdr->code = NG_L2CAP_ECHO_RSP;
1107         hdr->ident = ident;
1108         hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
1109
1110         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0);
1111         if (cmd == NULL) {
1112                 NG_FREE_M(con->rx_pkt);
1113
1114                 return (ENOBUFS);
1115         }
1116
1117         /* Attach data and link command to the queue */
1118         cmd->aux = con->rx_pkt;
1119         con->rx_pkt = NULL;
1120         ng_l2cap_link_cmd(con, cmd);
1121         ng_l2cap_lp_deliver(con);
1122
1123         return (0);
1124 } /* ng_l2cap_process_echo_req */
1125
1126 /*
1127  * Process L2CAP_EchoRsp command
1128  */
1129
1130 static int
1131 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident)
1132 {
1133         ng_l2cap_p      l2cap = con->l2cap;
1134         ng_l2cap_cmd_p  cmd = NULL;
1135         int             error = 0;
1136
1137         /* Check if we have this command */
1138         cmd = ng_l2cap_cmd_by_ident(con, ident);
1139         if (cmd != NULL) {
1140                 /* If command timeout already happened then ignore response */
1141                 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1142                         NG_FREE_M(con->rx_pkt);
1143                         return (error);
1144                 }
1145
1146                 ng_l2cap_unlink_cmd(cmd);
1147
1148                 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
1149                                 NG_L2CAP_SUCCESS, con->rx_pkt);
1150
1151                 ng_l2cap_free_cmd(cmd);
1152                 con->rx_pkt = NULL;
1153         } else {
1154                 NG_L2CAP_ERR(
1155 "%s: %s - unexpected L2CAP_EchoRsp command. " \
1156 "Requested ident does not exist, ident=%d\n",
1157                         __func__, NG_NODE_NAME(l2cap->node), ident);
1158                 NG_FREE_M(con->rx_pkt);
1159         }
1160
1161         return (error);
1162 } /* ng_l2cap_process_echo_rsp */
1163
1164 /*
1165  * Process L2CAP_InfoReq command
1166  */
1167
1168 static int
1169 ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident)
1170 {
1171         ng_l2cap_p      l2cap = con->l2cap;
1172         ng_l2cap_cmd_p  cmd = NULL;
1173         u_int16_t       type;
1174
1175         /* Get command parameters */
1176         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp));
1177         if (con->rx_pkt == NULL)
1178                 return (ENOBUFS);
1179
1180         type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type);
1181         NG_FREE_M(con->rx_pkt);
1182
1183         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0);
1184         if (cmd == NULL)
1185                 return (ENOMEM);
1186
1187         switch (type) {
1188         case NG_L2CAP_CONNLESS_MTU:
1189                 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU,
1190                                 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT);
1191                 break;
1192
1193         default:
1194                 _ng_l2cap_info_rsp(cmd->aux, ident, type,
1195                                 NG_L2CAP_NOT_SUPPORTED, 0);
1196                 break;
1197         }
1198
1199         if (cmd->aux == NULL) {
1200                 ng_l2cap_free_cmd(cmd);
1201
1202                 return (ENOBUFS);
1203         }
1204
1205         /* Link command to the queue */
1206         ng_l2cap_link_cmd(con, cmd);
1207         ng_l2cap_lp_deliver(con);
1208
1209         return (0);
1210 } /* ng_l2cap_process_info_req */
1211
1212 /*
1213  * Process L2CAP_InfoRsp command
1214  */
1215
1216 static int
1217 ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident)
1218 {
1219         ng_l2cap_p               l2cap = con->l2cap;
1220         ng_l2cap_info_rsp_cp    *cp = NULL;
1221         ng_l2cap_cmd_p           cmd = NULL;
1222         int                      error = 0;
1223
1224         /* Get command parameters */
1225         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1226         if (con->rx_pkt == NULL)
1227                 return (ENOBUFS);
1228
1229         cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *);
1230         cp->type = le16toh(cp->type);
1231         cp->result = le16toh(cp->result);
1232         m_adj(con->rx_pkt, sizeof(*cp));
1233
1234         /* Check if we have pending command descriptor */
1235         cmd = ng_l2cap_cmd_by_ident(con, ident);
1236         if (cmd == NULL) {
1237                 NG_L2CAP_ERR(
1238 "%s: %s - unexpected L2CAP_InfoRsp command. " \
1239 "Requested ident does not exist, ident=%d\n",
1240                         __func__, NG_NODE_NAME(l2cap->node), ident);
1241                 NG_FREE_M(con->rx_pkt);
1242
1243                 return (ENOENT);
1244         }
1245
1246         /* If command timeout already happened then ignore response */
1247         if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1248                 NG_FREE_M(con->rx_pkt);
1249                 return (error);
1250         }
1251
1252         ng_l2cap_unlink_cmd(cmd);
1253
1254         if (cp->result == NG_L2CAP_SUCCESS) {
1255                 switch (cp->type) {
1256                 case NG_L2CAP_CONNLESS_MTU:
1257                         if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t))
1258                                 *mtod(con->rx_pkt, u_int16_t *) =
1259                                         le16toh(*mtod(con->rx_pkt,u_int16_t *));
1260                         else {
1261                                 cp->result = NG_L2CAP_UNKNOWN; /* XXX */
1262
1263                                 NG_L2CAP_ERR(
1264 "%s: %s - invalid L2CAP_InfoRsp command. " \
1265 "Bad connectionless MTU parameter, len=%d\n",
1266                                         __func__, NG_NODE_NAME(l2cap->node),
1267                                         con->rx_pkt->m_pkthdr.len);
1268                         }
1269                         break;
1270
1271                 default:
1272                         NG_L2CAP_WARN(
1273 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n",
1274                                 __func__, NG_NODE_NAME(l2cap->node), cp->type);
1275                         break;
1276                 }
1277         }
1278
1279         error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
1280                         cp->result, con->rx_pkt);
1281
1282         ng_l2cap_free_cmd(cmd);
1283         con->rx_pkt = NULL;
1284
1285         return (error);
1286 } /* ng_l2cap_process_info_rsp */
1287
1288 /*
1289  * Send L2CAP reject
1290  */
1291
1292 static int
1293 send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason,
1294                 u_int16_t mtu, u_int16_t scid, u_int16_t dcid)
1295 {
1296         ng_l2cap_cmd_p  cmd = NULL;
1297
1298         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0);
1299         if (cmd == NULL)
1300                 return (ENOMEM);
1301
1302          _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid);
1303         if (cmd->aux == NULL) {
1304                 ng_l2cap_free_cmd(cmd);
1305
1306                 return (ENOBUFS);
1307         }
1308
1309         /* Link command to the queue */
1310         ng_l2cap_link_cmd(con, cmd);
1311         ng_l2cap_lp_deliver(con);
1312
1313         return (0);
1314 } /* send_l2cap_reject */
1315
1316 /*
1317  * Send L2CAP connection reject
1318  */
1319
1320 static int
1321 send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1322                 u_int16_t dcid, u_int16_t result)
1323 {
1324         ng_l2cap_cmd_p  cmd = NULL;
1325
1326         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0);
1327         if (cmd == NULL)
1328                 return (ENOMEM);
1329
1330         _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0);
1331         if (cmd->aux == NULL) {
1332                 ng_l2cap_free_cmd(cmd);
1333
1334                 return (ENOBUFS);
1335         }
1336
1337         /* Link command to the queue */
1338         ng_l2cap_link_cmd(con, cmd);
1339         ng_l2cap_lp_deliver(con);
1340
1341         return (0);
1342 } /* send_l2cap_con_rej */
1343
1344 /*
1345  * Send L2CAP config response
1346  */
1347
1348 static int
1349 send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1350                 u_int16_t result, struct mbuf *opt)
1351 {
1352         ng_l2cap_cmd_p  cmd = NULL;
1353
1354         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0);
1355         if (cmd == NULL) {
1356                 NG_FREE_M(opt);
1357
1358                 return (ENOMEM);
1359         }
1360
1361         _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt);
1362         if (cmd->aux == NULL) {
1363                 ng_l2cap_free_cmd(cmd);
1364
1365                 return (ENOBUFS);
1366         }
1367
1368         /* Link command to the queue */
1369         ng_l2cap_link_cmd(con, cmd);
1370         ng_l2cap_lp_deliver(con);
1371
1372         return (0);
1373 } /* send_l2cap_cfg_rsp */
1374
1375 static int
1376 send_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident,
1377                      u_int16_t result)
1378 {
1379         ng_l2cap_cmd_p  cmd = NULL;
1380
1381         cmd = ng_l2cap_new_cmd(con, NULL, ident,
1382                                NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE,
1383                                0);
1384         if (cmd == NULL) {
1385                 return (ENOMEM);
1386         }
1387
1388         _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result);
1389         if (cmd->aux == NULL) {
1390                 ng_l2cap_free_cmd(cmd);
1391
1392                 return (ENOBUFS);
1393         }
1394
1395         /* Link command to the queue */
1396         ng_l2cap_link_cmd(con, cmd);
1397         ng_l2cap_lp_deliver(con);
1398
1399         return (0);
1400 } /* send_l2cap_cfg_rsp */
1401
1402 /*
1403  * Get next L2CAP configuration option
1404  *
1405  * Return codes:
1406  *  0   no option
1407  *  1   we have got option
1408  * -1   header too short
1409  * -2   bad option value or length
1410  * -3   unknown option
1411  */
1412
1413 static int
1414 get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr,
1415                 ng_l2cap_cfg_opt_val_p val)
1416 {
1417         int     hint, len = m->m_pkthdr.len - (*off);
1418
1419         if (len == 0)
1420                 return (0);
1421         if (len < 0 || len < sizeof(*hdr))
1422                 return (-1);
1423
1424         m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr);
1425         *off += sizeof(*hdr);
1426         len  -= sizeof(*hdr);
1427
1428         hint = NG_L2CAP_OPT_HINT(hdr->type);
1429         hdr->type &= NG_L2CAP_OPT_HINT_MASK;
1430
1431         switch (hdr->type) {
1432         case NG_L2CAP_OPT_MTU:
1433                 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length)
1434                         return (-2);
1435
1436                 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val);
1437                 val->mtu = le16toh(val->mtu);
1438                 *off += NG_L2CAP_OPT_MTU_SIZE;
1439                 break;
1440
1441         case NG_L2CAP_OPT_FLUSH_TIMO:
1442                 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE ||
1443                     len < hdr->length)
1444                         return (-2);
1445
1446                 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val);
1447                 val->flush_timo = le16toh(val->flush_timo);
1448                 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE;
1449                 break;
1450
1451         case NG_L2CAP_OPT_QOS:
1452                 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length)
1453                         return (-2);
1454
1455                 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val);
1456                 val->flow.token_rate = le32toh(val->flow.token_rate);
1457                 val->flow.token_bucket_size =
1458                                 le32toh(val->flow.token_bucket_size);
1459                 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth);
1460                 val->flow.latency = le32toh(val->flow.latency);
1461                 val->flow.delay_variation = le32toh(val->flow.delay_variation);
1462                 *off += NG_L2CAP_OPT_QOS_SIZE;
1463                 break;
1464
1465         default:
1466                 if (hint)
1467                         *off += hdr->length;
1468                 else
1469                         return (-3);
1470                 break;
1471         }
1472
1473         return (1);
1474 } /* get_next_l2cap_opt */