sys/netbt/l2cap_signal.c

   1 /* $OpenBSD: src/sys/netbt/l2cap_signal.c,v 1.3 2008/02/24 21:34:48 uwe Exp $ */
   2 /* $NetBSD: l2cap_signal.c,v 1.9 2007/11/10 23:12:23 plunky Exp $ */
   3
   4 /*-
   5  * Copyright (c) 2005 Iain Hibbert.
   6  * Copyright (c) 2006 Itronix Inc.
   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  * 3. The name of Itronix Inc. may not be used to endorse
  18  *    or promote products derived from this software without specific
  19  *    prior written permission.
  20  *
  21  * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``AS IS'' AND
  22  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
  23  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  24  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL ITRONIX INC. BE LIABLE FOR ANY
  25  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  26  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  28  * ON ANY THEORY OF LIABILITY, WHETHER IN
  29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  31  * POSSIBILITY OF SUCH DAMAGE.
  32  */
  33
  34 #include <stdarg.h>
  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 #include <sys/endian.h>
  43
  44 #include <netbt/bluetooth.h>
  45 #include <netbt/hci.h>
  46 #include <netbt/l2cap.h>
  47
  48 /*******************************************************************************
  49  *
  50  *      L2CAP Signal processing
  51  */
  52
  53 static void l2cap_recv_command_rej(struct mbuf *, struct hci_link *);
  54 static void l2cap_recv_connect_req(struct mbuf *, struct hci_link *);
  55 static void l2cap_recv_connect_rsp(struct mbuf *, struct hci_link *);
  56 static void l2cap_recv_config_req(struct mbuf *, struct hci_link *);
  57 static void l2cap_recv_config_rsp(struct mbuf *, struct hci_link *);
  58 static void l2cap_recv_disconnect_req(struct mbuf *, struct hci_link *);
  59 static void l2cap_recv_disconnect_rsp(struct mbuf *, struct hci_link *);
  60 static void l2cap_recv_info_req(struct mbuf *, struct hci_link *);
  61 static int l2cap_send_signal(struct hci_link *, uint8_t, uint8_t, uint16_t, void *);
  62 static int l2cap_send_command_rej(struct hci_link *, uint8_t, uint16_t, ...);
  63
  64 /*
  65  * process incoming signal packets (CID 0x0001). Can contain multiple
  66  * requests/responses.
  67  */
  68 void
  69 l2cap_recv_signal(struct mbuf *m, struct hci_link *link)
  70 {
  71         l2cap_cmd_hdr_t cmd;
  72
  73         for(;;) {
  74                 if (m->m_pkthdr.len == 0)
  75                         goto finish;
  76
  77                 if (m->m_pkthdr.len < sizeof(cmd))
  78                         goto reject;
  79
  80                 m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
  81                 cmd.length = letoh16(cmd.length);
  82
  83                 if (m->m_pkthdr.len < sizeof(cmd) + cmd.length)
  84                         goto reject;
  85
  86                 DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
  87                         device_get_nameunit(link->hl_unit->hci_dev),
  88                         cmd.code, cmd.ident, cmd.length);
  89
  90                 switch (cmd.code) {
  91                 case L2CAP_COMMAND_REJ:
  92                         if (cmd.length > sizeof(l2cap_cmd_rej_cp))
  93                                 goto finish;
  94
  95                         l2cap_recv_command_rej(m, link);
  96                         break;
  97
  98                 case L2CAP_CONNECT_REQ:
  99                         if (cmd.length != sizeof(l2cap_con_req_cp))
 100                                 goto reject;
 101
 102                         l2cap_recv_connect_req(m, link);
 103                         break;
 104
 105                 case L2CAP_CONNECT_RSP:
 106                         if (cmd.length != sizeof(l2cap_con_rsp_cp))
 107                                 goto finish;
 108
 109                         l2cap_recv_connect_rsp(m, link);
 110                         break;
 111
 112                 case L2CAP_CONFIG_REQ:
 113                         l2cap_recv_config_req(m, link);
 114                         break;
 115
 116                 case L2CAP_CONFIG_RSP:
 117                         l2cap_recv_config_rsp(m, link);
 118                         break;
 119
 120                 case L2CAP_DISCONNECT_REQ:
 121                         if (cmd.length != sizeof(l2cap_discon_req_cp))
 122                                 goto reject;
 123
 124                         l2cap_recv_disconnect_req(m, link);
 125                         break;
 126
 127                 case L2CAP_DISCONNECT_RSP:
 128                         if (cmd.length != sizeof(l2cap_discon_rsp_cp))
 129                                 goto finish;
 130
 131                         l2cap_recv_disconnect_rsp(m, link);
 132                         break;
 133
 134                 case L2CAP_ECHO_REQ:
 135                         m_adj(m, sizeof(cmd) + cmd.length);
 136                         l2cap_send_signal(link, L2CAP_ECHO_RSP, cmd.ident,
 137                                         0, NULL);
 138                         break;
 139
 140                 case L2CAP_ECHO_RSP:
 141                         m_adj(m, sizeof(cmd) + cmd.length);
 142                         break;
 143
 144                 case L2CAP_INFO_REQ:
 145                         if (cmd.length != sizeof(l2cap_info_req_cp))
 146                                 goto reject;
 147
 148                         l2cap_recv_info_req(m, link);
 149                         break;
 150
 151                 case L2CAP_INFO_RSP:
 152                         m_adj(m, sizeof(cmd) + cmd.length);
 153                         break;
 154
 155                 default:
 156                         goto reject;
 157                 }
 158         }
 159
 160 #ifdef DIAGNOSTIC
 161         panic("impossible!");
 162 #endif
 163
 164 reject:
 165         l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
 166 finish:
 167         m_freem(m);
 168 }
 169
 170 /*
 171  * Process Received Command Reject. For now we dont try to recover gracefully
 172  * from this, it probably means that the link is garbled or the other end is
 173  * insufficiently capable of handling normal traffic. (not *my* fault, no way!)
 174  */
 175 static void
 176 l2cap_recv_command_rej(struct mbuf *m, struct hci_link *link)
 177 {
 178         struct l2cap_req *req;
 179         struct l2cap_channel *chan;
 180         l2cap_cmd_hdr_t cmd;
 181         l2cap_cmd_rej_cp cp;
 182
 183         m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
 184         m_adj(m, sizeof(cmd));
 185
 186         cmd.length = letoh16(cmd.length);
 187
 188         m_copydata(m, 0, cmd.length, (caddr_t)&cp);
 189         m_adj(m, cmd.length);
 190
 191         req = l2cap_request_lookup(link, cmd.ident);
 192         if (req == NULL)
 193                 return;
 194
 195         switch (letoh16(cp.reason)) {
 196         case L2CAP_REJ_NOT_UNDERSTOOD:
 197                 /*
 198                  * I dont know what to do, just move up the timeout
 199                  */
 200                 callout_reset(&req->lr_rtx,0,l2cap_rtx,req);
 201                 break;
 202
 203         case L2CAP_REJ_MTU_EXCEEDED:
 204                 /*
 205                  * I didnt send any commands over L2CAP_MTU_MINIMUM size, but..
 206                  *
 207                  * XXX maybe we should resend this, instead?
 208                  */
 209                 link->hl_mtu = letoh16(cp.data[0]);
 210                 callout_reset(&req->lr_rtx,0,l2cap_rtx,req);
 211                 break;
 212
 213         case L2CAP_REJ_INVALID_CID:
 214                 /*
 215                  * Well, if they dont have such a channel then our channel is
 216                  * most likely closed. Make it so.
 217                  */
 218                 chan = req->lr_chan;
 219                 l2cap_request_free(req);
 220                 if (chan != NULL && chan->lc_state != L2CAP_CLOSED)
 221                         l2cap_close(chan, ECONNABORTED);
 222
 223                 break;
 224
 225         default:
 226                 UNKNOWN(letoh16(cp.reason));
 227                 break;
 228         }
 229 }
 230
 231 /*
 232  * Process Received Connect Request. Find listening channel matching
 233  * psm & addr and ask upper layer for a new channel.
 234  */
 235 static void
 236 l2cap_recv_connect_req(struct mbuf *m, struct hci_link *link)
 237 {
 238         struct sockaddr_bt laddr, raddr;
 239         struct l2cap_channel *chan, *new;
 240         l2cap_cmd_hdr_t cmd;
 241         l2cap_con_req_cp cp;
 242         int err;
 243
 244         /* extract cmd */
 245         m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
 246         m_adj(m, sizeof(cmd));
 247
 248         /* extract request */
 249         m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
 250         m_adj(m, sizeof(cp));
 251
 252         cp.scid = letoh16(cp.scid);
 253         cp.psm = letoh16(cp.psm);
 254
 255         memset(&laddr, 0, sizeof(struct sockaddr_bt));
 256         laddr.bt_len = sizeof(struct sockaddr_bt);
 257         laddr.bt_family = AF_BLUETOOTH;
 258         laddr.bt_psm = cp.psm;
 259         bdaddr_copy(&laddr.bt_bdaddr, &link->hl_unit->hci_bdaddr);
 260
 261         memset(&raddr, 0, sizeof(struct sockaddr_bt));
 262         raddr.bt_len = sizeof(struct sockaddr_bt);
 263         raddr.bt_family = AF_BLUETOOTH;
 264         raddr.bt_psm = cp.psm;
 265         bdaddr_copy(&raddr.bt_bdaddr, &link->hl_bdaddr);
 266
 267         LIST_FOREACH(chan, &l2cap_listen_list, lc_ncid) {
 268                 if (chan->lc_laddr.bt_psm != laddr.bt_psm
 269                     && chan->lc_laddr.bt_psm != L2CAP_PSM_ANY)
 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                 KKASSERT(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), (caddr_t)&cmd);
 354         m_adj(m, sizeof(cmd));
 355
 356         m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
 357         m_adj(m, sizeof(cp));
 358
 359         cp.scid = letoh16(cp.scid);
 360         cp.dcid = letoh16(cp.dcid);
 361         cp.result = letoh16(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 Request.
 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), (caddr_t)&cmd);
 424         m_adj(m, sizeof(cmd));
 425         left = letoh16(cmd.length);
 426
 427         if (left < sizeof(cp))
 428                 goto reject;
 429
 430         m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
 431         m_adj(m, sizeof(cp));
 432         left -= sizeof(cp);
 433
 434         cp.dcid = letoh16(cp.dcid);
 435         cp.flags = letoh16(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), (caddr_t)&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, (caddr_t)&val);
 487                         val.mtu = letoh16(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                         if (rp.result == L2CAP_UNKNOWN_OPTION)
 527                                 break;
 528
 529                         if (opt.length != L2CAP_OPT_QOS_SIZE)
 530                                 goto reject;
 531
 532                         m_copydata(m, 0, L2CAP_OPT_QOS_SIZE, (caddr_t)&val);
 533                         if (val.qos.service_type == L2CAP_QOS_NO_TRAFFIC ||
 534                             val.qos.service_type == L2CAP_QOS_BEST_EFFORT)
 535                                 /*
 536                                  * In accordance with the spec, we choose to
 537                                  * ignore the fields an provide no response.
 538                                  */
 539                                 break;
 540
 541                         if (len + sizeof(opt) + L2CAP_OPT_QOS_SIZE > sizeof(buf))
 542                                 goto reject;
 543
 544                         if (val.qos.service_type != L2CAP_QOS_GUARANTEED) {
 545                                 /*
 546                                  * Instead of sending an "unacceptable
 547                                  * parameters" response, treat this as an
 548                                  * unknown option and include the option
 549                                  * value in the response.
 550                                  */
 551                                 rp.result = L2CAP_UNKNOWN_OPTION;
 552                         } else {
 553                                 /*
 554                                  * According to the spec, we must return
 555                                  * specific values for wild card parameters.
 556                                  * I don't know what to return without lying,
 557                                  * so return "unacceptable parameters" and
 558                                  * specify the preferred service type as
 559                                  * "Best Effort".
 560                                  */
 561                                 rp.result = L2CAP_UNACCEPTABLE_PARAMS;
 562                                 val.qos.service_type = L2CAP_QOS_BEST_EFFORT;
 563                         }
 564
 565                         memcpy(buf + len, &opt, sizeof(opt));
 566                         len += sizeof(opt);
 567                         memcpy(buf + len, &val, L2CAP_OPT_QOS_SIZE);
 568                         len += L2CAP_OPT_QOS_SIZE;
 569                         break;
 570
 571                 default:
 572                         /* ignore hints */
 573                         if (opt.type & L2CAP_OPT_HINT_BIT)
 574                                 break;
 575
 576                         /* unknown options supersede all else */
 577                         if (rp.result != L2CAP_UNKNOWN_OPTION) {
 578                                 rp.result = L2CAP_UNKNOWN_OPTION;
 579                                 len = sizeof(rp);
 580                         }
 581
 582                         /* ignore if it doesn't fit */
 583                         if (len + sizeof(opt) > sizeof(buf))
 584                                 break;
 585
 586                         /* return unknown option type, but no data */
 587                         buf[len++] = opt.type;
 588                         buf[len++] = 0;
 589                         break;
 590                 }
 591
 592                 m_adj(m, opt.length);
 593                 left -= opt.length;
 594         }
 595
 596         rp.result = htole16(rp.result);
 597         memcpy(buf, &rp, sizeof(rp));
 598         l2cap_send_signal(link, L2CAP_CONFIG_RSP, cmd.ident, len, buf);
 599
 600         if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0
 601             && rp.result == letoh16(L2CAP_SUCCESS)) {
 602
 603                 chan->lc_flags &= ~L2CAP_WAIT_CONFIG_REQ;
 604
 605                 if ((chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0) {
 606                         chan->lc_state = L2CAP_OPEN;
 607                         /* XXX how to distinguish REconfiguration? */
 608                         (*chan->lc_proto->connected)(chan->lc_upper);
 609                 }
 610         }
 611         return;
 612
 613 reject:
 614         l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
 615 out:
 616         m_adj(m, left);
 617 }
 618
 619 /*
 620  * Process Received Config Response.
 621  */
 622 static void
 623 l2cap_recv_config_rsp(struct mbuf *m, struct hci_link *link)
 624 {
 625         l2cap_cmd_hdr_t cmd;
 626         l2cap_cfg_rsp_cp cp;
 627         l2cap_cfg_opt_t opt;
 628         l2cap_cfg_opt_val_t val;
 629         struct l2cap_req *req;
 630         struct l2cap_channel *chan;
 631         int left;
 632
 633         m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
 634         m_adj(m, sizeof(cmd));
 635         left = letoh16(cmd.length);
 636
 637         if (left < sizeof(cp))
 638                 goto out;
 639
 640         m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
 641         m_adj(m, sizeof(cp));
 642         left -= sizeof(cp);
 643
 644         cp.scid = letoh16(cp.scid);
 645         cp.flags = letoh16(cp.flags);
 646         cp.result = letoh16(cp.result);
 647
 648         req = l2cap_request_lookup(link, cmd.ident);
 649         if (req == NULL || req->lr_code != L2CAP_CONFIG_REQ)
 650                 goto out;
 651
 652         chan = req->lr_chan;
 653         if (chan != NULL && chan->lc_lcid != cp.scid)
 654                 goto out;
 655
 656         l2cap_request_free(req);
 657
 658         if (chan == NULL || chan->lc_state != L2CAP_WAIT_CONFIG
 659             || (chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0)
 660                 goto out;
 661
 662         if ((cp.flags & L2CAP_OPT_CFLAG_BIT)) {
 663                 l2cap_cfg_req_cp rp;
 664
 665                 /*
 666                  * They have more to tell us and want another ID to
 667                  * use, so send an empty config request
 668                  */
 669                 if (l2cap_request_alloc(chan, L2CAP_CONFIG_REQ))
 670                         goto discon;
 671
 672                 rp.dcid = htole16(cp.scid);
 673                 rp.flags = 0;
 674
 675                 if (l2cap_send_signal(link, L2CAP_CONFIG_REQ, link->hl_lastid,
 676                                         sizeof(rp), &rp))
 677                         goto discon;
 678         }
 679
 680         switch(cp.result) {
 681         case L2CAP_SUCCESS:
 682                 /*
 683                  * If continuation flag was not set, our config request was
 684                  * accepted. We may have to wait for their config request to
 685                  * complete, so check that but otherwise we are open
 686                  *
 687                  * There may be 'advisory' values in the packet but we just
 688                  * ignore those..
 689                  */
 690                 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) {
 691                         chan->lc_flags &= ~L2CAP_WAIT_CONFIG_RSP;
 692
 693                         if ((chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) {
 694                                 chan->lc_state = L2CAP_OPEN;
 695                                 /* XXX how to distinguish REconfiguration? */
 696                                 (*chan->lc_proto->connected)(chan->lc_upper);
 697                         }
 698                 }
 699                 goto out;
 700
 701         case L2CAP_UNACCEPTABLE_PARAMS:
 702                 /*
 703                  * Packet contains unacceptable parameters with preferred values
 704                  */
 705                 while (left > 0) {
 706                         if (left < sizeof(opt))
 707                                 goto discon;
 708
 709                         m_copydata(m, 0, sizeof(opt), (caddr_t)&opt);
 710                         m_adj(m, sizeof(opt));
 711                         left -= sizeof(opt);
 712
 713                         if (left < opt.length)
 714                                 goto discon;
 715
 716                         switch (opt.type) {
 717                         case L2CAP_OPT_MTU:
 718                                 if (opt.length != L2CAP_OPT_MTU_SIZE)
 719                                         goto discon;
 720
 721                                 m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, (caddr_t)&val);
 722                                 chan->lc_imtu = letoh16(val.mtu);
 723                                 if (chan->lc_imtu < L2CAP_MTU_MINIMUM)
 724                                         chan->lc_imtu = L2CAP_MTU_DEFAULT;
 725                                 break;
 726
 727                         case L2CAP_OPT_FLUSH_TIMO:
 728                                 if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE)
 729                                         goto discon;
 730
 731                                 /*
 732                                  * Spec says: If we cannot honor proposed value,
 733                                  * either disconnect or try again with original
 734                                  * value. I can't really see why they want to
 735                                  * interfere with OUR flush timeout in any case
 736                                  * so we just punt for now.
 737                                  */
 738                                 goto discon;
 739
 740                         case L2CAP_OPT_QOS:
 741                                 break;
 742
 743                         default:
 744                                 UNKNOWN(opt.type);
 745                                 goto discon;
 746                         }
 747
 748                         m_adj(m, opt.length);
 749                         left -= opt.length;
 750                 }
 751
 752                 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
 753                         l2cap_send_config_req(chan);    /* no state change */
 754
 755                 goto out;
 756
 757         case L2CAP_REJECT:
 758                 goto discon;
 759
 760         case L2CAP_UNKNOWN_OPTION:
 761                 /*
 762                  * Packet contains options not understood. Turn off unknown
 763                  * options by setting them to default values (means they will
 764                  * not be requested again).
 765                  *
 766                  * If our option was already off then fail (paranoia?)
 767                  *
 768                  * XXX Should we consider that options were set for a reason?
 769                  */
 770                 while (left > 0) {
 771                         if (left < sizeof(opt))
 772                                 goto discon;
 773
 774                         m_copydata(m, 0, sizeof(opt), (caddr_t)&opt);
 775                         m_adj(m, sizeof(opt));
 776                         left -= sizeof(opt);
 777
 778                         if (left < opt.length)
 779                                 goto discon;
 780
 781                         m_adj(m, opt.length);
 782                         left -= opt.length;
 783
 784                         switch(opt.type) {
 785                         case L2CAP_OPT_MTU:
 786                                 if (chan->lc_imtu == L2CAP_MTU_DEFAULT)
 787                                         goto discon;
 788
 789                                 chan->lc_imtu = L2CAP_MTU_DEFAULT;
 790                                 break;
 791
 792                         case L2CAP_OPT_FLUSH_TIMO:
 793                                 if (chan->lc_flush == L2CAP_FLUSH_TIMO_DEFAULT)
 794                                         goto discon;
 795
 796                                 chan->lc_flush = L2CAP_FLUSH_TIMO_DEFAULT;
 797                                 break;
 798
 799                         case L2CAP_OPT_QOS:
 800                                 break;
 801
 802                         default:
 803                                 UNKNOWN(opt.type);
 804                                 goto discon;
 805                         }
 806                 }
 807
 808                 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
 809                         l2cap_send_config_req(chan);    /* no state change */
 810
 811                 goto out;
 812
 813         default:
 814                 UNKNOWN(cp.result);
 815                 goto discon;
 816         }
 817
 818         DPRINTF("how did I get here!?\n");
 819
 820 discon:
 821         l2cap_send_disconnect_req(chan);
 822         l2cap_close(chan, ECONNABORTED);
 823
 824 out:
 825         m_adj(m, left);
 826 }
 827
 828 /*
 829  * Process Received Disconnect Request. We must validate scid and dcid
 830  * just in case but otherwise this connection is finished.
 831  */
 832 static void
 833 l2cap_recv_disconnect_req(struct mbuf *m, struct hci_link *link)
 834 {
 835         l2cap_cmd_hdr_t cmd;
 836         l2cap_discon_req_cp cp;
 837         l2cap_discon_rsp_cp rp;
 838         struct l2cap_channel *chan;
 839
 840         m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
 841         m_adj(m, sizeof(cmd));
 842
 843         m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
 844         m_adj(m, sizeof(cp));
 845
 846         cp.scid = letoh16(cp.scid);
 847         cp.dcid = letoh16(cp.dcid);
 848
 849         chan = l2cap_cid_lookup(cp.dcid);
 850         if (chan == NULL || chan->lc_link != link || chan->lc_rcid != cp.scid) {
 851                 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
 852                                         cp.dcid, cp.scid);
 853                 return;
 854         }
 855
 856         rp.dcid = htole16(chan->lc_lcid);
 857         rp.scid = htole16(chan->lc_rcid);
 858         l2cap_send_signal(link, L2CAP_DISCONNECT_RSP, cmd.ident,
 859                                 sizeof(rp), &rp);
 860
 861         if (chan->lc_state != L2CAP_CLOSED)
 862                 l2cap_close(chan, ECONNRESET);
 863 }
 864
 865 /*
 866  * Process Received Disconnect Response. We must validate scid and dcid but
 867  * unless we were waiting for this signal, ignore it.
 868  */
 869 static void
 870 l2cap_recv_disconnect_rsp(struct mbuf *m, struct hci_link *link)
 871 {
 872         l2cap_cmd_hdr_t cmd;
 873         l2cap_discon_rsp_cp cp;
 874         struct l2cap_req *req;
 875         struct l2cap_channel *chan;
 876
 877         m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
 878         m_adj(m, sizeof(cmd));
 879
 880         m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
 881         m_adj(m, sizeof(cp));
 882
 883         cp.scid = letoh16(cp.scid);
 884         cp.dcid = letoh16(cp.dcid);
 885
 886         req = l2cap_request_lookup(link, cmd.ident);
 887         if (req == NULL || req->lr_code != L2CAP_DISCONNECT_REQ)
 888                 return;
 889
 890         chan = req->lr_chan;
 891         if (chan == NULL
 892             || chan->lc_lcid != cp.scid
 893             || chan->lc_rcid != cp.dcid)
 894                 return;
 895
 896         l2cap_request_free(req);
 897
 898         if (chan->lc_state != L2CAP_WAIT_DISCONNECT)
 899                 return;
 900
 901         l2cap_close(chan, 0);
 902 }
 903
 904 /*
 905  * Process Received Info Request. We must respond but alas dont
 906  * support anything as yet so thats easy.
 907  */
 908 static void
 909 l2cap_recv_info_req(struct mbuf *m, struct hci_link *link)
 910 {
 911         l2cap_cmd_hdr_t cmd;
 912         l2cap_info_req_cp cp;
 913         l2cap_info_rsp_cp rp;
 914
 915         m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
 916         m_adj(m, sizeof(cmd));
 917
 918         m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
 919         m_adj(m, sizeof(cp));
 920
 921         switch(letoh16(cp.type)) {
 922         case L2CAP_CONNLESS_MTU:
 923         case L2CAP_EXTENDED_FEATURES:
 924         default:
 925                 rp.type = cp.type;
 926                 rp.result = htole16(L2CAP_NOT_SUPPORTED);
 927
 928                 l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident,
 929                                         sizeof(rp), &rp);
 930                 break;
 931         }
 932 }
 933
 934 /*
 935  * Construct signal and wrap in C-Frame for link.
 936  */
 937 static int
 938 l2cap_send_signal(struct hci_link *link, uint8_t code, uint8_t ident,
 939                         uint16_t length, void *data)
 940 {
 941         struct mbuf *m;
 942         l2cap_hdr_t *hdr;
 943         l2cap_cmd_hdr_t *cmd;
 944
 945 #ifdef DIAGNOSTIC
 946         if (link == NULL)
 947                 return ENETDOWN;
 948
 949         if (sizeof(l2cap_cmd_hdr_t) + length > link->hl_mtu)
 950                 kprintf("(%s) exceeding L2CAP Signal MTU for link!\n",
 951                     device_get_nameunit(link->hl_unit->hci_dev));
 952 #endif
 953
 954         m = m_gethdr(M_NOWAIT, MT_DATA);
 955         if (m == NULL)
 956                 return ENOMEM;
 957
 958         hdr = mtod(m, l2cap_hdr_t *);
 959         cmd = (l2cap_cmd_hdr_t *)(hdr + 1);
 960
 961         m->m_len = m->m_pkthdr.len = MHLEN;
 962
 963         /* Command Data */
 964         if (length > 0)
 965                 m_copyback(m, sizeof(*hdr) + sizeof(*cmd), length, data);
 966
 967         /* Command Header */
 968         cmd->code = code;
 969         cmd->ident = ident;
 970         cmd->length = htole16(length);
 971         length += sizeof(*cmd);
 972
 973         /* C-Frame Header */
 974         hdr->length = htole16(length);
 975         hdr->dcid = htole16(L2CAP_SIGNAL_CID);
 976         length += sizeof(*hdr);
 977
 978         if (m->m_pkthdr.len != MAX(MHLEN, length)) {
 979                 m_freem(m);
 980                 return ENOMEM;
 981         }
 982
 983         m->m_pkthdr.len = length;
 984         m->m_len = MIN(length, MHLEN);
 985
 986         DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
 987                 device_get_nameunit(link->hl_unit->hci_dev), code, ident,
 988                 length);
 989
 990         return hci_acl_send(m, link, NULL);
 991 }
 992
 993 /*
 994  * Send Command Reject packet.
 995  */
 996 static int
 997 l2cap_send_command_rej(struct hci_link *link, uint8_t ident,
 998                         uint16_t reason, ...)
 999 {
1000         l2cap_cmd_rej_cp cp;
1001         int len = 0;
1002         va_list ap;
1003
1004         va_start(ap, reason);
1005
1006         cp.reason = htole16(reason);
1007
1008         switch (reason) {
1009         case L2CAP_REJ_NOT_UNDERSTOOD:
1010                 len = 2;
1011                 break;
1012
1013         case L2CAP_REJ_MTU_EXCEEDED:
1014                 len = 4;
1015                 cp.data[0] = va_arg(ap, int);           /* SigMTU */
1016                 cp.data[0] = htole16(cp.data[0]);
1017                 break;
1018
1019         case L2CAP_REJ_INVALID_CID:
1020                 len = 6;
1021                 cp.data[0] = va_arg(ap, int);           /* dcid */
1022                 cp.data[0] = htole16(cp.data[0]);
1023                 cp.data[1] = va_arg(ap, int);           /* scid */
1024                 cp.data[1] = htole16(cp.data[1]);
1025                 break;
1026
1027         default:
1028                 UNKNOWN(reason);
1029                 return EINVAL;
1030         }
1031
1032         va_end(ap);
1033
1034         return l2cap_send_signal(link, L2CAP_COMMAND_REJ, ident, len, &cp);
1035 }
1036
1037 /*
1038  * Send Connect Request
1039  */
1040 int
1041 l2cap_send_connect_req(struct l2cap_channel *chan)
1042 {
1043         l2cap_con_req_cp cp;
1044         int err;
1045
1046         err = l2cap_request_alloc(chan, L2CAP_CONNECT_REQ);
1047         if (err)
1048                 return err;
1049
1050         cp.psm = htole16(chan->lc_raddr.bt_psm);
1051         cp.scid = htole16(chan->lc_lcid);
1052
1053         return l2cap_send_signal(chan->lc_link, L2CAP_CONNECT_REQ,
1054                                 chan->lc_link->hl_lastid, sizeof(cp), &cp);
1055 }
1056
1057 /*
1058  * Send Config Request
1059  *
1060  * For outgoing config request, we only put options in the packet if they
1061  * differ from the default and would have to be actioned. We dont support
1062  * enough option types to make overflowing SigMTU an issue so it can all
1063  * go in one packet.
1064  */
1065 int
1066 l2cap_send_config_req(struct l2cap_channel *chan)
1067 {
1068         l2cap_cfg_req_cp *cp;
1069         l2cap_cfg_opt_t *opt;
1070         l2cap_cfg_opt_val_t *val;
1071         uint8_t *next, buf[L2CAP_MTU_MINIMUM];
1072         int err;
1073
1074         err = l2cap_request_alloc(chan, L2CAP_CONFIG_REQ);
1075         if (err)
1076                 return err;
1077
1078         /* Config Header (4 octets) */
1079         cp = (l2cap_cfg_req_cp *)buf;
1080         cp->dcid = htole16(chan->lc_rcid);
1081         cp->flags = 0;  /* "No Continuation" */
1082
1083         next = buf + sizeof(l2cap_cfg_req_cp);
1084
1085         /* Incoming MTU (4 octets) */
1086         if (chan->lc_imtu != L2CAP_MTU_DEFAULT) {
1087                 opt = (l2cap_cfg_opt_t *)next;
1088                 opt->type = L2CAP_OPT_MTU;
1089                 opt->length = L2CAP_OPT_MTU_SIZE;
1090
1091                 val = (l2cap_cfg_opt_val_t *)(opt + 1);
1092                 val->mtu = htole16(chan->lc_imtu);
1093
1094                 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_MTU_SIZE;
1095         }
1096
1097         /* Flush Timeout (4 octets) */
1098         if (chan->lc_flush != L2CAP_FLUSH_TIMO_DEFAULT) {
1099                 opt = (l2cap_cfg_opt_t *)next;
1100                 opt->type = L2CAP_OPT_FLUSH_TIMO;
1101                 opt->length = L2CAP_OPT_FLUSH_TIMO_SIZE;
1102
1103                 val = (l2cap_cfg_opt_val_t *)(opt + 1);
1104                 val->flush_timo = htole16(chan->lc_flush);
1105
1106                 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_FLUSH_TIMO_SIZE;
1107         }
1108
1109         /* Outgoing QoS Flow (24 octets) */
1110         /* Retransmission & Flow Control (11 octets) */
1111         /*
1112          * From here we need to start paying attention to SigMTU as we have
1113          * possibly overflowed the minimum supported..
1114          */
1115
1116         return l2cap_send_signal(chan->lc_link, L2CAP_CONFIG_REQ,
1117                                     chan->lc_link->hl_lastid, (int)(next - buf), buf);
1118 }
1119
1120 /*
1121  * Send Disconnect Request
1122  */
1123 int
1124 l2cap_send_disconnect_req(struct l2cap_channel *chan)
1125 {
1126         l2cap_discon_req_cp cp;
1127         int err;
1128
1129         err = l2cap_request_alloc(chan, L2CAP_DISCONNECT_REQ);
1130         if (err)
1131                 return err;
1132
1133         cp.dcid = htole16(chan->lc_rcid);
1134         cp.scid = htole16(chan->lc_lcid);
1135
1136         return l2cap_send_signal(chan->lc_link, L2CAP_DISCONNECT_REQ,
1137                                     chan->lc_link->hl_lastid, sizeof(cp), &cp);
1138 }
1139
1140 /*
1141  * Send Connect Response
1142  */
1143 int
1144 l2cap_send_connect_rsp(struct hci_link *link, uint8_t ident, uint16_t dcid,
1145     uint16_t scid, uint16_t result)
1146 {
1147         l2cap_con_rsp_cp cp;
1148
1149         memset(&cp, 0, sizeof(cp));
1150         cp.dcid = htole16(dcid);
1151         cp.scid = htole16(scid);
1152         cp.result = htole16(result);
1153
1154         return l2cap_send_signal(link, L2CAP_CONNECT_RSP, ident, sizeof(cp), &cp);
1155 }