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                                  * Adjust mbuf so we can get to the start
 718                                  * of the first option we did not like.
 719                                  */
 720
 721                                 m_adj(m, off - sizeof(hdr));
 722                                 m->m_pkthdr.len = sizeof(hdr) + hdr.length;
 723
 724                                 result = NG_L2CAP_UNKNOWN_OPTION;
 725                         } else {
 726                                 /* XXX FIXME Send other reject codes? */
 727                                 NG_FREE_M(m);
 728                                 result = NG_L2CAP_REJECT;
 729                         }
 730
 731                         break;
 732                 }
 733         }
 734
 735         /*
 736          * Now check and see if we have to respond. If everything was OK then
 737          * respond contain "C flag" and (if set) we will respond with empty
 738          * packet and will wait for more options.
 739          *
 740          * Other case is that we did not like peer's options and will respond
 741          * with L2CAP_Config response command with Reject error code.
 742          *
 743          * When "respond == 0" than we have received all options and we will
 744          * sent L2CA_ConfigInd event to the upper layer protocol.
 745          */
 746
 747         if (respond) {
 748                 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m);
 749                 if (error != 0) {
 750                         ng_l2cap_l2ca_discon_ind(ch);
 751                         ng_l2cap_free_chan(ch);
 752                 }
 753         } else {
 754                 /* Send L2CA_ConfigInd event to the upper layer protocol */
 755                 ch->ident = ident;
 756                 error = ng_l2cap_l2ca_cfg_ind(ch);
 757                 if (error != 0)
 758                         ng_l2cap_free_chan(ch);
 759         }
 760
 761         return (error);
 762
 763 reject:
 764         /* Send reject. Do not really care about the result */
 765         NG_FREE_M(m);
 766
 767         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid);
 768
 769         return (0);
 770 } /* ng_l2cap_process_cfg_req */
 771
 772 /*
 773  * Process L2CAP_ConfigRsp command
 774  */
 775
 776 static int
 777 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident)
 778 {
 779         ng_l2cap_p               l2cap = con->l2cap;
 780         struct mbuf             *m = con->rx_pkt;
 781         ng_l2cap_cfg_rsp_cp     *cp = NULL;
 782         ng_l2cap_cmd_p           cmd = NULL;
 783         u_int16_t                scid, cflag, result;
 784         ng_l2cap_cfg_opt_t       hdr;
 785         ng_l2cap_cfg_opt_val_t   val;
 786         int                      off, error = 0;
 787
 788         /* Get command parameters */
 789         con->rx_pkt = NULL;
 790         NG_L2CAP_M_PULLUP(m, sizeof(*cp));
 791         if (m == NULL)
 792                 return (ENOBUFS);
 793
 794         cp = mtod(m, ng_l2cap_cfg_rsp_cp *);
 795         scid = le16toh(cp->scid);
 796         cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
 797         result = le16toh(cp->result);
 798         m_adj(m, sizeof(*cp));
 799
 800         /* Check if we have this command */
 801         cmd = ng_l2cap_cmd_by_ident(con, ident);
 802         if (cmd == NULL) {
 803                 NG_L2CAP_ERR(
 804 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n",
 805                         __func__, NG_NODE_NAME(l2cap->node), ident,
 806                         con->con_handle);
 807                 NG_FREE_M(m);
 808
 809                 return (ENOENT);
 810         }
 811
 812         /* Verify CIDs and send reject if does not match */
 813         if (cmd->ch->scid != scid) {
 814                 NG_L2CAP_ERR(
 815 "%s: %s - unexpected L2CAP_ConfigRsp. " \
 816 "Channel ID does not match, scid=%d(%d)\n",
 817                         __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
 818                         scid);
 819                 goto reject;
 820         }
 821
 822         /* Verify channel state and reject if invalid */
 823         if (cmd->ch->state != NG_L2CAP_CONFIG) {
 824                 NG_L2CAP_ERR(
 825 "%s: %s - unexpected L2CAP_ConfigRsp. " \
 826 "Invalid channel state, scid=%d, state=%d\n",
 827                         __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
 828                         cmd->ch->state);
 829                 goto reject;
 830         }
 831
 832         /*
 833          * Looks like it is our response, so process it. First parse options,
 834          * then verify C flag. If it is set then we shall expect more
 835          * configuration options from the peer and we will wait. Otherwise we
 836          * have received all options and we will send L2CA_ConfigRsp event to
 837          * the upper layer protocol. If command timeout already happened then
 838          * ignore response.
 839          */
 840
 841         if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
 842                 NG_FREE_M(m);
 843                 return (error);
 844         }
 845
 846         for (off = 0; ; ) {
 847                 error = get_next_l2cap_opt(m, &off, &hdr, &val);
 848                 if (error == 0) /* We done with this packet */
 849                         break;
 850                 else if (error > 0) { /* Got option */
 851                         switch (hdr.type) {
 852                         case NG_L2CAP_OPT_MTU:
 853                                 cmd->ch->imtu = val.mtu;
 854                         break;
 855
 856                         case NG_L2CAP_OPT_FLUSH_TIMO:
 857                                 cmd->ch->flush_timo = val.flush_timo;
 858                                 break;
 859
 860                         case NG_L2CAP_OPT_QOS:
 861                                 bcopy(&val.flow, &cmd->ch->oflow,
 862                                         sizeof(cmd->ch->oflow));
 863                         break;
 864
 865                         default: /* Ignore unknown hint option */
 866                                 break;
 867                         }
 868                 } else {
 869                         /*
 870                          * XXX FIXME What to do here?
 871                          *
 872                          * This is really BAD :( options packet was broken, or
 873                          * peer sent us option that we did not understand. Let
 874                          * upper layer know and do not wait for more options.
 875                          */
 876
 877                         NG_L2CAP_ALERT(
 878 "%s: %s - failed to parse configuration options, error=%d\n",
 879                                 __func__, NG_NODE_NAME(l2cap->node), error);
 880
 881                         result = NG_L2CAP_UNKNOWN;
 882                         cflag = 0;
 883
 884                         break;
 885                 }
 886         }
 887
 888         NG_FREE_M(m);
 889
 890         if (cflag) /* Restart timer and wait for more options */
 891                 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout());
 892         else {
 893                 ng_l2cap_unlink_cmd(cmd);
 894
 895                 /* Send L2CA_Config response to the upper layer protocol */
 896                 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result);
 897                 if (error != 0) {
 898                         /*
 899                          * XXX FIXME what to do here? we were not able to send
 900                          * response to the upper layer protocol, so for now
 901                          * just close the channel. Send L2CAP_Disconnect to
 902                          * remote peer?
 903                          */
 904
 905                         NG_L2CAP_ERR(
 906 "%s: %s - failed to send L2CA_Config response, error=%d\n",
 907                         __func__, NG_NODE_NAME(l2cap->node), error);
 908
 909                         ng_l2cap_free_chan(cmd->ch);
 910                 }
 911
 912                 ng_l2cap_free_cmd(cmd);
 913         }
 914
 915         return (error);
 916
 917 reject:
 918         /* Send reject. Do not really care about the result */
 919         NG_FREE_M(m);
 920
 921         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0);
 922
 923         return (0);
 924 } /* ng_l2cap_process_cfg_rsp */
 925
 926 /*
 927  * Process L2CAP_DisconnectReq command
 928  */
 929
 930 static int
 931 ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident)
 932 {
 933         ng_l2cap_p               l2cap = con->l2cap;
 934         ng_l2cap_discon_req_cp  *cp = NULL;
 935         ng_l2cap_chan_p          ch = NULL;
 936         ng_l2cap_cmd_p           cmd = NULL;
 937         u_int16_t                scid, dcid;
 938
 939         /* Get command parameters */
 940         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
 941         if (con->rx_pkt == NULL)
 942                 return (ENOBUFS);
 943
 944         cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *);
 945         dcid = le16toh(cp->dcid);
 946         scid = le16toh(cp->scid);
 947
 948         NG_FREE_M(con->rx_pkt);
 949
 950         /* Check if we have this channel and it is in valid state */
 951         ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
 952         if (ch == NULL) {
 953                 NG_L2CAP_ERR(
 954 "%s: %s - unexpected L2CAP_DisconnectReq message. " \
 955 "Channel does not exist, cid=%d\n",
 956                         __func__, NG_NODE_NAME(l2cap->node), dcid);
 957                 goto reject;
 958         }
 959
 960         /* XXX Verify channel state and reject if invalid -- is that true? */
 961         if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG &&
 962             ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
 963                 NG_L2CAP_ERR(
 964 "%s: %s - unexpected L2CAP_DisconnectReq. " \
 965 "Invalid channel state, cid=%d, state=%d\n",
 966                         __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
 967                 goto reject;
 968         }
 969
 970         /* Match destination channel ID */
 971         if (ch->dcid != scid || ch->scid != dcid) {
 972                 NG_L2CAP_ERR(
 973 "%s: %s - unexpected L2CAP_DisconnectReq. " \
 974 "Channel IDs does not match, channel: scid=%d, dcid=%d, " \
 975 "request: scid=%d, dcid=%d\n",
 976                         __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid,
 977                         scid, dcid);
 978                 goto reject;
 979         }
 980
 981         /*
 982          * Looks good, so notify upper layer protocol that channel is about
 983          * to be disconnected and send L2CA_DisconnectInd message. Then respond
 984          * with L2CAP_DisconnectRsp.
 985          */
 986
 987         if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
 988                 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */
 989                 ng_l2cap_free_chan(ch);
 990         }
 991
 992         /* Send L2CAP_DisconnectRsp */
 993         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0);
 994         if (cmd == NULL)
 995                 return (ENOMEM);
 996
 997         _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid);
 998         if (cmd->aux == NULL) {
 999                 ng_l2cap_free_cmd(cmd);
1000
1001                 return (ENOBUFS);
1002         }
1003
1004         /* Link command to the queue */
1005         ng_l2cap_link_cmd(con, cmd);
1006         ng_l2cap_lp_deliver(con);
1007
1008         return (0);
1009
1010 reject:
1011         /* Send reject. Do not really care about the result */
1012         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
1013
1014         return (0);
1015 } /* ng_l2cap_process_discon_req */
1016
1017 /*
1018  * Process L2CAP_DisconnectRsp command
1019  */
1020
1021 static int
1022 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident)
1023 {
1024         ng_l2cap_p               l2cap = con->l2cap;
1025         ng_l2cap_discon_rsp_cp  *cp = NULL;
1026         ng_l2cap_cmd_p           cmd = NULL;
1027         u_int16_t                scid, dcid;
1028         int                      error = 0;
1029
1030         /* Get command parameters */
1031         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1032         if (con->rx_pkt == NULL)
1033                 return (ENOBUFS);
1034
1035         cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *);
1036         dcid = le16toh(cp->dcid);
1037         scid = le16toh(cp->scid);
1038
1039         NG_FREE_M(con->rx_pkt);
1040
1041         /* Check if we have pending command descriptor */
1042         cmd = ng_l2cap_cmd_by_ident(con, ident);
1043         if (cmd == NULL) {
1044                 NG_L2CAP_ERR(
1045 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n",
1046                         __func__, NG_NODE_NAME(l2cap->node), ident,
1047                         con->con_handle);
1048                 goto out;
1049         }
1050
1051         /* Verify channel state, do nothing if invalid */
1052         if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
1053                 NG_L2CAP_ERR(
1054 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1055 "Invalid channel state, cid=%d, state=%d\n",
1056                         __func__, NG_NODE_NAME(l2cap->node), scid,
1057                         cmd->ch->state);
1058                 goto out;
1059         }
1060
1061         /* Verify CIDs and send reject if does not match */
1062         if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) {
1063                 NG_L2CAP_ERR(
1064 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1065 "Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n",
1066                         __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
1067                         scid, cmd->ch->dcid, dcid);
1068                 goto out;
1069         }
1070
1071         /*
1072          * Looks like we have successfully disconnected channel, so notify
1073          * upper layer. If command timeout already happened then ignore
1074          * response.
1075          */
1076
1077         if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
1078                 goto out;
1079
1080         error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS);
1081         ng_l2cap_free_chan(cmd->ch); /* this will free commands too */
1082 out:
1083         return (error);
1084 } /* ng_l2cap_process_discon_rsp */
1085
1086 /*
1087  * Process L2CAP_EchoReq command
1088  */
1089
1090 static int
1091 ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident)
1092 {
1093         ng_l2cap_p               l2cap = con->l2cap;
1094         ng_l2cap_cmd_hdr_t      *hdr = NULL;
1095         ng_l2cap_cmd_p           cmd = NULL;
1096
1097         con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr));
1098         if (con->rx_pkt == NULL) {
1099                 NG_L2CAP_ALERT(
1100 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n",
1101                         __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr));
1102
1103                 return (ENOBUFS);
1104         }
1105
1106         hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
1107         hdr->code = NG_L2CAP_ECHO_RSP;
1108         hdr->ident = ident;
1109         hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
1110
1111         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0);
1112         if (cmd == NULL) {
1113                 NG_FREE_M(con->rx_pkt);
1114
1115                 return (ENOBUFS);
1116         }
1117
1118         /* Attach data and link command to the queue */
1119         cmd->aux = con->rx_pkt;
1120         con->rx_pkt = NULL;
1121         ng_l2cap_link_cmd(con, cmd);
1122         ng_l2cap_lp_deliver(con);
1123
1124         return (0);
1125 } /* ng_l2cap_process_echo_req */
1126
1127 /*
1128  * Process L2CAP_EchoRsp command
1129  */
1130
1131 static int
1132 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident)
1133 {
1134         ng_l2cap_p      l2cap = con->l2cap;
1135         ng_l2cap_cmd_p  cmd = NULL;
1136         int             error = 0;
1137
1138         /* Check if we have this command */
1139         cmd = ng_l2cap_cmd_by_ident(con, ident);
1140         if (cmd != NULL) {
1141                 /* If command timeout already happened then ignore response */
1142                 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1143                         NG_FREE_M(con->rx_pkt);
1144                         return (error);
1145                 }
1146
1147                 ng_l2cap_unlink_cmd(cmd);
1148
1149                 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
1150                                 NG_L2CAP_SUCCESS, con->rx_pkt);
1151
1152                 ng_l2cap_free_cmd(cmd);
1153                 con->rx_pkt = NULL;
1154         } else {
1155                 NG_L2CAP_ERR(
1156 "%s: %s - unexpected L2CAP_EchoRsp command. " \
1157 "Requested ident does not exist, ident=%d\n",
1158                         __func__, NG_NODE_NAME(l2cap->node), ident);
1159                 NG_FREE_M(con->rx_pkt);
1160         }
1161
1162         return (error);
1163 } /* ng_l2cap_process_echo_rsp */
1164
1165 /*
1166  * Process L2CAP_InfoReq command
1167  */
1168
1169 static int
1170 ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident)
1171 {
1172         ng_l2cap_p      l2cap = con->l2cap;
1173         ng_l2cap_cmd_p  cmd = NULL;
1174         u_int16_t       type;
1175
1176         /* Get command parameters */
1177         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp));
1178         if (con->rx_pkt == NULL)
1179                 return (ENOBUFS);
1180
1181         type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type);
1182         NG_FREE_M(con->rx_pkt);
1183
1184         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0);
1185         if (cmd == NULL)
1186                 return (ENOMEM);
1187
1188         switch (type) {
1189         case NG_L2CAP_CONNLESS_MTU:
1190                 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU,
1191                                 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT);
1192                 break;
1193
1194         default:
1195                 _ng_l2cap_info_rsp(cmd->aux, ident, type,
1196                                 NG_L2CAP_NOT_SUPPORTED, 0);
1197                 break;
1198         }
1199
1200         if (cmd->aux == NULL) {
1201                 ng_l2cap_free_cmd(cmd);
1202
1203                 return (ENOBUFS);
1204         }
1205
1206         /* Link command to the queue */
1207         ng_l2cap_link_cmd(con, cmd);
1208         ng_l2cap_lp_deliver(con);
1209
1210         return (0);
1211 } /* ng_l2cap_process_info_req */
1212
1213 /*
1214  * Process L2CAP_InfoRsp command
1215  */
1216
1217 static int
1218 ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident)
1219 {
1220         ng_l2cap_p               l2cap = con->l2cap;
1221         ng_l2cap_info_rsp_cp    *cp = NULL;
1222         ng_l2cap_cmd_p           cmd = NULL;
1223         int                      error = 0;
1224
1225         /* Get command parameters */
1226         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1227         if (con->rx_pkt == NULL)
1228                 return (ENOBUFS);
1229
1230         cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *);
1231         cp->type = le16toh(cp->type);
1232         cp->result = le16toh(cp->result);
1233         m_adj(con->rx_pkt, sizeof(*cp));
1234
1235         /* Check if we have pending command descriptor */
1236         cmd = ng_l2cap_cmd_by_ident(con, ident);
1237         if (cmd == NULL) {
1238                 NG_L2CAP_ERR(
1239 "%s: %s - unexpected L2CAP_InfoRsp command. " \
1240 "Requested ident does not exist, ident=%d\n",
1241                         __func__, NG_NODE_NAME(l2cap->node), ident);
1242                 NG_FREE_M(con->rx_pkt);
1243
1244                 return (ENOENT);
1245         }
1246
1247         /* If command timeout already happened then ignore response */
1248         if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1249                 NG_FREE_M(con->rx_pkt);
1250                 return (error);
1251         }
1252
1253         ng_l2cap_unlink_cmd(cmd);
1254
1255         if (cp->result == NG_L2CAP_SUCCESS) {
1256                 switch (cp->type) {
1257                 case NG_L2CAP_CONNLESS_MTU:
1258                         if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t))
1259                                 *mtod(con->rx_pkt, u_int16_t *) =
1260                                         le16toh(*mtod(con->rx_pkt,u_int16_t *));
1261                         else {
1262                                 cp->result = NG_L2CAP_UNKNOWN; /* XXX */
1263
1264                                 NG_L2CAP_ERR(
1265 "%s: %s - invalid L2CAP_InfoRsp command. " \
1266 "Bad connectionless MTU parameter, len=%d\n",
1267                                         __func__, NG_NODE_NAME(l2cap->node),
1268                                         con->rx_pkt->m_pkthdr.len);
1269                         }
1270                         break;
1271
1272                 default:
1273                         NG_L2CAP_WARN(
1274 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n",
1275                                 __func__, NG_NODE_NAME(l2cap->node), cp->type);
1276                         break;
1277                 }
1278         }
1279
1280         error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
1281                         cp->result, con->rx_pkt);
1282
1283         ng_l2cap_free_cmd(cmd);
1284         con->rx_pkt = NULL;
1285
1286         return (error);
1287 } /* ng_l2cap_process_info_rsp */
1288
1289 /*
1290  * Send L2CAP reject
1291  */
1292
1293 static int
1294 send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason,
1295                 u_int16_t mtu, u_int16_t scid, u_int16_t dcid)
1296 {
1297         ng_l2cap_cmd_p  cmd = NULL;
1298
1299         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0);
1300         if (cmd == NULL)
1301                 return (ENOMEM);
1302
1303          _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid);
1304         if (cmd->aux == NULL) {
1305                 ng_l2cap_free_cmd(cmd);
1306
1307                 return (ENOBUFS);
1308         }
1309
1310         /* Link command to the queue */
1311         ng_l2cap_link_cmd(con, cmd);
1312         ng_l2cap_lp_deliver(con);
1313
1314         return (0);
1315 } /* send_l2cap_reject */
1316
1317 /*
1318  * Send L2CAP connection reject
1319  */
1320
1321 static int
1322 send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1323                 u_int16_t dcid, u_int16_t result)
1324 {
1325         ng_l2cap_cmd_p  cmd = NULL;
1326
1327         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0);
1328         if (cmd == NULL)
1329                 return (ENOMEM);
1330
1331         _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0);
1332         if (cmd->aux == NULL) {
1333                 ng_l2cap_free_cmd(cmd);
1334
1335                 return (ENOBUFS);
1336         }
1337
1338         /* Link command to the queue */
1339         ng_l2cap_link_cmd(con, cmd);
1340         ng_l2cap_lp_deliver(con);
1341
1342         return (0);
1343 } /* send_l2cap_con_rej */
1344
1345 /*
1346  * Send L2CAP config response
1347  */
1348
1349 static int
1350 send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1351                 u_int16_t result, struct mbuf *opt)
1352 {
1353         ng_l2cap_cmd_p  cmd = NULL;
1354
1355         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0);
1356         if (cmd == NULL) {
1357                 NG_FREE_M(opt);
1358
1359                 return (ENOMEM);
1360         }
1361
1362         _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt);
1363         if (cmd->aux == NULL) {
1364                 ng_l2cap_free_cmd(cmd);
1365
1366                 return (ENOBUFS);
1367         }
1368
1369         /* Link command to the queue */
1370         ng_l2cap_link_cmd(con, cmd);
1371         ng_l2cap_lp_deliver(con);
1372
1373         return (0);
1374 } /* send_l2cap_cfg_rsp */
1375
1376 static int
1377 send_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident,
1378                      u_int16_t result)
1379 {
1380         ng_l2cap_cmd_p  cmd = NULL;
1381
1382         cmd = ng_l2cap_new_cmd(con, NULL, ident,
1383                                NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE,
1384                                0);
1385         if (cmd == NULL) {
1386                 return (ENOMEM);
1387         }
1388
1389         _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result);
1390         if (cmd->aux == NULL) {
1391                 ng_l2cap_free_cmd(cmd);
1392
1393                 return (ENOBUFS);
1394         }
1395
1396         /* Link command to the queue */
1397         ng_l2cap_link_cmd(con, cmd);
1398         ng_l2cap_lp_deliver(con);
1399
1400         return (0);
1401 } /* send_l2cap_cfg_rsp */
1402
1403 /*
1404  * Get next L2CAP configuration option
1405  *
1406  * Return codes:
1407  *  0   no option
1408  *  1   we have got option
1409  * -1   header too short
1410  * -2   bad option value or length
1411  * -3   unknown option
1412  */
1413
1414 static int
1415 get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr,
1416                 ng_l2cap_cfg_opt_val_p val)
1417 {
1418         int     hint, len = m->m_pkthdr.len - (*off);
1419
1420         if (len == 0)
1421                 return (0);
1422         if (len < 0 || len < sizeof(*hdr))
1423                 return (-1);
1424
1425         m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr);
1426         *off += sizeof(*hdr);
1427         len  -= sizeof(*hdr);
1428
1429         hint = NG_L2CAP_OPT_HINT(hdr->type);
1430         hdr->type &= NG_L2CAP_OPT_HINT_MASK;
1431
1432         switch (hdr->type) {
1433         case NG_L2CAP_OPT_MTU:
1434                 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length)
1435                         return (-2);
1436
1437                 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val);
1438                 val->mtu = le16toh(val->mtu);
1439                 *off += NG_L2CAP_OPT_MTU_SIZE;
1440                 break;
1441
1442         case NG_L2CAP_OPT_FLUSH_TIMO:
1443                 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE ||
1444                     len < hdr->length)
1445                         return (-2);
1446
1447                 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val);
1448                 val->flush_timo = le16toh(val->flush_timo);
1449                 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE;
1450                 break;
1451
1452         case NG_L2CAP_OPT_QOS:
1453                 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length)
1454                         return (-2);
1455
1456                 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val);
1457                 val->flow.token_rate = le32toh(val->flow.token_rate);
1458                 val->flow.token_bucket_size =
1459                                 le32toh(val->flow.token_bucket_size);
1460                 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth);
1461                 val->flow.latency = le32toh(val->flow.latency);
1462                 val->flow.delay_variation = le32toh(val->flow.delay_variation);
1463                 *off += NG_L2CAP_OPT_QOS_SIZE;
1464                 break;
1465
1466         default:
1467                 if (hint)
1468                         *off += hdr->length;
1469                 else
1470                         return (-3);
1471                 break;
1472         }
1473
1474         return (1);
1475 } /* get_next_l2cap_opt */