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