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