sys/netbt/l2cap_signal.c

   1 /*      $NetBSD: l2cap_signal.c,v 1.9 2007/11/10 23:12:23 plunky Exp $  */
   2
   3 /*-
   4  * Copyright (c) 2005 Iain Hibbert.
   5  * Copyright (c) 2006 Itronix Inc.
   6  * All rights reserved.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  * 3. The name of Itronix Inc. may not be used to endorse
  17  *    or promote products derived from this software without specific
  18  *    prior written permission.
  19  *
  20  * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``AS IS'' AND
  21  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
  22  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  23  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL ITRONIX INC. BE LIABLE FOR ANY
  24  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  25  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  26  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  27  * ON ANY THEORY OF LIABILITY, WHETHER IN
  28  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  29  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  30  * POSSIBILITY OF SUCH DAMAGE.
  31  */
  32
  33 #include <sys/cdefs.h>
  34 __KERNEL_RCSID(0, "$NetBSD: l2cap_signal.c,v 1.9 2007/11/10 23:12:23 plunky Exp $");
  35
  36 #include <sys/param.h>
  37 #include <sys/kernel.h>
  38 #include <sys/mbuf.h>
  39 #include <sys/proc.h>
  40 #include <sys/queue.h>
  41 #include <sys/systm.h>
  42
  43 #include <machine/stdarg.h>
  44
  45 #include <netbt/bluetooth.h>
  46 #include <netbt/hci.h>
  47 #include <netbt/l2cap.h>
  48
  49 /*******************************************************************************
  50  *
  51  *      L2CAP Signal processing
  52  */
  53
  54 static void l2cap_recv_command_rej(struct mbuf *, struct hci_link *);
  55 static void l2cap_recv_connect_req(struct mbuf *, struct hci_link *);
  56 static void l2cap_recv_connect_rsp(struct mbuf *, struct hci_link *);
  57 static void l2cap_recv_config_req(struct mbuf *, struct hci_link *);
  58 static void l2cap_recv_config_rsp(struct mbuf *, struct hci_link *);
  59 static void l2cap_recv_disconnect_req(struct mbuf *, struct hci_link *);
  60 static void l2cap_recv_disconnect_rsp(struct mbuf *, struct hci_link *);
  61 static void l2cap_recv_info_req(struct mbuf *, struct hci_link *);
  62 static int l2cap_send_signal(struct hci_link *, uint8_t, uint8_t, uint16_t, void *);
  63 static int l2cap_send_command_rej(struct hci_link *, uint8_t, uint16_t, ...);
  64
  65 /*
  66  * process incoming signal packets (CID 0x0001). Can contain multiple
  67  * requests/responses.
  68  */
  69 void
  70 l2cap_recv_signal(struct mbuf *m, struct hci_link *link)
  71 {
  72         l2cap_cmd_hdr_t cmd;
  73
  74         for(;;) {
  75                 if (m->m_pkthdr.len == 0)
  76                         goto finish;
  77
  78                 if (m->m_pkthdr.len < sizeof(cmd))
  79                         goto reject;
  80
  81                 m_copydata(m, 0, sizeof(cmd), &cmd);
  82                 cmd.length = le16toh(cmd.length);
  83
  84                 if (m->m_pkthdr.len < sizeof(cmd) + cmd.length)
  85                         goto reject;
  86
  87                 DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
  88                         device_xname(link->hl_unit->hci_dev),
  89                         cmd.code, cmd.ident, cmd.length);
  90
  91                 switch (cmd.code) {
  92                 case L2CAP_COMMAND_REJ:
  93                         if (cmd.length > sizeof(l2cap_cmd_rej_cp))
  94                                 goto finish;
  95
  96                         l2cap_recv_command_rej(m, link);
  97                         break;
  98
  99                 case L2CAP_CONNECT_REQ:
 100                         if (cmd.length != sizeof(l2cap_con_req_cp))
 101                                 goto reject;
 102
 103                         l2cap_recv_connect_req(m, link);
 104                         break;
 105
 106                 case L2CAP_CONNECT_RSP:
 107                         if (cmd.length != sizeof(l2cap_con_rsp_cp))
 108                                 goto finish;
 109
 110                         l2cap_recv_connect_rsp(m, link);
 111                         break;
 112
 113                 case L2CAP_CONFIG_REQ:
 114                         l2cap_recv_config_req(m, link);
 115                         break;
 116
 117                 case L2CAP_CONFIG_RSP:
 118                         l2cap_recv_config_rsp(m, link);
 119                         break;
 120
 121                 case L2CAP_DISCONNECT_REQ:
 122                         if (cmd.length != sizeof(l2cap_discon_req_cp))
 123                                 goto reject;
 124
 125                         l2cap_recv_disconnect_req(m, link);
 126                         break;
 127
 128                 case L2CAP_DISCONNECT_RSP:
 129                         if (cmd.length != sizeof(l2cap_discon_rsp_cp))
 130                                 goto finish;
 131
 132                         l2cap_recv_disconnect_rsp(m, link);
 133                         break;
 134
 135                 case L2CAP_ECHO_REQ:
 136                         m_adj(m, sizeof(cmd) + cmd.length);
 137                         l2cap_send_signal(link, L2CAP_ECHO_RSP, cmd.ident,
 138                                         0, NULL);
 139                         break;
 140
 141                 case L2CAP_ECHO_RSP:
 142                         m_adj(m, sizeof(cmd) + cmd.length);
 143                         break;
 144
 145                 case L2CAP_INFO_REQ:
 146                         if (cmd.length != sizeof(l2cap_info_req_cp))
 147                                 goto reject;
 148
 149                         l2cap_recv_info_req(m, link);
 150                         break;
 151
 152                 case L2CAP_INFO_RSP:
 153                         m_adj(m, sizeof(cmd) + cmd.length);
 154                         break;
 155
 156                 default:
 157                         goto reject;
 158                 }
 159         }
 160
 161 #ifdef DIAGNOSTIC
 162         panic("impossible!");
 163 #endif
 164
 165 reject:
 166         l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
 167 finish:
 168         m_freem(m);
 169 }
 170
 171 /*
 172  * Process Received Command Reject. For now we dont try to recover gracefully
 173  * from this, it probably means that the link is garbled or the other end is
 174  * insufficiently capable of handling normal traffic. (not *my* fault, no way!)
 175  */
 176 static void
 177 l2cap_recv_command_rej(struct mbuf *m, struct hci_link *link)
 178 {
 179         struct l2cap_req *req;
 180         struct l2cap_channel *chan;
 181         l2cap_cmd_hdr_t cmd;
 182         l2cap_cmd_rej_cp cp;
 183
 184         m_copydata(m, 0, sizeof(cmd), &cmd);
 185         m_adj(m, sizeof(cmd));
 186
 187         cmd.length = le16toh(cmd.length);
 188
 189         m_copydata(m, 0, cmd.length, &cp);
 190         m_adj(m, cmd.length);
 191
 192         req = l2cap_request_lookup(link, cmd.ident);
 193         if (req == NULL)
 194                 return;
 195
 196         switch (le16toh(cp.reason)) {
 197         case L2CAP_REJ_NOT_UNDERSTOOD:
 198                 /*
 199                  * I dont know what to do, just move up the timeout
 200                  */
 201                 callout_schedule(&req->lr_rtx, 0);
 202                 break;
 203
 204         case L2CAP_REJ_MTU_EXCEEDED:
 205                 /*
 206                  * I didnt send any commands over L2CAP_MTU_MINIMUM size, but..
 207                  *
 208                  * XXX maybe we should resend this, instead?
 209                  */
 210                 link->hl_mtu = le16toh(cp.data[0]);
 211                 callout_schedule(&req->lr_rtx, 0);
 212                 break;
 213
 214         case L2CAP_REJ_INVALID_CID:
 215                 /*
 216                  * Well, if they dont have such a channel then our channel is
 217                  * most likely closed. Make it so.
 218                  */
 219                 chan = req->lr_chan;
 220                 l2cap_request_free(req);
 221                 if (chan != NULL && chan->lc_state != L2CAP_CLOSED)
 222                         l2cap_close(chan, ECONNABORTED);
 223
 224                 break;
 225
 226         default:
 227                 UNKNOWN(le16toh(cp.reason));
 228                 break;
 229         }
 230 }
 231
 232 /*
 233  * Process Received Connect Request. Find listening channel matching
 234  * psm & addr and ask upper layer for a new channel.
 235  */
 236 static void
 237 l2cap_recv_connect_req(struct mbuf *m, struct hci_link *link)
 238 {
 239         struct sockaddr_bt laddr, raddr;
 240         struct l2cap_channel *chan, *new;
 241         l2cap_cmd_hdr_t cmd;
 242         l2cap_con_req_cp cp;
 243         int err;
 244
 245         /* extract cmd */
 246         m_copydata(m, 0, sizeof(cmd), &cmd);
 247         m_adj(m, sizeof(cmd));
 248
 249         /* extract request */
 250         m_copydata(m, 0, sizeof(cp), &cp);
 251         m_adj(m, sizeof(cp));
 252
 253         cp.scid = le16toh(cp.scid);
 254         cp.psm = le16toh(cp.psm);
 255
 256         memset(&laddr, 0, sizeof(struct sockaddr_bt));
 257         laddr.bt_len = sizeof(struct sockaddr_bt);
 258         laddr.bt_family = AF_BLUETOOTH;
 259         laddr.bt_psm = cp.psm;
 260         bdaddr_copy(&laddr.bt_bdaddr, &link->hl_unit->hci_bdaddr);
 261
 262         memset(&raddr, 0, sizeof(struct sockaddr_bt));
 263         raddr.bt_len = sizeof(struct sockaddr_bt);
 264         raddr.bt_family = AF_BLUETOOTH;
 265         raddr.bt_psm = cp.psm;
 266         bdaddr_copy(&raddr.bt_bdaddr, &link->hl_bdaddr);
 267
 268         LIST_FOREACH(chan, &l2cap_listen_list, lc_ncid) {
 269                 if (chan->lc_laddr.bt_psm != laddr.bt_psm)
 270                         continue;
 271
 272                 if (!bdaddr_same(&laddr.bt_bdaddr, &chan->lc_laddr.bt_bdaddr)
 273                     && bdaddr_any(&chan->lc_laddr.bt_bdaddr) == 0)
 274                         continue;
 275
 276                 new= (*chan->lc_proto->newconn)(chan->lc_upper, &laddr, &raddr);
 277                 if (new == NULL)
 278                         continue;
 279
 280                 err = l2cap_cid_alloc(new);
 281                 if (err) {
 282                         l2cap_send_connect_rsp(link, cmd.ident,
 283                                                 0, cp.scid,
 284                                                 L2CAP_NO_RESOURCES);
 285
 286                         (*new->lc_proto->disconnected)(new->lc_upper, err);
 287                         return;
 288                 }
 289
 290                 new->lc_link = hci_acl_open(link->hl_unit, &link->hl_bdaddr);
 291                 KASSERT(new->lc_link == link);
 292
 293                 new->lc_rcid = cp.scid;
 294                 new->lc_ident = cmd.ident;
 295
 296                 memcpy(&new->lc_laddr, &laddr, sizeof(struct sockaddr_bt));
 297                 memcpy(&new->lc_raddr, &raddr, sizeof(struct sockaddr_bt));
 298
 299                 new->lc_mode = chan->lc_mode;
 300
 301                 err = l2cap_setmode(new);
 302                 if (err == EINPROGRESS) {
 303                         new->lc_state = L2CAP_WAIT_SEND_CONNECT_RSP;
 304                         (*new->lc_proto->connecting)(new->lc_upper);
 305                         return;
 306                 }
 307                 if (err) {
 308                         new->lc_state = L2CAP_CLOSED;
 309                         hci_acl_close(link, err);
 310                         new->lc_link = NULL;
 311
 312                         l2cap_send_connect_rsp(link, cmd.ident,
 313                                                 0, cp.scid,
 314                                                 L2CAP_NO_RESOURCES);
 315
 316                         (*new->lc_proto->disconnected)(new->lc_upper, err);
 317                         return;
 318                 }
 319
 320                 err = l2cap_send_connect_rsp(link, cmd.ident,
 321                                               new->lc_lcid, new->lc_rcid,
 322                                               L2CAP_SUCCESS);
 323                 if (err) {
 324                         l2cap_close(new, err);
 325                         return;
 326                 }
 327
 328                 new->lc_state = L2CAP_WAIT_CONFIG;
 329                 new->lc_flags |= (L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP);
 330                 err = l2cap_send_config_req(new);
 331                 if (err)
 332                         l2cap_close(new, err);
 333
 334                 return;
 335         }
 336
 337         l2cap_send_connect_rsp(link, cmd.ident,
 338                                 0, cp.scid,
 339                                 L2CAP_PSM_NOT_SUPPORTED);
 340 }
 341
 342 /*
 343  * Process Received Connect Response.
 344  */
 345 static void
 346 l2cap_recv_connect_rsp(struct mbuf *m, struct hci_link *link)
 347 {
 348         l2cap_cmd_hdr_t cmd;
 349         l2cap_con_rsp_cp cp;
 350         struct l2cap_req *req;
 351         struct l2cap_channel *chan;
 352
 353         m_copydata(m, 0, sizeof(cmd), &cmd);
 354         m_adj(m, sizeof(cmd));
 355
 356         m_copydata(m, 0, sizeof(cp), &cp);
 357         m_adj(m, sizeof(cp));
 358
 359         cp.scid = le16toh(cp.scid);
 360         cp.dcid = le16toh(cp.dcid);
 361         cp.result = le16toh(cp.result);
 362
 363         req = l2cap_request_lookup(link, cmd.ident);
 364         if (req == NULL || req->lr_code != L2CAP_CONNECT_REQ)
 365                 return;
 366
 367         chan = req->lr_chan;
 368         if (chan != NULL && chan->lc_lcid != cp.scid)
 369                 return;
 370
 371         if (chan == NULL || chan->lc_state != L2CAP_WAIT_RECV_CONNECT_RSP) {
 372                 l2cap_request_free(req);
 373                 return;
 374         }
 375
 376         switch (cp.result) {
 377         case L2CAP_SUCCESS:
 378                 /*
 379                  * Ok, at this point we have a connection to the other party. We
 380                  * could indicate upstream that we are ready for business and
 381                  * wait for a "Configure Channel Request" but I'm not so sure
 382                  * that is required in our case - we will proceed directly to
 383                  * sending our config request. We set two state bits because in
 384                  * the config state we are waiting for requests and responses.
 385                  */
 386                 l2cap_request_free(req);
 387                 chan->lc_rcid = cp.dcid;
 388                 chan->lc_state = L2CAP_WAIT_CONFIG;
 389                 chan->lc_flags |= (L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP);
 390                 l2cap_send_config_req(chan);
 391                 break;
 392
 393         case L2CAP_PENDING:
 394                 /* XXX dont release request, should start eRTX timeout? */
 395                 (*chan->lc_proto->connecting)(chan->lc_upper);
 396                 break;
 397
 398         case L2CAP_PSM_NOT_SUPPORTED:
 399         case L2CAP_SECURITY_BLOCK:
 400         case L2CAP_NO_RESOURCES:
 401         default:
 402                 l2cap_request_free(req);
 403                 l2cap_close(chan, ECONNREFUSED);
 404                 break;
 405         }
 406 }
 407
 408 /*
 409  * Process Received Config Reqest.
 410  */
 411 static void
 412 l2cap_recv_config_req(struct mbuf *m, struct hci_link *link)
 413 {
 414         uint8_t buf[L2CAP_MTU_MINIMUM];
 415         l2cap_cmd_hdr_t cmd;
 416         l2cap_cfg_req_cp cp;
 417         l2cap_cfg_opt_t opt;
 418         l2cap_cfg_opt_val_t val;
 419         l2cap_cfg_rsp_cp rp;
 420         struct l2cap_channel *chan;
 421         int left, len;
 422
 423         m_copydata(m, 0, sizeof(cmd), &cmd);
 424         m_adj(m, sizeof(cmd));
 425         left = le16toh(cmd.length);
 426
 427         if (left < sizeof(cp))
 428                 goto reject;
 429
 430         m_copydata(m, 0, sizeof(cp), &cp);
 431         m_adj(m, sizeof(cp));
 432         left -= sizeof(cp);
 433
 434         cp.dcid = le16toh(cp.dcid);
 435         cp.flags = le16toh(cp.flags);
 436
 437         chan = l2cap_cid_lookup(cp.dcid);
 438         if (chan == NULL || chan->lc_link != link
 439             || chan->lc_state != L2CAP_WAIT_CONFIG
 440             || (chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) {
 441                 /* XXX we should really accept reconfiguration requests */
 442                 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
 443                                         L2CAP_NULL_CID, cp.dcid);
 444                 goto out;
 445         }
 446
 447         /* ready our response packet */
 448         rp.scid = htole16(chan->lc_rcid);
 449         rp.flags = 0;   /* "No Continuation" */
 450         rp.result = L2CAP_SUCCESS;
 451         len = sizeof(rp);
 452
 453         /*
 454          * Process the packet. We build the return packet on the fly adding any
 455          * unacceptable parameters as we go. As we can only return one result,
 456          * unknown option takes precedence so we start our return packet anew
 457          * and ignore option values thereafter as they will be re-sent.
 458          *
 459          * Since we do not support enough options to make overflowing the min
 460          * MTU size an issue in normal use, we just reject config requests that
 461          * make that happen. This could be because options are repeated or the
 462          * packet is corrupted in some way.
 463          *
 464          * If unknown option types threaten to overflow the packet, we just
 465          * ignore them. We can deny them next time.
 466          */
 467         while (left > 0) {
 468                 if (left < sizeof(opt))
 469                         goto reject;
 470
 471                 m_copydata(m, 0, sizeof(opt), &opt);
 472                 m_adj(m, sizeof(opt));
 473                 left -= sizeof(opt);
 474
 475                 if (left < opt.length)
 476                         goto reject;
 477
 478                 switch(opt.type & L2CAP_OPT_HINT_MASK) {
 479                 case L2CAP_OPT_MTU:
 480                         if (rp.result == L2CAP_UNKNOWN_OPTION)
 481                                 break;
 482
 483                         if (opt.length != L2CAP_OPT_MTU_SIZE)
 484                                 goto reject;
 485
 486                         m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val);
 487                         val.mtu = le16toh(val.mtu);
 488
 489                         /*
 490                          * XXX how do we know what the minimum acceptable MTU is
 491                          * for a channel? Spec says some profiles have a higher
 492                          * minimum but I have no way to find that out at this
 493                          * juncture..
 494                          */
 495                         if (val.mtu < L2CAP_MTU_MINIMUM) {
 496                                 if (len + sizeof(opt) + L2CAP_OPT_MTU_SIZE > sizeof(buf))
 497                                         goto reject;
 498
 499                                 rp.result = L2CAP_UNACCEPTABLE_PARAMS;
 500                                 memcpy(buf + len, &opt, sizeof(opt));
 501                                 len += sizeof(opt);
 502                                 val.mtu = htole16(L2CAP_MTU_MINIMUM);
 503                                 memcpy(buf + len, &val, L2CAP_OPT_MTU_SIZE);
 504                                 len += L2CAP_OPT_MTU_SIZE;
 505                         } else
 506                                 chan->lc_omtu = val.mtu;
 507
 508                         break;
 509
 510                 case L2CAP_OPT_FLUSH_TIMO:
 511                         if (rp.result == L2CAP_UNKNOWN_OPTION)
 512                                 break;
 513
 514                         if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE)
 515                                 goto reject;
 516
 517                         /*
 518                          * I think that this is informational only - he is
 519                          * informing us of the flush timeout he will be using.
 520                          * I dont think this affects us in any significant way,
 521                          * so just ignore this value for now.
 522                          */
 523                         break;
 524
 525                 case L2CAP_OPT_QOS:
 526                 default:
 527                         /* ignore hints */
 528                         if (opt.type & L2CAP_OPT_HINT_BIT)
 529                                 break;
 530
 531                         /* unknown options supercede all else */
 532                         if (rp.result != L2CAP_UNKNOWN_OPTION) {
 533                                 rp.result = L2CAP_UNKNOWN_OPTION;
 534                                 len = sizeof(rp);
 535                         }
 536
 537                         /* ignore if it don't fit */
 538                         if (len + sizeof(opt) > sizeof(buf))
 539                                 break;
 540
 541                         /* return unknown option type, but no data */
 542                         buf[len++] = opt.type;
 543                         buf[len++] = 0;
 544                         break;
 545                 }
 546
 547                 m_adj(m, opt.length);
 548                 left -= opt.length;
 549         }
 550
 551         rp.result = htole16(rp.result);
 552         memcpy(buf, &rp, sizeof(rp));
 553         l2cap_send_signal(link, L2CAP_CONFIG_RSP, cmd.ident, len, buf);
 554
 555         if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0
 556             && rp.result == le16toh(L2CAP_SUCCESS)) {
 557
 558                 chan->lc_flags &= ~L2CAP_WAIT_CONFIG_REQ;
 559
 560                 if ((chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0) {
 561                         chan->lc_state = L2CAP_OPEN;
 562                         /* XXX how to distinguish REconfiguration? */
 563                         (*chan->lc_proto->connected)(chan->lc_upper);
 564                 }
 565         }
 566         return;
 567
 568 reject:
 569         l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
 570 out:
 571         m_adj(m, left);
 572 }
 573
 574 /*
 575  * Process Received Config Response.
 576  */
 577 static void
 578 l2cap_recv_config_rsp(struct mbuf *m, struct hci_link *link)
 579 {
 580         l2cap_cmd_hdr_t cmd;
 581         l2cap_cfg_rsp_cp cp;
 582         l2cap_cfg_opt_t opt;
 583         l2cap_cfg_opt_val_t val;
 584         struct l2cap_req *req;
 585         struct l2cap_channel *chan;
 586         int left;
 587
 588         m_copydata(m, 0, sizeof(cmd), &cmd);
 589         m_adj(m, sizeof(cmd));
 590         left = le16toh(cmd.length);
 591
 592         if (left < sizeof(cp))
 593                 goto out;
 594
 595         m_copydata(m, 0, sizeof(cp), &cp);
 596         m_adj(m, sizeof(cp));
 597         left -= sizeof(cp);
 598
 599         cp.scid = le16toh(cp.scid);
 600         cp.flags = le16toh(cp.flags);
 601         cp.result = le16toh(cp.result);
 602
 603         req = l2cap_request_lookup(link, cmd.ident);
 604         if (req == NULL || req->lr_code != L2CAP_CONFIG_REQ)
 605                 goto out;
 606
 607         chan = req->lr_chan;
 608         if (chan != NULL && chan->lc_lcid != cp.scid)
 609                 goto out;
 610
 611         l2cap_request_free(req);
 612
 613         if (chan == NULL || chan->lc_state != L2CAP_WAIT_CONFIG
 614             || (chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0)
 615                 goto out;
 616
 617         if ((cp.flags & L2CAP_OPT_CFLAG_BIT)) {
 618                 l2cap_cfg_req_cp rp;
 619
 620                 /*
 621                  * They have more to tell us and want another ID to
 622                  * use, so send an empty config request
 623                  */
 624                 if (l2cap_request_alloc(chan, L2CAP_CONFIG_REQ))
 625                         goto discon;
 626
 627                 rp.dcid = htole16(cp.scid);
 628                 rp.flags = 0;
 629
 630                 if (l2cap_send_signal(link, L2CAP_CONFIG_REQ, link->hl_lastid,
 631                                         sizeof(rp), &rp))
 632                         goto discon;
 633         }
 634
 635         switch(cp.result) {
 636         case L2CAP_SUCCESS:
 637                 /*
 638                  * If continuation flag was not set, our config request was
 639                  * accepted. We may have to wait for their config request to
 640                  * complete, so check that but otherwise we are open
 641                  *
 642                  * There may be 'advisory' values in the packet but we just
 643                  * ignore those..
 644                  */
 645                 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) {
 646                         chan->lc_flags &= ~L2CAP_WAIT_CONFIG_RSP;
 647
 648                         if ((chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) {
 649                                 chan->lc_state = L2CAP_OPEN;
 650                                 /* XXX how to distinguish REconfiguration? */
 651                                 (*chan->lc_proto->connected)(chan->lc_upper);
 652                         }
 653                 }
 654                 goto out;
 655
 656         case L2CAP_UNACCEPTABLE_PARAMS:
 657                 /*
 658                  * Packet contains unacceptable parameters with preferred values
 659                  */
 660                 while (left > 0) {
 661                         if (left < sizeof(opt))
 662                                 goto discon;
 663
 664                         m_copydata(m, 0, sizeof(opt), &opt);
 665                         m_adj(m, sizeof(opt));
 666                         left -= sizeof(opt);
 667
 668                         if (left < opt.length)
 669                                 goto discon;
 670
 671                         switch (opt.type) {
 672                         case L2CAP_OPT_MTU:
 673                                 if (opt.length != L2CAP_OPT_MTU_SIZE)
 674                                         goto discon;
 675
 676                                 m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val);
 677                                 chan->lc_imtu = le16toh(val.mtu);
 678                                 if (chan->lc_imtu < L2CAP_MTU_MINIMUM)
 679                                         chan->lc_imtu = L2CAP_MTU_DEFAULT;
 680                                 break;
 681
 682                         case L2CAP_OPT_FLUSH_TIMO:
 683                                 if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE)
 684                                         goto discon;
 685
 686                                 /*
 687                                  * Spec says: If we cannot honor proposed value,
 688                                  * either disconnect or try again with original
 689                                  * value. I can't really see why they want to
 690                                  * interfere with OUR flush timeout in any case
 691                                  * so we just punt for now.
 692                                  */
 693                                 goto discon;
 694
 695                         case L2CAP_OPT_QOS:
 696                                 break;
 697
 698                         default:
 699                                 UNKNOWN(opt.type);
 700                                 goto discon;
 701                         }
 702
 703                         m_adj(m, opt.length);
 704                         left -= opt.length;
 705                 }
 706
 707                 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
 708                         l2cap_send_config_req(chan);    /* no state change */
 709
 710                 goto out;
 711
 712         case L2CAP_REJECT:
 713                 goto discon;
 714
 715         case L2CAP_UNKNOWN_OPTION:
 716                 /*
 717                  * Packet contains options not understood. Turn off unknown
 718                  * options by setting them to default values (means they will
 719                  * not be requested again).
 720                  *
 721                  * If our option was already off then fail (paranoia?)
 722                  *
 723                  * XXX Should we consider that options were set for a reason?
 724                  */
 725                 while (left > 0) {
 726                         if (left < sizeof(opt))
 727                                 goto discon;
 728
 729                         m_copydata(m, 0, sizeof(opt), &opt);
 730                         m_adj(m, sizeof(opt));
 731                         left -= sizeof(opt);
 732
 733                         if (left < opt.length)
 734                                 goto discon;
 735
 736                         m_adj(m, opt.length);
 737                         left -= opt.length;
 738
 739                         switch(opt.type) {
 740                         case L2CAP_OPT_MTU:
 741                                 if (chan->lc_imtu == L2CAP_MTU_DEFAULT)
 742                                         goto discon;
 743
 744                                 chan->lc_imtu = L2CAP_MTU_DEFAULT;
 745                                 break;
 746
 747                         case L2CAP_OPT_FLUSH_TIMO:
 748                                 if (chan->lc_flush == L2CAP_FLUSH_TIMO_DEFAULT)
 749                                         goto discon;
 750
 751                                 chan->lc_flush = L2CAP_FLUSH_TIMO_DEFAULT;
 752                                 break;
 753
 754                         case L2CAP_OPT_QOS:
 755                                 break;
 756
 757                         default:
 758                                 UNKNOWN(opt.type);
 759                                 goto discon;
 760                         }
 761                 }
 762
 763                 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
 764                         l2cap_send_config_req(chan);    /* no state change */
 765
 766                 goto out;
 767
 768         default:
 769                 UNKNOWN(cp.result);
 770                 goto discon;
 771         }
 772
 773         DPRINTF("how did I get here!?\n");
 774
 775 discon:
 776         l2cap_send_disconnect_req(chan);
 777         l2cap_close(chan, ECONNABORTED);
 778
 779 out:
 780         m_adj(m, left);
 781 }
 782
 783 /*
 784  * Process Received Disconnect Request. We must validate scid and dcid
 785  * just in case but otherwise this connection is finished.
 786  */
 787 static void
 788 l2cap_recv_disconnect_req(struct mbuf *m, struct hci_link *link)
 789 {
 790         l2cap_cmd_hdr_t cmd;
 791         l2cap_discon_req_cp cp;
 792         l2cap_discon_rsp_cp rp;
 793         struct l2cap_channel *chan;
 794
 795         m_copydata(m, 0, sizeof(cmd), &cmd);
 796         m_adj(m, sizeof(cmd));
 797
 798         m_copydata(m, 0, sizeof(cp), &cp);
 799         m_adj(m, sizeof(cp));
 800
 801         cp.scid = le16toh(cp.scid);
 802         cp.dcid = le16toh(cp.dcid);
 803
 804         chan = l2cap_cid_lookup(cp.dcid);
 805         if (chan == NULL || chan->lc_link != link || chan->lc_rcid != cp.scid) {
 806                 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
 807                                         cp.dcid, cp.scid);
 808                 return;
 809         }
 810
 811         rp.dcid = htole16(chan->lc_lcid);
 812         rp.scid = htole16(chan->lc_rcid);
 813         l2cap_send_signal(link, L2CAP_DISCONNECT_RSP, cmd.ident,
 814                                 sizeof(rp), &rp);
 815
 816         if (chan->lc_state != L2CAP_CLOSED)
 817                 l2cap_close(chan, ECONNRESET);
 818 }
 819
 820 /*
 821  * Process Received Disconnect Response. We must validate scid and dcid but
 822  * unless we were waiting for this signal, ignore it.
 823  */
 824 static void
 825 l2cap_recv_disconnect_rsp(struct mbuf *m, struct hci_link *link)
 826 {
 827         l2cap_cmd_hdr_t cmd;
 828         l2cap_discon_rsp_cp cp;
 829         struct l2cap_req *req;
 830         struct l2cap_channel *chan;
 831
 832         m_copydata(m, 0, sizeof(cmd), &cmd);
 833         m_adj(m, sizeof(cmd));
 834
 835         m_copydata(m, 0, sizeof(cp), &cp);
 836         m_adj(m, sizeof(cp));
 837
 838         cp.scid = le16toh(cp.scid);
 839         cp.dcid = le16toh(cp.dcid);
 840
 841         req = l2cap_request_lookup(link, cmd.ident);
 842         if (req == NULL || req->lr_code != L2CAP_DISCONNECT_REQ)
 843                 return;
 844
 845         chan = req->lr_chan;
 846         if (chan == NULL
 847             || chan->lc_lcid != cp.scid
 848             || chan->lc_rcid != cp.dcid)
 849                 return;
 850
 851         l2cap_request_free(req);
 852
 853         if (chan->lc_state != L2CAP_WAIT_DISCONNECT)
 854                 return;
 855
 856         l2cap_close(chan, 0);
 857 }
 858
 859 /*
 860  * Process Received Info Request. We must respond but alas dont
 861  * support anything as yet so thats easy.
 862  */
 863 static void
 864 l2cap_recv_info_req(struct mbuf *m, struct hci_link *link)
 865 {
 866         l2cap_cmd_hdr_t cmd;
 867         l2cap_info_req_cp cp;
 868         l2cap_info_rsp_cp rp;
 869
 870         m_copydata(m, 0, sizeof(cmd), &cmd);
 871         m_adj(m, sizeof(cmd));
 872
 873         m_copydata(m, 0, sizeof(cp), &cp);
 874         m_adj(m, sizeof(cp));
 875
 876         switch(le16toh(cp.type)) {
 877         case L2CAP_CONNLESS_MTU:
 878         case L2CAP_EXTENDED_FEATURES:
 879         default:
 880                 rp.type = cp.type;
 881                 rp.result = htole16(L2CAP_NOT_SUPPORTED);
 882
 883                 l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident,
 884                                         sizeof(rp), &rp);
 885                 break;
 886         }
 887 }
 888
 889 /*
 890  * Construct signal and wrap in C-Frame for link.
 891  */
 892 static int
 893 l2cap_send_signal(struct hci_link *link, uint8_t code, uint8_t ident,
 894                         uint16_t length, void *data)
 895 {
 896         struct mbuf *m;
 897         l2cap_hdr_t *hdr;
 898         l2cap_cmd_hdr_t *cmd;
 899
 900 #ifdef DIAGNOSTIC
 901         if (link == NULL)
 902                 return ENETDOWN;
 903
 904         if (sizeof(l2cap_cmd_hdr_t) + length > link->hl_mtu)
 905                 aprint_error_dev(link->hl_unit->hci_dev,
 906                     "exceeding L2CAP Signal MTU for link!\n");
 907 #endif
 908
 909         m = m_gethdr(M_DONTWAIT, MT_DATA);
 910         if (m == NULL)
 911                 return ENOMEM;
 912
 913         hdr = mtod(m, l2cap_hdr_t *);
 914         cmd = (l2cap_cmd_hdr_t *)(hdr + 1);
 915
 916         m->m_len = m->m_pkthdr.len = MHLEN;
 917
 918         /* Command Data */
 919         if (length > 0)
 920                 m_copyback(m, sizeof(*hdr) + sizeof(*cmd), length, data);
 921
 922         /* Command Header */
 923         cmd->code = code;
 924         cmd->ident = ident;
 925         cmd->length = htole16(length);
 926         length += sizeof(*cmd);
 927
 928         /* C-Frame Header */
 929         hdr->length = htole16(length);
 930         hdr->dcid = htole16(L2CAP_SIGNAL_CID);
 931         length += sizeof(*hdr);
 932
 933         if (m->m_pkthdr.len != MAX(MHLEN, length)) {
 934                 m_freem(m);
 935                 return ENOMEM;
 936         }
 937
 938         m->m_pkthdr.len = length;
 939         m->m_len = MIN(length, MHLEN);
 940
 941         DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
 942                 device_xname(link->hl_unit->hci_dev), code, ident, length);
 943
 944         return hci_acl_send(m, link, NULL);
 945 }
 946
 947 /*
 948  * Send Command Reject packet.
 949  */
 950 static int
 951 l2cap_send_command_rej(struct hci_link *link, uint8_t ident,
 952                         uint16_t reason, ...)
 953 {
 954         l2cap_cmd_rej_cp cp;
 955         int len = 0;
 956         va_list ap;
 957
 958         va_start(ap, reason);
 959
 960         cp.reason = htole16(reason);
 961
 962         switch (reason) {
 963         case L2CAP_REJ_NOT_UNDERSTOOD:
 964                 len = 2;
 965                 break;
 966
 967         case L2CAP_REJ_MTU_EXCEEDED:
 968                 len = 4;
 969                 cp.data[0] = va_arg(ap, int);           /* SigMTU */
 970                 cp.data[0] = htole16(cp.data[0]);
 971                 break;
 972
 973         case L2CAP_REJ_INVALID_CID:
 974                 len = 6;
 975                 cp.data[0] = va_arg(ap, int);           /* dcid */
 976                 cp.data[0] = htole16(cp.data[0]);
 977                 cp.data[1] = va_arg(ap, int);           /* scid */
 978                 cp.data[1] = htole16(cp.data[1]);
 979                 break;
 980
 981         default:
 982                 UNKNOWN(reason);
 983                 return EINVAL;
 984         }
 985
 986         va_end(ap);
 987
 988         return l2cap_send_signal(link, L2CAP_COMMAND_REJ, ident, len, &cp);
 989 }
 990
 991 /*
 992  * Send Connect Request
 993  */
 994 int
 995 l2cap_send_connect_req(struct l2cap_channel *chan)
 996 {
 997         l2cap_con_req_cp cp;
 998         int err;
 999
1000         err = l2cap_request_alloc(chan, L2CAP_CONNECT_REQ);
1001         if (err)
1002                 return err;
1003
1004         cp.psm = htole16(chan->lc_raddr.bt_psm);
1005         cp.scid = htole16(chan->lc_lcid);
1006
1007         return l2cap_send_signal(chan->lc_link, L2CAP_CONNECT_REQ,
1008                                 chan->lc_link->hl_lastid, sizeof(cp), &cp);
1009 }
1010
1011 /*
1012  * Send Config Request
1013  *
1014  * For outgoing config request, we only put options in the packet if they
1015  * differ from the default and would have to be actioned. We dont support
1016  * enough option types to make overflowing SigMTU an issue so it can all
1017  * go in one packet.
1018  */
1019 int
1020 l2cap_send_config_req(struct l2cap_channel *chan)
1021 {
1022         l2cap_cfg_req_cp *cp;
1023         l2cap_cfg_opt_t *opt;
1024         l2cap_cfg_opt_val_t *val;
1025         uint8_t *next, buf[L2CAP_MTU_MINIMUM];
1026         int err;
1027
1028         err = l2cap_request_alloc(chan, L2CAP_CONFIG_REQ);
1029         if (err)
1030                 return err;
1031
1032         /* Config Header (4 octets) */
1033         cp = (l2cap_cfg_req_cp *)buf;
1034         cp->dcid = htole16(chan->lc_rcid);
1035         cp->flags = 0;  /* "No Continuation" */
1036
1037         next = buf + sizeof(l2cap_cfg_req_cp);
1038
1039         /* Incoming MTU (4 octets) */
1040         if (chan->lc_imtu != L2CAP_MTU_DEFAULT) {
1041                 opt = (l2cap_cfg_opt_t *)next;
1042                 opt->type = L2CAP_OPT_MTU;
1043                 opt->length = L2CAP_OPT_MTU_SIZE;
1044
1045                 val = (l2cap_cfg_opt_val_t *)(opt + 1);
1046                 val->mtu = htole16(chan->lc_imtu);
1047
1048                 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_MTU_SIZE;
1049         }
1050
1051         /* Flush Timeout (4 octets) */
1052         if (chan->lc_flush != L2CAP_FLUSH_TIMO_DEFAULT) {
1053                 opt = (l2cap_cfg_opt_t *)next;
1054                 opt->type = L2CAP_OPT_FLUSH_TIMO;
1055                 opt->length = L2CAP_OPT_FLUSH_TIMO_SIZE;
1056
1057                 val = (l2cap_cfg_opt_val_t *)(opt + 1);
1058                 val->flush_timo = htole16(chan->lc_flush);
1059
1060                 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_FLUSH_TIMO_SIZE;
1061         }
1062
1063         /* Outgoing QoS Flow (24 octets) */
1064         /* Retransmission & Flow Control (11 octets) */
1065         /*
1066          * From here we need to start paying attention to SigMTU as we have
1067          * possibly overflowed the minimum supported..
1068          */
1069
1070         return l2cap_send_signal(chan->lc_link, L2CAP_CONFIG_REQ,
1071                                     chan->lc_link->hl_lastid, (int)(next - buf), buf);
1072 }
1073
1074 /*
1075  * Send Disconnect Request
1076  */
1077 int
1078 l2cap_send_disconnect_req(struct l2cap_channel *chan)
1079 {
1080         l2cap_discon_req_cp cp;
1081         int err;
1082
1083         err = l2cap_request_alloc(chan, L2CAP_DISCONNECT_REQ);
1084         if (err)
1085                 return err;
1086
1087         cp.dcid = htole16(chan->lc_rcid);
1088         cp.scid = htole16(chan->lc_lcid);
1089
1090         return l2cap_send_signal(chan->lc_link, L2CAP_DISCONNECT_REQ,
1091                                     chan->lc_link->hl_lastid, sizeof(cp), &cp);
1092 }
1093
1094 /*
1095  * Send Connect Response
1096  */
1097 int
1098 l2cap_send_connect_rsp(struct hci_link *link, uint8_t ident, uint16_t dcid, uint16_t scid, uint16_t result)
1099 {
1100         l2cap_con_rsp_cp cp;
1101
1102         memset(&cp, 0, sizeof(cp));
1103         cp.dcid = htole16(dcid);
1104         cp.scid = htole16(scid);
1105         cp.result = htole16(result);
1106
1107         return l2cap_send_signal(link, L2CAP_CONNECT_RSP, ident, sizeof(cp), &cp);
1108 }