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 paramter negotiasion 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( con->linktype != NG_HCI_LINK_ACL)
 479                 idtype = NG_L2CAP_L2CA_IDTYPE_LE;
 480         else
 481                 idtype = NG_L2CAP_L2CA_IDTYPE_BREDR;
 482
 483         /*
 484          * Create new channel and send L2CA_ConnectInd notification
 485          * to the upper layer protocol.
 486          */
 487
 488         ch = ng_l2cap_new_chan(l2cap, con, psm, idtype);
 489
 490         if (ch == NULL)
 491                 return (send_l2cap_con_rej(con, ident, 0, dcid,
 492                                 NG_L2CAP_NO_RESOURCES));
 493
 494         /* Update channel IDs */
 495         ch->dcid = dcid;
 496
 497         /* Sent L2CA_ConnectInd notification to the upper layer */
 498         ch->ident = ident;
 499         ch->state = NG_L2CAP_W4_L2CA_CON_RSP;
 500
 501         error = ng_l2cap_l2ca_con_ind(ch);
 502         if (error != 0) {
 503                 send_l2cap_con_rej(con, ident, ch->scid, dcid,
 504                         (error == ENOMEM)? NG_L2CAP_NO_RESOURCES :
 505                                 NG_L2CAP_PSM_NOT_SUPPORTED);
 506                 ng_l2cap_free_chan(ch);
 507         }
 508
 509         return (error);
 510 } /* ng_l2cap_process_con_req */
 511
 512 /*
 513  * Process L2CAP_ConnectRsp command
 514  */
 515
 516 static int
 517 ng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident)
 518 {
 519         ng_l2cap_p               l2cap = con->l2cap;
 520         struct mbuf             *m = con->rx_pkt;
 521         ng_l2cap_con_rsp_cp     *cp = NULL;
 522         ng_l2cap_cmd_p           cmd = NULL;
 523         u_int16_t                scid, dcid, result, status;
 524         int                      error = 0;
 525
 526         /* Get command parameters */
 527         NG_L2CAP_M_PULLUP(m, sizeof(*cp));
 528         if (m == NULL)
 529                 return (ENOBUFS);
 530
 531         cp = mtod(m, ng_l2cap_con_rsp_cp *);
 532         dcid = le16toh(cp->dcid);
 533         scid = le16toh(cp->scid);
 534         result = le16toh(cp->result);
 535         status = le16toh(cp->status);
 536
 537         NG_FREE_M(m);
 538         con->rx_pkt = NULL;
 539
 540         /* Check if we have pending command descriptor */
 541         cmd = ng_l2cap_cmd_by_ident(con, ident);
 542         if (cmd == NULL) {
 543                 NG_L2CAP_ERR(
 544 "%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n",
 545                         __func__, NG_NODE_NAME(l2cap->node), ident,
 546                         con->con_handle);
 547
 548                 return (ENOENT);
 549         }
 550
 551         /* Verify channel state, if invalid - do nothing */
 552         if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) {
 553                 NG_L2CAP_ERR(
 554 "%s: %s - unexpected L2CAP_ConnectRsp. " \
 555 "Invalid channel state, cid=%d, state=%d\n",
 556                         __func__, NG_NODE_NAME(l2cap->node), scid,
 557                         cmd->ch->state);
 558                 goto reject;
 559         }
 560
 561         /* Verify CIDs and send reject if does not match */
 562         if (cmd->ch->scid != scid) {
 563                 NG_L2CAP_ERR(
 564 "%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n",
 565                          __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
 566                         scid);
 567                 goto reject;
 568         }
 569
 570         /*
 571          * Looks good. We got confirmation from our peer. Now process
 572          * it. First disable RTX timer. Then check the result and send
 573          * notification to the upper layer. If command timeout already
 574          * happened then ignore response.
 575          */
 576
 577         if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
 578                 return (error);
 579
 580         if (result == NG_L2CAP_PENDING) {
 581                 /*
 582                  * Our peer wants more time to complete connection. We shall
 583                  * start ERTX timer and wait. Keep command in the list.
 584                  */
 585
 586                 cmd->ch->dcid = dcid;
 587                 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout());
 588
 589                 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
 590                                 result, status);
 591                 if (error != 0)
 592                         ng_l2cap_free_chan(cmd->ch);
 593         } else {
 594                 ng_l2cap_unlink_cmd(cmd);
 595
 596                 if (result == NG_L2CAP_SUCCESS) {
 597                         /*
 598                          * Channel is open. Complete command and move to CONFIG
 599                          * state. Since we have sent positive confirmation we
 600                          * expect to receive L2CA_Config request from the upper
 601                          * layer protocol.
 602                          */
 603
 604                         cmd->ch->dcid = dcid;
 605                         cmd->ch->state = (cmd->ch->scid == NG_L2CAP_ATT_CID)?
 606                           NG_L2CAP_OPEN : NG_L2CAP_CONFIG;
 607                 } else
 608                         /* There was an error, so close the channel */
 609                         NG_L2CAP_INFO(
 610 "%s: %s - failed to open L2CAP channel, result=%d, status=%d\n",
 611                                 __func__, NG_NODE_NAME(l2cap->node), result,
 612                                 status);
 613
 614                 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
 615                                 result, status);
 616
 617                 /* XXX do we have to remove the channel on error? */
 618                 if (error != 0 || result != NG_L2CAP_SUCCESS)
 619                         ng_l2cap_free_chan(cmd->ch);
 620
 621                 ng_l2cap_free_cmd(cmd);
 622         }
 623
 624         return (error);
 625
 626 reject:
 627         /* Send reject. Do not really care about the result */
 628         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
 629
 630         return (0);
 631 } /* ng_l2cap_process_con_rsp */
 632
 633 /*
 634  * Process L2CAP_ConfigReq command
 635  */
 636
 637 static int
 638 ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident)
 639 {
 640         ng_l2cap_p               l2cap = con->l2cap;
 641         struct mbuf             *m = con->rx_pkt;
 642         ng_l2cap_cfg_req_cp     *cp = NULL;
 643         ng_l2cap_chan_p          ch = NULL;
 644         u_int16_t                dcid, respond, result;
 645         ng_l2cap_cfg_opt_t       hdr;
 646         ng_l2cap_cfg_opt_val_t   val;
 647         int                      off, error = 0;
 648
 649         /* Get command parameters */
 650         con->rx_pkt = NULL;
 651         NG_L2CAP_M_PULLUP(m, sizeof(*cp));
 652         if (m == NULL)
 653                 return (ENOBUFS);
 654
 655         cp = mtod(m, ng_l2cap_cfg_req_cp *);
 656         dcid = le16toh(cp->dcid);
 657         respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
 658         m_adj(m, sizeof(*cp));
 659
 660         /* Check if we have this channel and it is in valid state */
 661         ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
 662         if (ch == NULL) {
 663                 NG_L2CAP_ERR(
 664 "%s: %s - unexpected L2CAP_ConfigReq command. " \
 665 "Channel does not exist, cid=%d\n",
 666                         __func__, NG_NODE_NAME(l2cap->node), dcid);
 667                 goto reject;
 668         }
 669
 670         /* Verify channel state */
 671         if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) {
 672                 NG_L2CAP_ERR(
 673 "%s: %s - unexpected L2CAP_ConfigReq. " \
 674 "Invalid channel state, cid=%d, state=%d\n",
 675                         __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
 676                 goto reject;
 677         }
 678
 679         if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */
 680                 ch->cfg_state = 0;
 681                 ch->state = NG_L2CAP_CONFIG;
 682         }
 683
 684         for (result = 0, off = 0; ; ) {
 685                 error = get_next_l2cap_opt(m, &off, &hdr, &val);
 686                 if (error == 0) { /* We done with this packet */
 687                         NG_FREE_M(m);
 688                         break;
 689                 } else if (error > 0) { /* Got option */
 690                         switch (hdr.type) {
 691                         case NG_L2CAP_OPT_MTU:
 692                                 ch->omtu = val.mtu;
 693                                 break;
 694
 695                         case NG_L2CAP_OPT_FLUSH_TIMO:
 696                                 ch->flush_timo = val.flush_timo;
 697                                 break;
 698
 699                         case NG_L2CAP_OPT_QOS:
 700                                 bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow));
 701                                 break;
 702
 703                         default: /* Ignore unknown hint option */
 704                                 break;
 705                         }
 706                 } else { /* Oops, something is wrong */
 707                         respond = 1;
 708
 709                         if (error == -3) {
 710
 711                                 /*
 712                                  * Adjust mbuf so we can get to the start
 713                                  * of the first option we did not like.
 714                                  */
 715
 716                                 m_adj(m, off - sizeof(hdr));
 717                                 m->m_pkthdr.len = sizeof(hdr) + hdr.length;
 718
 719                                 result = NG_L2CAP_UNKNOWN_OPTION;
 720                         } else {
 721                                 /* XXX FIXME Send other reject codes? */
 722                                 NG_FREE_M(m);
 723                                 result = NG_L2CAP_REJECT;
 724                         }
 725
 726                         break;
 727                 }
 728         }
 729
 730         /*
 731          * Now check and see if we have to respond. If everything was OK then
 732          * respond contain "C flag" and (if set) we will respond with empty
 733          * packet and will wait for more options.
 734          *
 735          * Other case is that we did not like peer's options and will respond
 736          * with L2CAP_Config response command with Reject error code.
 737          *
 738          * When "respond == 0" than we have received all options and we will
 739          * sent L2CA_ConfigInd event to the upper layer protocol.
 740          */
 741
 742         if (respond) {
 743                 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m);
 744                 if (error != 0) {
 745                         ng_l2cap_l2ca_discon_ind(ch);
 746                         ng_l2cap_free_chan(ch);
 747                 }
 748         } else {
 749                 /* Send L2CA_ConfigInd event to the upper layer protocol */
 750                 ch->ident = ident;
 751                 error = ng_l2cap_l2ca_cfg_ind(ch);
 752                 if (error != 0)
 753                         ng_l2cap_free_chan(ch);
 754         }
 755
 756         return (error);
 757
 758 reject:
 759         /* Send reject. Do not really care about the result */
 760         NG_FREE_M(m);
 761
 762         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid);
 763
 764         return (0);
 765 } /* ng_l2cap_process_cfg_req */
 766
 767 /*
 768  * Process L2CAP_ConfigRsp command
 769  */
 770
 771 static int
 772 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident)
 773 {
 774         ng_l2cap_p               l2cap = con->l2cap;
 775         struct mbuf             *m = con->rx_pkt;
 776         ng_l2cap_cfg_rsp_cp     *cp = NULL;
 777         ng_l2cap_cmd_p           cmd = NULL;
 778         u_int16_t                scid, cflag, result;
 779         ng_l2cap_cfg_opt_t       hdr;
 780         ng_l2cap_cfg_opt_val_t   val;
 781         int                      off, error = 0;
 782
 783         /* Get command parameters */
 784         con->rx_pkt = NULL;
 785         NG_L2CAP_M_PULLUP(m, sizeof(*cp));
 786         if (m == NULL)
 787                 return (ENOBUFS);
 788
 789         cp = mtod(m, ng_l2cap_cfg_rsp_cp *);
 790         scid = le16toh(cp->scid);
 791         cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
 792         result = le16toh(cp->result);
 793         m_adj(m, sizeof(*cp));
 794
 795         /* Check if we have this command */
 796         cmd = ng_l2cap_cmd_by_ident(con, ident);
 797         if (cmd == NULL) {
 798                 NG_L2CAP_ERR(
 799 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n",
 800                         __func__, NG_NODE_NAME(l2cap->node), ident,
 801                         con->con_handle);
 802                 NG_FREE_M(m);
 803
 804                 return (ENOENT);
 805         }
 806
 807         /* Verify CIDs and send reject if does not match */
 808         if (cmd->ch->scid != scid) {
 809                 NG_L2CAP_ERR(
 810 "%s: %s - unexpected L2CAP_ConfigRsp. " \
 811 "Channel ID does not match, scid=%d(%d)\n",
 812                         __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
 813                         scid);
 814                 goto reject;
 815         }
 816
 817         /* Verify channel state and reject if invalid */
 818         if (cmd->ch->state != NG_L2CAP_CONFIG) {
 819                 NG_L2CAP_ERR(
 820 "%s: %s - unexpected L2CAP_ConfigRsp. " \
 821 "Invalid channel state, scid=%d, state=%d\n",
 822                         __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
 823                         cmd->ch->state);
 824                 goto reject;
 825         }
 826
 827         /*
 828          * Looks like it is our response, so process it. First parse options,
 829          * then verify C flag. If it is set then we shall expect more
 830          * configuration options from the peer and we will wait. Otherwise we
 831          * have received all options and we will send L2CA_ConfigRsp event to
 832          * the upper layer protocol. If command timeout already happened then
 833          * ignore response.
 834          */
 835
 836         if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
 837                 NG_FREE_M(m);
 838                 return (error);
 839         }
 840
 841         for (off = 0; ; ) {
 842                 error = get_next_l2cap_opt(m, &off, &hdr, &val);
 843                 if (error == 0) /* We done with this packet */
 844                         break;
 845                 else if (error > 0) { /* Got option */
 846                         switch (hdr.type) {
 847                         case NG_L2CAP_OPT_MTU:
 848                                 cmd->ch->imtu = val.mtu;
 849                         break;
 850
 851                         case NG_L2CAP_OPT_FLUSH_TIMO:
 852                                 cmd->ch->flush_timo = val.flush_timo;
 853                                 break;
 854
 855                         case NG_L2CAP_OPT_QOS:
 856                                 bcopy(&val.flow, &cmd->ch->oflow,
 857                                         sizeof(cmd->ch->oflow));
 858                         break;
 859
 860                         default: /* Ignore unknown hint option */
 861                                 break;
 862                         }
 863                 } else {
 864                         /*
 865                          * XXX FIXME What to do here?
 866                          *
 867                          * This is really BAD :( options packet was broken, or
 868                          * peer sent us option that we did not understand. Let
 869                          * upper layer know and do not wait for more options.
 870                          */
 871
 872                         NG_L2CAP_ALERT(
 873 "%s: %s - failed to parse configuration options, error=%d\n",
 874                                 __func__, NG_NODE_NAME(l2cap->node), error);
 875
 876                         result = NG_L2CAP_UNKNOWN;
 877                         cflag = 0;
 878
 879                         break;
 880                 }
 881         }
 882
 883         NG_FREE_M(m);
 884
 885         if (cflag) /* Restart timer and wait for more options */
 886                 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout());
 887         else {
 888                 ng_l2cap_unlink_cmd(cmd);
 889
 890                 /* Send L2CA_Config response to the upper layer protocol */
 891                 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result);
 892                 if (error != 0) {
 893                         /*
 894                          * XXX FIXME what to do here? we were not able to send
 895                          * response to the upper layer protocol, so for now
 896                          * just close the channel. Send L2CAP_Disconnect to
 897                          * remote peer?
 898                          */
 899
 900                         NG_L2CAP_ERR(
 901 "%s: %s - failed to send L2CA_Config response, error=%d\n",
 902                         __func__, NG_NODE_NAME(l2cap->node), error);
 903
 904                         ng_l2cap_free_chan(cmd->ch);
 905                 }
 906
 907                 ng_l2cap_free_cmd(cmd);
 908         }
 909
 910         return (error);
 911
 912 reject:
 913         /* Send reject. Do not really care about the result */
 914         NG_FREE_M(m);
 915
 916         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0);
 917
 918         return (0);
 919 } /* ng_l2cap_process_cfg_rsp */
 920
 921 /*
 922  * Process L2CAP_DisconnectReq command
 923  */
 924
 925 static int
 926 ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident)
 927 {
 928         ng_l2cap_p               l2cap = con->l2cap;
 929         ng_l2cap_discon_req_cp  *cp = NULL;
 930         ng_l2cap_chan_p          ch = NULL;
 931         ng_l2cap_cmd_p           cmd = NULL;
 932         u_int16_t                scid, dcid;
 933
 934         /* Get command parameters */
 935         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
 936         if (con->rx_pkt == NULL)
 937                 return (ENOBUFS);
 938
 939         cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *);
 940         dcid = le16toh(cp->dcid);
 941         scid = le16toh(cp->scid);
 942
 943         NG_FREE_M(con->rx_pkt);
 944
 945         /* Check if we have this channel and it is in valid state */
 946         ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
 947         if (ch == NULL) {
 948                 NG_L2CAP_ERR(
 949 "%s: %s - unexpected L2CAP_DisconnectReq message. " \
 950 "Channel does not exist, cid=%d\n",
 951                         __func__, NG_NODE_NAME(l2cap->node), dcid);
 952                 goto reject;
 953         }
 954
 955         /* XXX Verify channel state and reject if invalid -- is that true? */
 956         if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG &&
 957             ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
 958                 NG_L2CAP_ERR(
 959 "%s: %s - unexpected L2CAP_DisconnectReq. " \
 960 "Invalid channel state, cid=%d, state=%d\n",
 961                         __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
 962                 goto reject;
 963         }
 964
 965         /* Match destination channel ID */
 966         if (ch->dcid != scid || ch->scid != dcid) {
 967                 NG_L2CAP_ERR(
 968 "%s: %s - unexpected L2CAP_DisconnectReq. " \
 969 "Channel IDs does not match, channel: scid=%d, dcid=%d, " \
 970 "request: scid=%d, dcid=%d\n",
 971                         __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid,
 972                         scid, dcid);
 973                 goto reject;
 974         }
 975
 976         /*
 977          * Looks good, so notify upper layer protocol that channel is about
 978          * to be disconnected and send L2CA_DisconnectInd message. Then respond
 979          * with L2CAP_DisconnectRsp.
 980          */
 981
 982         if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
 983                 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */
 984                 ng_l2cap_free_chan(ch);
 985         }
 986
 987         /* Send L2CAP_DisconnectRsp */
 988         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0);
 989         if (cmd == NULL)
 990                 return (ENOMEM);
 991
 992         _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid);
 993         if (cmd->aux == NULL) {
 994                 ng_l2cap_free_cmd(cmd);
 995
 996                 return (ENOBUFS);
 997         }
 998
 999         /* Link command to the queue */
1000         ng_l2cap_link_cmd(con, cmd);
1001         ng_l2cap_lp_deliver(con);
1002
1003         return (0);
1004
1005 reject:
1006         /* Send reject. Do not really care about the result */
1007         send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
1008
1009         return (0);
1010 } /* ng_l2cap_process_discon_req */
1011
1012 /*
1013  * Process L2CAP_DisconnectRsp command
1014  */
1015
1016 static int
1017 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident)
1018 {
1019         ng_l2cap_p               l2cap = con->l2cap;
1020         ng_l2cap_discon_rsp_cp  *cp = NULL;
1021         ng_l2cap_cmd_p           cmd = NULL;
1022         u_int16_t                scid, dcid;
1023         int                      error = 0;
1024
1025         /* Get command parameters */
1026         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1027         if (con->rx_pkt == NULL)
1028                 return (ENOBUFS);
1029
1030         cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *);
1031         dcid = le16toh(cp->dcid);
1032         scid = le16toh(cp->scid);
1033
1034         NG_FREE_M(con->rx_pkt);
1035
1036         /* Check if we have pending command descriptor */
1037         cmd = ng_l2cap_cmd_by_ident(con, ident);
1038         if (cmd == NULL) {
1039                 NG_L2CAP_ERR(
1040 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n",
1041                         __func__, NG_NODE_NAME(l2cap->node), ident,
1042                         con->con_handle);
1043                 goto out;
1044         }
1045
1046         /* Verify channel state, do nothing if invalid */
1047         if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
1048                 NG_L2CAP_ERR(
1049 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1050 "Invalid channel state, cid=%d, state=%d\n",
1051                         __func__, NG_NODE_NAME(l2cap->node), scid,
1052                         cmd->ch->state);
1053                 goto out;
1054         }
1055
1056         /* Verify CIDs and send reject if does not match */
1057         if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) {
1058                 NG_L2CAP_ERR(
1059 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1060 "Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n",
1061                         __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
1062                         scid, cmd->ch->dcid, dcid);
1063                 goto out;
1064         }
1065
1066         /*
1067          * Looks like we have successfuly disconnected channel, so notify
1068          * upper layer. If command timeout already happened then ignore
1069          * response.
1070          */
1071
1072         if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
1073                 goto out;
1074
1075         error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS);
1076         ng_l2cap_free_chan(cmd->ch); /* this will free commands too */
1077 out:
1078         return (error);
1079 } /* ng_l2cap_process_discon_rsp */
1080
1081 /*
1082  * Process L2CAP_EchoReq command
1083  */
1084
1085 static int
1086 ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident)
1087 {
1088         ng_l2cap_p               l2cap = con->l2cap;
1089         ng_l2cap_cmd_hdr_t      *hdr = NULL;
1090         ng_l2cap_cmd_p           cmd = NULL;
1091
1092         con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr));
1093         if (con->rx_pkt == NULL) {
1094                 NG_L2CAP_ALERT(
1095 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n",
1096                         __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr));
1097
1098                 return (ENOBUFS);
1099         }
1100
1101         hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
1102         hdr->code = NG_L2CAP_ECHO_RSP;
1103         hdr->ident = ident;
1104         hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
1105
1106         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0);
1107         if (cmd == NULL) {
1108                 NG_FREE_M(con->rx_pkt);
1109
1110                 return (ENOBUFS);
1111         }
1112
1113         /* Attach data and link command to the queue */
1114         cmd->aux = con->rx_pkt;
1115         con->rx_pkt = NULL;
1116         ng_l2cap_link_cmd(con, cmd);
1117         ng_l2cap_lp_deliver(con);
1118
1119         return (0);
1120 } /* ng_l2cap_process_echo_req */
1121
1122 /*
1123  * Process L2CAP_EchoRsp command
1124  */
1125
1126 static int
1127 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident)
1128 {
1129         ng_l2cap_p      l2cap = con->l2cap;
1130         ng_l2cap_cmd_p  cmd = NULL;
1131         int             error = 0;
1132
1133         /* Check if we have this command */
1134         cmd = ng_l2cap_cmd_by_ident(con, ident);
1135         if (cmd != NULL) {
1136                 /* If command timeout already happened then ignore response */
1137                 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1138                         NG_FREE_M(con->rx_pkt);
1139                         return (error);
1140                 }
1141
1142                 ng_l2cap_unlink_cmd(cmd);
1143
1144                 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
1145                                 NG_L2CAP_SUCCESS, con->rx_pkt);
1146
1147                 ng_l2cap_free_cmd(cmd);
1148                 con->rx_pkt = NULL;
1149         } else {
1150                 NG_L2CAP_ERR(
1151 "%s: %s - unexpected L2CAP_EchoRsp command. " \
1152 "Requested ident does not exist, ident=%d\n",
1153                         __func__, NG_NODE_NAME(l2cap->node), ident);
1154                 NG_FREE_M(con->rx_pkt);
1155         }
1156
1157         return (error);
1158 } /* ng_l2cap_process_echo_rsp */
1159
1160 /*
1161  * Process L2CAP_InfoReq command
1162  */
1163
1164 static int
1165 ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident)
1166 {
1167         ng_l2cap_p      l2cap = con->l2cap;
1168         ng_l2cap_cmd_p  cmd = NULL;
1169         u_int16_t       type;
1170
1171         /* Get command parameters */
1172         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp));
1173         if (con->rx_pkt == NULL)
1174                 return (ENOBUFS);
1175
1176         type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type);
1177         NG_FREE_M(con->rx_pkt);
1178
1179         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0);
1180         if (cmd == NULL)
1181                 return (ENOMEM);
1182
1183         switch (type) {
1184         case NG_L2CAP_CONNLESS_MTU:
1185                 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU,
1186                                 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT);
1187                 break;
1188
1189         default:
1190                 _ng_l2cap_info_rsp(cmd->aux, ident, type,
1191                                 NG_L2CAP_NOT_SUPPORTED, 0);
1192                 break;
1193         }
1194
1195         if (cmd->aux == NULL) {
1196                 ng_l2cap_free_cmd(cmd);
1197
1198                 return (ENOBUFS);
1199         }
1200
1201         /* Link command to the queue */
1202         ng_l2cap_link_cmd(con, cmd);
1203         ng_l2cap_lp_deliver(con);
1204
1205         return (0);
1206 } /* ng_l2cap_process_info_req */
1207
1208 /*
1209  * Process L2CAP_InfoRsp command
1210  */
1211
1212 static int
1213 ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident)
1214 {
1215         ng_l2cap_p               l2cap = con->l2cap;
1216         ng_l2cap_info_rsp_cp    *cp = NULL;
1217         ng_l2cap_cmd_p           cmd = NULL;
1218         int                      error = 0;
1219
1220         /* Get command parameters */
1221         NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1222         if (con->rx_pkt == NULL)
1223                 return (ENOBUFS);
1224
1225         cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *);
1226         cp->type = le16toh(cp->type);
1227         cp->result = le16toh(cp->result);
1228         m_adj(con->rx_pkt, sizeof(*cp));
1229
1230         /* Check if we have pending command descriptor */
1231         cmd = ng_l2cap_cmd_by_ident(con, ident);
1232         if (cmd == NULL) {
1233                 NG_L2CAP_ERR(
1234 "%s: %s - unexpected L2CAP_InfoRsp command. " \
1235 "Requested ident does not exist, ident=%d\n",
1236                         __func__, NG_NODE_NAME(l2cap->node), ident);
1237                 NG_FREE_M(con->rx_pkt);
1238
1239                 return (ENOENT);
1240         }
1241
1242         /* If command timeout already happened then ignore response */
1243         if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1244                 NG_FREE_M(con->rx_pkt);
1245                 return (error);
1246         }
1247
1248         ng_l2cap_unlink_cmd(cmd);
1249
1250         if (cp->result == NG_L2CAP_SUCCESS) {
1251                 switch (cp->type) {
1252                 case NG_L2CAP_CONNLESS_MTU:
1253                         if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t))
1254                                 *mtod(con->rx_pkt, u_int16_t *) =
1255                                         le16toh(*mtod(con->rx_pkt,u_int16_t *));
1256                         else {
1257                                 cp->result = NG_L2CAP_UNKNOWN; /* XXX */
1258
1259                                 NG_L2CAP_ERR(
1260 "%s: %s - invalid L2CAP_InfoRsp command. " \
1261 "Bad connectionless MTU parameter, len=%d\n",
1262                                         __func__, NG_NODE_NAME(l2cap->node),
1263                                         con->rx_pkt->m_pkthdr.len);
1264                         }
1265                         break;
1266
1267                 default:
1268                         NG_L2CAP_WARN(
1269 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n",
1270                                 __func__, NG_NODE_NAME(l2cap->node), cp->type);
1271                         break;
1272                 }
1273         }
1274
1275         error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
1276                         cp->result, con->rx_pkt);
1277
1278         ng_l2cap_free_cmd(cmd);
1279         con->rx_pkt = NULL;
1280
1281         return (error);
1282 } /* ng_l2cap_process_info_rsp */
1283
1284 /*
1285  * Send L2CAP reject
1286  */
1287
1288 static int
1289 send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason,
1290                 u_int16_t mtu, u_int16_t scid, u_int16_t dcid)
1291 {
1292         ng_l2cap_cmd_p  cmd = NULL;
1293
1294         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0);
1295         if (cmd == NULL)
1296                 return (ENOMEM);
1297
1298          _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid);
1299         if (cmd->aux == NULL) {
1300                 ng_l2cap_free_cmd(cmd);
1301
1302                 return (ENOBUFS);
1303         }
1304
1305         /* Link command to the queue */
1306         ng_l2cap_link_cmd(con, cmd);
1307         ng_l2cap_lp_deliver(con);
1308
1309         return (0);
1310 } /* send_l2cap_reject */
1311
1312 /*
1313  * Send L2CAP connection reject
1314  */
1315
1316 static int
1317 send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1318                 u_int16_t dcid, u_int16_t result)
1319 {
1320         ng_l2cap_cmd_p  cmd = NULL;
1321
1322         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0);
1323         if (cmd == NULL)
1324                 return (ENOMEM);
1325
1326         _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0);
1327         if (cmd->aux == NULL) {
1328                 ng_l2cap_free_cmd(cmd);
1329
1330                 return (ENOBUFS);
1331         }
1332
1333         /* Link command to the queue */
1334         ng_l2cap_link_cmd(con, cmd);
1335         ng_l2cap_lp_deliver(con);
1336
1337         return (0);
1338 } /* send_l2cap_con_rej */
1339
1340 /*
1341  * Send L2CAP config response
1342  */
1343
1344 static int
1345 send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1346                 u_int16_t result, struct mbuf *opt)
1347 {
1348         ng_l2cap_cmd_p  cmd = NULL;
1349
1350         cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0);
1351         if (cmd == NULL) {
1352                 NG_FREE_M(opt);
1353
1354                 return (ENOMEM);
1355         }
1356
1357         _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt);
1358         if (cmd->aux == NULL) {
1359                 ng_l2cap_free_cmd(cmd);
1360
1361                 return (ENOBUFS);
1362         }
1363
1364         /* Link command to the queue */
1365         ng_l2cap_link_cmd(con, cmd);
1366         ng_l2cap_lp_deliver(con);
1367
1368         return (0);
1369 } /* send_l2cap_cfg_rsp */
1370
1371 static int
1372 send_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident,
1373                      u_int16_t result)
1374 {
1375         ng_l2cap_cmd_p  cmd = NULL;
1376
1377         cmd = ng_l2cap_new_cmd(con, NULL, ident,
1378                                NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE,
1379                                0);
1380         if (cmd == NULL) {
1381
1382                 return (ENOMEM);
1383         }
1384
1385         _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result);
1386         if (cmd->aux == NULL) {
1387                 ng_l2cap_free_cmd(cmd);
1388
1389                 return (ENOBUFS);
1390         }
1391
1392         /* Link command to the queue */
1393         ng_l2cap_link_cmd(con, cmd);
1394         ng_l2cap_lp_deliver(con);
1395
1396         return (0);
1397 } /* send_l2cap_cfg_rsp */
1398
1399 /*
1400  * Get next L2CAP configuration option
1401  *
1402  * Return codes:
1403  *  0   no option
1404  *  1   we have got option
1405  * -1   header too short
1406  * -2   bad option value or length
1407  * -3   unknown option
1408  */
1409
1410 static int
1411 get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr,
1412                 ng_l2cap_cfg_opt_val_p val)
1413 {
1414         int     hint, len = m->m_pkthdr.len - (*off);
1415
1416         if (len == 0)
1417                 return (0);
1418         if (len < 0 || len < sizeof(*hdr))
1419                 return (-1);
1420
1421         m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr);
1422         *off += sizeof(*hdr);
1423         len  -= sizeof(*hdr);
1424
1425         hint = NG_L2CAP_OPT_HINT(hdr->type);
1426         hdr->type &= NG_L2CAP_OPT_HINT_MASK;
1427
1428         switch (hdr->type) {
1429         case NG_L2CAP_OPT_MTU:
1430                 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length)
1431                         return (-2);
1432
1433                 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val);
1434                 val->mtu = le16toh(val->mtu);
1435                 *off += NG_L2CAP_OPT_MTU_SIZE;
1436                 break;
1437
1438         case NG_L2CAP_OPT_FLUSH_TIMO:
1439                 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE ||
1440                     len < hdr->length)
1441                         return (-2);
1442
1443                 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val);
1444                 val->flush_timo = le16toh(val->flush_timo);
1445                 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE;
1446                 break;
1447
1448         case NG_L2CAP_OPT_QOS:
1449                 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length)
1450                         return (-2);
1451
1452                 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val);
1453                 val->flow.token_rate = le32toh(val->flow.token_rate);
1454                 val->flow.token_bucket_size =
1455                                 le32toh(val->flow.token_bucket_size);
1456                 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth);
1457                 val->flow.latency = le32toh(val->flow.latency);
1458                 val->flow.delay_variation = le32toh(val->flow.delay_variation);
1459                 *off += NG_L2CAP_OPT_QOS_SIZE;
1460                 break;
1461
1462         default:
1463                 if (hint)
1464                         *off += hdr->length;
1465                 else
1466                         return (-3);
1467                 break;
1468         }
1469
1470         return (1);
1471 } /* get_next_l2cap_opt */
1472