sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c

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