sys/netbt/l2cap_signal.c

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