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