sys/netbt/l2cap_signal.c

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