Annotation of sys/netbt/l2cap_signal.c, Revision 1.1
1.1 ! nbrk 1: /* $OpenBSD: l2cap_signal.c,v 1.2 2007/07/22 21:05:00 gwk Exp $ */
! 2: /* $NetBSD: l2cap_signal.c,v 1.8 2007/05/16 18:34:49 plunky Exp $ */
! 3:
! 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/cdefs.h>
! 35:
! 36: #include <sys/param.h>
! 37: #include <sys/kernel.h>
! 38: #include <sys/mbuf.h>
! 39: #include <sys/proc.h>
! 40: #include <sys/queue.h>
! 41: #include <sys/systm.h>
! 42:
! 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: link->hl_unit->hci_devname,
! 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: timeout_add(&req->lr_rtx, 0);
! 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: timeout_add(&req->lr_rtx, 0);
! 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: KASSERT(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 Reqest.
! 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: default:
! 526: /* ignore hints */
! 527: if (opt.type & L2CAP_OPT_HINT_BIT)
! 528: break;
! 529:
! 530: /* unknown options supercede all else */
! 531: if (rp.result != L2CAP_UNKNOWN_OPTION) {
! 532: rp.result = L2CAP_UNKNOWN_OPTION;
! 533: len = sizeof(rp);
! 534: }
! 535:
! 536: /* ignore if it don't fit */
! 537: if (len + sizeof(opt) > sizeof(buf))
! 538: break;
! 539:
! 540: /* return unknown option type, but no data */
! 541: buf[len++] = opt.type;
! 542: buf[len++] = 0;
! 543: break;
! 544: }
! 545:
! 546: m_adj(m, opt.length);
! 547: left -= opt.length;
! 548: }
! 549:
! 550: rp.result = htole16(rp.result);
! 551: memcpy(buf, &rp, sizeof(rp));
! 552: l2cap_send_signal(link, L2CAP_CONFIG_RSP, cmd.ident, len, buf);
! 553:
! 554: if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0
! 555: && rp.result == letoh16(L2CAP_SUCCESS)) {
! 556:
! 557: chan->lc_flags &= ~L2CAP_WAIT_CONFIG_REQ;
! 558:
! 559: if ((chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0) {
! 560: chan->lc_state = L2CAP_OPEN;
! 561: /* XXX how to distinguish REconfiguration? */
! 562: (*chan->lc_proto->connected)(chan->lc_upper);
! 563: }
! 564: }
! 565: return;
! 566:
! 567: reject:
! 568: l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
! 569: out:
! 570: m_adj(m, left);
! 571: }
! 572:
! 573: /*
! 574: * Process Received Config Response.
! 575: */
! 576: static void
! 577: l2cap_recv_config_rsp(struct mbuf *m, struct hci_link *link)
! 578: {
! 579: l2cap_cmd_hdr_t cmd;
! 580: l2cap_cfg_rsp_cp cp;
! 581: l2cap_cfg_opt_t opt;
! 582: l2cap_cfg_opt_val_t val;
! 583: struct l2cap_req *req;
! 584: struct l2cap_channel *chan;
! 585: int left;
! 586:
! 587: m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
! 588: m_adj(m, sizeof(cmd));
! 589: left = letoh16(cmd.length);
! 590:
! 591: if (left < sizeof(cp))
! 592: goto out;
! 593:
! 594: m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
! 595: m_adj(m, sizeof(cp));
! 596: left -= sizeof(cp);
! 597:
! 598: cp.scid = letoh16(cp.scid);
! 599: cp.flags = letoh16(cp.flags);
! 600: cp.result = letoh16(cp.result);
! 601:
! 602: req = l2cap_request_lookup(link, cmd.ident);
! 603: if (req == NULL || req->lr_code != L2CAP_CONFIG_REQ)
! 604: goto out;
! 605:
! 606: chan = req->lr_chan;
! 607: if (chan != NULL && chan->lc_lcid != cp.scid)
! 608: goto out;
! 609:
! 610: l2cap_request_free(req);
! 611:
! 612: if (chan == NULL || chan->lc_state != L2CAP_WAIT_CONFIG
! 613: || (chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0)
! 614: goto out;
! 615:
! 616: if ((cp.flags & L2CAP_OPT_CFLAG_BIT)) {
! 617: l2cap_cfg_req_cp rp;
! 618:
! 619: /*
! 620: * They have more to tell us and want another ID to
! 621: * use, so send an empty config request
! 622: */
! 623: if (l2cap_request_alloc(chan, L2CAP_CONFIG_REQ))
! 624: goto discon;
! 625:
! 626: rp.dcid = htole16(cp.scid);
! 627: rp.flags = 0;
! 628:
! 629: if (l2cap_send_signal(link, L2CAP_CONFIG_REQ, link->hl_lastid,
! 630: sizeof(rp), &rp))
! 631: goto discon;
! 632: }
! 633:
! 634: switch(cp.result) {
! 635: case L2CAP_SUCCESS:
! 636: /*
! 637: * If continuation flag was not set, our config request was
! 638: * accepted. We may have to wait for their config request to
! 639: * complete, so check that but otherwise we are open
! 640: *
! 641: * There may be 'advisory' values in the packet but we just
! 642: * ignore those..
! 643: */
! 644: if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) {
! 645: chan->lc_flags &= ~L2CAP_WAIT_CONFIG_RSP;
! 646:
! 647: if ((chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) {
! 648: chan->lc_state = L2CAP_OPEN;
! 649: /* XXX how to distinguish REconfiguration? */
! 650: (*chan->lc_proto->connected)(chan->lc_upper);
! 651: }
! 652: }
! 653: goto out;
! 654:
! 655: case L2CAP_UNACCEPTABLE_PARAMS:
! 656: /*
! 657: * Packet contains unacceptable parameters with preferred values
! 658: */
! 659: while (left > 0) {
! 660: if (left < sizeof(opt))
! 661: goto discon;
! 662:
! 663: m_copydata(m, 0, sizeof(opt), (caddr_t)&opt);
! 664: m_adj(m, sizeof(opt));
! 665: left -= sizeof(opt);
! 666:
! 667: if (left < opt.length)
! 668: goto discon;
! 669:
! 670: switch (opt.type) {
! 671: case L2CAP_OPT_MTU:
! 672: if (opt.length != L2CAP_OPT_MTU_SIZE)
! 673: goto discon;
! 674:
! 675: m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, (caddr_t)&val);
! 676: chan->lc_imtu = letoh16(val.mtu);
! 677: if (chan->lc_imtu < L2CAP_MTU_MINIMUM)
! 678: chan->lc_imtu = L2CAP_MTU_DEFAULT;
! 679: break;
! 680:
! 681: case L2CAP_OPT_FLUSH_TIMO:
! 682: if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE)
! 683: goto discon;
! 684:
! 685: /*
! 686: * Spec says: If we cannot honor proposed value,
! 687: * either disconnect or try again with original
! 688: * value. I can't really see why they want to
! 689: * interfere with OUR flush timeout in any case
! 690: * so we just punt for now.
! 691: */
! 692: goto discon;
! 693:
! 694: case L2CAP_OPT_QOS:
! 695: break;
! 696:
! 697: default:
! 698: UNKNOWN(opt.type);
! 699: goto discon;
! 700: }
! 701:
! 702: m_adj(m, opt.length);
! 703: left -= opt.length;
! 704: }
! 705:
! 706: if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
! 707: l2cap_send_config_req(chan); /* no state change */
! 708:
! 709: goto out;
! 710:
! 711: case L2CAP_REJECT:
! 712: goto discon;
! 713:
! 714: case L2CAP_UNKNOWN_OPTION:
! 715: /*
! 716: * Packet contains options not understood. Turn off unknown
! 717: * options by setting them to default values (means they will
! 718: * not be requested again).
! 719: *
! 720: * If our option was already off then fail (paranoia?)
! 721: *
! 722: * XXX Should we consider that options were set for a reason?
! 723: */
! 724: while (left > 0) {
! 725: if (left < sizeof(opt))
! 726: goto discon;
! 727:
! 728: m_copydata(m, 0, sizeof(opt), (caddr_t)&opt);
! 729: m_adj(m, sizeof(opt));
! 730: left -= sizeof(opt);
! 731:
! 732: if (left < opt.length)
! 733: goto discon;
! 734:
! 735: m_adj(m, opt.length);
! 736: left -= opt.length;
! 737:
! 738: switch(opt.type) {
! 739: case L2CAP_OPT_MTU:
! 740: if (chan->lc_imtu == L2CAP_MTU_DEFAULT)
! 741: goto discon;
! 742:
! 743: chan->lc_imtu = L2CAP_MTU_DEFAULT;
! 744: break;
! 745:
! 746: case L2CAP_OPT_FLUSH_TIMO:
! 747: if (chan->lc_flush == L2CAP_FLUSH_TIMO_DEFAULT)
! 748: goto discon;
! 749:
! 750: chan->lc_flush = L2CAP_FLUSH_TIMO_DEFAULT;
! 751: break;
! 752:
! 753: case L2CAP_OPT_QOS:
! 754: break;
! 755:
! 756: default:
! 757: UNKNOWN(opt.type);
! 758: goto discon;
! 759: }
! 760: }
! 761:
! 762: if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
! 763: l2cap_send_config_req(chan); /* no state change */
! 764:
! 765: goto out;
! 766:
! 767: default:
! 768: UNKNOWN(cp.result);
! 769: goto discon;
! 770: }
! 771:
! 772: DPRINTF("how did I get here!?\n");
! 773:
! 774: discon:
! 775: l2cap_send_disconnect_req(chan);
! 776: l2cap_close(chan, ECONNABORTED);
! 777:
! 778: out:
! 779: m_adj(m, left);
! 780: }
! 781:
! 782: /*
! 783: * Process Received Disconnect Request. We must validate scid and dcid
! 784: * just in case but otherwise this connection is finished.
! 785: */
! 786: static void
! 787: l2cap_recv_disconnect_req(struct mbuf *m, struct hci_link *link)
! 788: {
! 789: l2cap_cmd_hdr_t cmd;
! 790: l2cap_discon_req_cp cp;
! 791: l2cap_discon_rsp_cp rp;
! 792: struct l2cap_channel *chan;
! 793:
! 794: m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
! 795: m_adj(m, sizeof(cmd));
! 796:
! 797: m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
! 798: m_adj(m, sizeof(cp));
! 799:
! 800: cp.scid = letoh16(cp.scid);
! 801: cp.dcid = letoh16(cp.dcid);
! 802:
! 803: chan = l2cap_cid_lookup(cp.dcid);
! 804: if (chan == NULL || chan->lc_link != link || chan->lc_rcid != cp.scid) {
! 805: l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
! 806: cp.dcid, cp.scid);
! 807: return;
! 808: }
! 809:
! 810: rp.dcid = htole16(chan->lc_lcid);
! 811: rp.scid = htole16(chan->lc_rcid);
! 812: l2cap_send_signal(link, L2CAP_DISCONNECT_RSP, cmd.ident,
! 813: sizeof(rp), &rp);
! 814:
! 815: if (chan->lc_state != L2CAP_CLOSED)
! 816: l2cap_close(chan, ECONNRESET);
! 817: }
! 818:
! 819: /*
! 820: * Process Received Disconnect Response. We must validate scid and dcid but
! 821: * unless we were waiting for this signal, ignore it.
! 822: */
! 823: static void
! 824: l2cap_recv_disconnect_rsp(struct mbuf *m, struct hci_link *link)
! 825: {
! 826: l2cap_cmd_hdr_t cmd;
! 827: l2cap_discon_rsp_cp cp;
! 828: struct l2cap_req *req;
! 829: struct l2cap_channel *chan;
! 830:
! 831: m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
! 832: m_adj(m, sizeof(cmd));
! 833:
! 834: m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
! 835: m_adj(m, sizeof(cp));
! 836:
! 837: cp.scid = letoh16(cp.scid);
! 838: cp.dcid = letoh16(cp.dcid);
! 839:
! 840: req = l2cap_request_lookup(link, cmd.ident);
! 841: if (req == NULL || req->lr_code != L2CAP_DISCONNECT_REQ)
! 842: return;
! 843:
! 844: chan = req->lr_chan;
! 845: if (chan == NULL
! 846: || chan->lc_lcid != cp.scid
! 847: || chan->lc_rcid != cp.dcid)
! 848: return;
! 849:
! 850: l2cap_request_free(req);
! 851:
! 852: if (chan->lc_state != L2CAP_WAIT_DISCONNECT)
! 853: return;
! 854:
! 855: l2cap_close(chan, 0);
! 856: }
! 857:
! 858: /*
! 859: * Process Received Info Request. We must respond but alas dont
! 860: * support anything as yet so thats easy.
! 861: */
! 862: static void
! 863: l2cap_recv_info_req(struct mbuf *m, struct hci_link *link)
! 864: {
! 865: l2cap_cmd_hdr_t cmd;
! 866: l2cap_info_req_cp cp;
! 867: l2cap_info_rsp_cp rp;
! 868:
! 869: m_copydata(m, 0, sizeof(cmd), (caddr_t)&cmd);
! 870: m_adj(m, sizeof(cmd));
! 871:
! 872: m_copydata(m, 0, sizeof(cp), (caddr_t)&cp);
! 873: m_adj(m, sizeof(cp));
! 874:
! 875: switch(letoh16(cp.type)) {
! 876: case L2CAP_CONNLESS_MTU:
! 877: case L2CAP_EXTENDED_FEATURES:
! 878: default:
! 879: rp.type = cp.type;
! 880: rp.result = htole16(L2CAP_NOT_SUPPORTED);
! 881:
! 882: l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident,
! 883: sizeof(rp), &rp);
! 884: break;
! 885: }
! 886: }
! 887:
! 888: /*
! 889: * Construct signal and wrap in C-Frame for link.
! 890: */
! 891: static int
! 892: l2cap_send_signal(struct hci_link *link, uint8_t code, uint8_t ident,
! 893: uint16_t length, void *data)
! 894: {
! 895: struct mbuf *m;
! 896: l2cap_hdr_t *hdr;
! 897: l2cap_cmd_hdr_t *cmd;
! 898:
! 899: #ifdef DIAGNOSTIC
! 900: if (link == NULL)
! 901: return ENETDOWN;
! 902:
! 903: if (sizeof(l2cap_cmd_hdr_t) + length > link->hl_mtu)
! 904: printf("(%s) exceeding L2CAP Signal MTU for link!\n",
! 905: link->hl_unit->hci_devname);
! 906: #endif
! 907:
! 908: m = m_gethdr(M_DONTWAIT, MT_DATA);
! 909: if (m == NULL)
! 910: return ENOMEM;
! 911:
! 912: hdr = mtod(m, l2cap_hdr_t *);
! 913: cmd = (l2cap_cmd_hdr_t *)(hdr + 1);
! 914:
! 915: m->m_len = m->m_pkthdr.len = MHLEN;
! 916:
! 917: /* Command Data */
! 918: if (length > 0)
! 919: m_copyback(m, sizeof(*hdr) + sizeof(*cmd), length, data);
! 920:
! 921: /* Command Header */
! 922: cmd->code = code;
! 923: cmd->ident = ident;
! 924: cmd->length = htole16(length);
! 925: length += sizeof(*cmd);
! 926:
! 927: /* C-Frame Header */
! 928: hdr->length = htole16(length);
! 929: hdr->dcid = htole16(L2CAP_SIGNAL_CID);
! 930: length += sizeof(*hdr);
! 931:
! 932: if (m->m_pkthdr.len != MAX(MHLEN, length)) {
! 933: m_freem(m);
! 934: return ENOMEM;
! 935: }
! 936:
! 937: m->m_pkthdr.len = length;
! 938: m->m_len = MIN(length, MHLEN);
! 939:
! 940: DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
! 941: link->hl_unit->hci_devname, code, ident, length);
! 942:
! 943: return hci_acl_send(m, link, NULL);
! 944: }
! 945:
! 946: /*
! 947: * Send Command Reject packet.
! 948: */
! 949: static int
! 950: l2cap_send_command_rej(struct hci_link *link, uint8_t ident,
! 951: uint16_t reason, ...)
! 952: {
! 953: l2cap_cmd_rej_cp cp;
! 954: int len = 0;
! 955: va_list ap;
! 956:
! 957: va_start(ap, reason);
! 958:
! 959: cp.reason = htole16(reason);
! 960:
! 961: switch (reason) {
! 962: case L2CAP_REJ_NOT_UNDERSTOOD:
! 963: len = 2;
! 964: break;
! 965:
! 966: case L2CAP_REJ_MTU_EXCEEDED:
! 967: len = 4;
! 968: cp.data[0] = va_arg(ap, int); /* SigMTU */
! 969: cp.data[0] = htole16(cp.data[0]);
! 970: break;
! 971:
! 972: case L2CAP_REJ_INVALID_CID:
! 973: len = 6;
! 974: cp.data[0] = va_arg(ap, int); /* dcid */
! 975: cp.data[0] = htole16(cp.data[0]);
! 976: cp.data[1] = va_arg(ap, int); /* scid */
! 977: cp.data[1] = htole16(cp.data[1]);
! 978: break;
! 979:
! 980: default:
! 981: UNKNOWN(reason);
! 982: return EINVAL;
! 983: }
! 984:
! 985: va_end(ap);
! 986:
! 987: return l2cap_send_signal(link, L2CAP_COMMAND_REJ, ident, len, &cp);
! 988: }
! 989:
! 990: /*
! 991: * Send Connect Request
! 992: */
! 993: int
! 994: l2cap_send_connect_req(struct l2cap_channel *chan)
! 995: {
! 996: l2cap_con_req_cp cp;
! 997: int err;
! 998:
! 999: err = l2cap_request_alloc(chan, L2CAP_CONNECT_REQ);
! 1000: if (err)
! 1001: return err;
! 1002:
! 1003: cp.psm = htole16(chan->lc_raddr.bt_psm);
! 1004: cp.scid = htole16(chan->lc_lcid);
! 1005:
! 1006: return l2cap_send_signal(chan->lc_link, L2CAP_CONNECT_REQ,
! 1007: chan->lc_link->hl_lastid, sizeof(cp), &cp);
! 1008: }
! 1009:
! 1010: /*
! 1011: * Send Config Request
! 1012: *
! 1013: * For outgoing config request, we only put options in the packet if they
! 1014: * differ from the default and would have to be actioned. We dont support
! 1015: * enough option types to make overflowing SigMTU an issue so it can all
! 1016: * go in one packet.
! 1017: */
! 1018: int
! 1019: l2cap_send_config_req(struct l2cap_channel *chan)
! 1020: {
! 1021: l2cap_cfg_req_cp *cp;
! 1022: l2cap_cfg_opt_t *opt;
! 1023: l2cap_cfg_opt_val_t *val;
! 1024: uint8_t *next, buf[L2CAP_MTU_MINIMUM];
! 1025: int err;
! 1026:
! 1027: err = l2cap_request_alloc(chan, L2CAP_CONFIG_REQ);
! 1028: if (err)
! 1029: return err;
! 1030:
! 1031: /* Config Header (4 octets) */
! 1032: cp = (l2cap_cfg_req_cp *)buf;
! 1033: cp->dcid = htole16(chan->lc_rcid);
! 1034: cp->flags = 0; /* "No Continuation" */
! 1035:
! 1036: next = buf + sizeof(l2cap_cfg_req_cp);
! 1037:
! 1038: /* Incoming MTU (4 octets) */
! 1039: if (chan->lc_imtu != L2CAP_MTU_DEFAULT) {
! 1040: opt = (l2cap_cfg_opt_t *)next;
! 1041: opt->type = L2CAP_OPT_MTU;
! 1042: opt->length = L2CAP_OPT_MTU_SIZE;
! 1043:
! 1044: val = (l2cap_cfg_opt_val_t *)(opt + 1);
! 1045: val->mtu = htole16(chan->lc_imtu);
! 1046:
! 1047: next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_MTU_SIZE;
! 1048: }
! 1049:
! 1050: /* Flush Timeout (4 octets) */
! 1051: if (chan->lc_flush != L2CAP_FLUSH_TIMO_DEFAULT) {
! 1052: opt = (l2cap_cfg_opt_t *)next;
! 1053: opt->type = L2CAP_OPT_FLUSH_TIMO;
! 1054: opt->length = L2CAP_OPT_FLUSH_TIMO_SIZE;
! 1055:
! 1056: val = (l2cap_cfg_opt_val_t *)(opt + 1);
! 1057: val->flush_timo = htole16(chan->lc_flush);
! 1058:
! 1059: next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_FLUSH_TIMO_SIZE;
! 1060: }
! 1061:
! 1062: /* Outgoing QoS Flow (24 octets) */
! 1063: /* Retransmission & Flow Control (11 octets) */
! 1064: /*
! 1065: * From here we need to start paying attention to SigMTU as we have
! 1066: * possibly overflowed the minimum supported..
! 1067: */
! 1068:
! 1069: return l2cap_send_signal(chan->lc_link, L2CAP_CONFIG_REQ,
! 1070: chan->lc_link->hl_lastid, (int)(next - buf), buf);
! 1071: }
! 1072:
! 1073: /*
! 1074: * Send Disconnect Request
! 1075: */
! 1076: int
! 1077: l2cap_send_disconnect_req(struct l2cap_channel *chan)
! 1078: {
! 1079: l2cap_discon_req_cp cp;
! 1080: int err;
! 1081:
! 1082: err = l2cap_request_alloc(chan, L2CAP_DISCONNECT_REQ);
! 1083: if (err)
! 1084: return err;
! 1085:
! 1086: cp.dcid = htole16(chan->lc_rcid);
! 1087: cp.scid = htole16(chan->lc_lcid);
! 1088:
! 1089: return l2cap_send_signal(chan->lc_link, L2CAP_DISCONNECT_REQ,
! 1090: chan->lc_link->hl_lastid, sizeof(cp), &cp);
! 1091: }
! 1092:
! 1093: /*
! 1094: * Send Connect Response
! 1095: */
! 1096: int
! 1097: l2cap_send_connect_rsp(struct hci_link *link, uint8_t ident, uint16_t dcid,
! 1098: uint16_t scid, uint16_t result)
! 1099: {
! 1100: l2cap_con_rsp_cp cp;
! 1101:
! 1102: memset(&cp, 0, sizeof(cp));
! 1103: cp.dcid = htole16(dcid);
! 1104: cp.scid = htole16(scid);
! 1105: cp.result = htole16(result);
! 1106:
! 1107: return l2cap_send_signal(link, L2CAP_CONNECT_RSP, ident, sizeof(cp), &cp);
! 1108: }
CVSweb