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