sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c

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