a5c89047c0
NetBSD Foundation Membership still pending.) This stack was written by Iain under sponsorship from Itronix Inc. The stack includes support for rfcomm networking (networking via your bluetooth enabled cell phone), hid devices (keyboards/mice), and headsets. Drivers for both PCMCIA and USB bluetooth controllers are included.
1052 lines
25 KiB
C
1052 lines
25 KiB
C
/* $NetBSD: l2cap_signal.c,v 1.1 2006/06/19 15:44:45 gdamore Exp $ */
|
|
|
|
/*-
|
|
* Copyright (c) 2005 Iain Hibbert.
|
|
* Copyright (c) 2006 Itronix Inc.
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
* documentation and/or other materials provided with the distribution.
|
|
* 3. The name of Itronix Inc. may not be used to endorse
|
|
* or promote products derived from this software without specific
|
|
* prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``AS IS'' AND
|
|
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
|
|
* TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL ITRONIX INC. BE LIABLE FOR ANY
|
|
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
|
* ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include <sys/cdefs.h>
|
|
__KERNEL_RCSID(0, "$NetBSD: l2cap_signal.c,v 1.1 2006/06/19 15:44:45 gdamore Exp $");
|
|
|
|
#include <sys/param.h>
|
|
#include <sys/kernel.h>
|
|
#include <sys/mbuf.h>
|
|
#include <sys/proc.h>
|
|
#include <sys/queue.h>
|
|
#include <sys/systm.h>
|
|
|
|
#include <machine/stdarg.h>
|
|
|
|
#include <netbt/bluetooth.h>
|
|
#include <netbt/hci.h>
|
|
#include <netbt/l2cap.h>
|
|
|
|
/*******************************************************************************
|
|
*
|
|
* L2CAP Signal processing
|
|
*/
|
|
|
|
static void l2cap_recv_command_rej(struct mbuf *, struct hci_link *);
|
|
static void l2cap_recv_connect_req(struct mbuf *, struct hci_link *);
|
|
static void l2cap_recv_connect_rsp(struct mbuf *, struct hci_link *);
|
|
static void l2cap_recv_config_req(struct mbuf *, struct hci_link *);
|
|
static void l2cap_recv_config_rsp(struct mbuf *, struct hci_link *);
|
|
static void l2cap_recv_disconnect_req(struct mbuf *, struct hci_link *);
|
|
static void l2cap_recv_disconnect_rsp(struct mbuf *, struct hci_link *);
|
|
static void l2cap_recv_info_req(struct mbuf *, struct hci_link *);
|
|
static int l2cap_send_signal(struct hci_link *, uint8_t, uint8_t, uint16_t, void *);
|
|
static int l2cap_send_command_rej(struct hci_link *, uint8_t, uint16_t, ...);
|
|
|
|
/*
|
|
* process incoming signal packets (CID 0x0001). Can contain multiple
|
|
* requests/responses.
|
|
*/
|
|
void
|
|
l2cap_recv_signal(struct mbuf *m, struct hci_link *link)
|
|
{
|
|
l2cap_cmd_hdr_t cmd;
|
|
|
|
for(;;) {
|
|
if (m->m_pkthdr.len == 0)
|
|
goto finish;
|
|
|
|
if (m->m_pkthdr.len < sizeof(cmd))
|
|
goto reject;
|
|
|
|
m_copydata(m, 0, sizeof(cmd), &cmd);
|
|
cmd.length = le16toh(cmd.length);
|
|
|
|
if (m->m_pkthdr.len < sizeof(cmd) + cmd.length)
|
|
goto reject;
|
|
|
|
DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
|
|
link->hl_unit->hci_devname,
|
|
cmd.code, cmd.ident, cmd.length);
|
|
|
|
switch (cmd.code) {
|
|
case L2CAP_COMMAND_REJ:
|
|
if (cmd.length > sizeof(l2cap_cmd_rej_cp))
|
|
goto finish;
|
|
|
|
l2cap_recv_command_rej(m, link);
|
|
break;
|
|
|
|
case L2CAP_CONNECT_REQ:
|
|
if (cmd.length != sizeof(l2cap_con_req_cp))
|
|
goto reject;
|
|
|
|
l2cap_recv_connect_req(m, link);
|
|
break;
|
|
|
|
case L2CAP_CONNECT_RSP:
|
|
if (cmd.length != sizeof(l2cap_con_rsp_cp))
|
|
goto finish;
|
|
|
|
l2cap_recv_connect_rsp(m, link);
|
|
break;
|
|
|
|
case L2CAP_CONFIG_REQ:
|
|
l2cap_recv_config_req(m, link);
|
|
break;
|
|
|
|
case L2CAP_CONFIG_RSP:
|
|
l2cap_recv_config_rsp(m, link);
|
|
break;
|
|
|
|
case L2CAP_DISCONNECT_REQ:
|
|
if (cmd.length != sizeof(l2cap_discon_req_cp))
|
|
goto reject;
|
|
|
|
l2cap_recv_disconnect_req(m, link);
|
|
break;
|
|
|
|
case L2CAP_DISCONNECT_RSP:
|
|
if (cmd.length != sizeof(l2cap_discon_rsp_cp))
|
|
goto finish;
|
|
|
|
l2cap_recv_disconnect_rsp(m, link);
|
|
break;
|
|
|
|
case L2CAP_ECHO_REQ:
|
|
m_adj(m, sizeof(cmd) + cmd.length);
|
|
l2cap_send_signal(link, L2CAP_ECHO_RSP, cmd.ident,
|
|
0, NULL);
|
|
break;
|
|
|
|
case L2CAP_ECHO_RSP:
|
|
m_adj(m, sizeof(cmd) + cmd.length);
|
|
break;
|
|
|
|
case L2CAP_INFO_REQ:
|
|
if (cmd.length != sizeof(l2cap_info_req_cp))
|
|
goto reject;
|
|
|
|
l2cap_recv_info_req(m, link);
|
|
break;
|
|
|
|
case L2CAP_INFO_RSP:
|
|
m_adj(m, sizeof(cmd) + cmd.length);
|
|
break;
|
|
|
|
default:
|
|
goto reject;
|
|
}
|
|
}
|
|
|
|
#ifdef DIAGNOSTIC
|
|
panic("impossible!");
|
|
#endif
|
|
|
|
reject:
|
|
l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
|
|
finish:
|
|
m_freem(m);
|
|
}
|
|
|
|
/*
|
|
* Process Received Command Reject. For now we dont try to recover gracefully
|
|
* from this, it probably means that the link is garbled or the other end is
|
|
* insufficiently capable of handling normal traffic. (not *my* fault, no way!)
|
|
*/
|
|
static void
|
|
l2cap_recv_command_rej(struct mbuf *m, struct hci_link *link)
|
|
{
|
|
struct l2cap_req *req;
|
|
struct l2cap_channel *chan;
|
|
l2cap_cmd_hdr_t cmd;
|
|
l2cap_cmd_rej_cp cp;
|
|
|
|
m_copydata(m, 0, sizeof(cmd), &cmd);
|
|
m_adj(m, sizeof(cmd));
|
|
|
|
cmd.length = le16toh(cmd.length);
|
|
|
|
m_copydata(m, 0, cmd.length, &cp);
|
|
m_adj(m, cmd.length);
|
|
|
|
req = l2cap_request_lookup(link, cmd.ident);
|
|
if (req == NULL)
|
|
return;
|
|
|
|
switch (le16toh(cp.reason)) {
|
|
case L2CAP_REJ_NOT_UNDERSTOOD:
|
|
/*
|
|
* I dont know what to do, just move up the timeout
|
|
*/
|
|
callout_schedule(&req->lr_rtx, 0);
|
|
break;
|
|
|
|
case L2CAP_REJ_MTU_EXCEEDED:
|
|
/*
|
|
* I didnt send any commands over L2CAP_MTU_MINIMUM size, but..
|
|
*/
|
|
link->hl_mtu = le16toh(cp.data[0]);
|
|
callout_schedule(&req->lr_rtx, 0); // XX maybe resend instead?
|
|
break;
|
|
|
|
case L2CAP_REJ_INVALID_CID:
|
|
/*
|
|
* Well, if they dont have such a channel then our channel is
|
|
* most likely closed. Make it so.
|
|
*/
|
|
chan = req->lr_chan;
|
|
l2cap_request_free(req);
|
|
if (chan != NULL && chan->lc_state != L2CAP_CLOSED)
|
|
l2cap_close(chan, ECONNABORTED);
|
|
|
|
break;
|
|
|
|
default:
|
|
UNKNOWN(le16toh(cp.reason));
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Process Received Connect Request. Find listening channel matching
|
|
* psm & addr and ask upper layer for a new channel.
|
|
*/
|
|
static void
|
|
l2cap_recv_connect_req(struct mbuf *m, struct hci_link *link)
|
|
{
|
|
struct sockaddr_bt laddr, raddr;
|
|
struct l2cap_channel *chan, *new;
|
|
l2cap_cmd_hdr_t cmd;
|
|
l2cap_con_req_cp cp;
|
|
l2cap_con_rsp_cp rp;
|
|
int err;
|
|
|
|
/* extract cmd */
|
|
m_copydata(m, 0, sizeof(cmd), &cmd);
|
|
m_adj(m, sizeof(cmd));
|
|
|
|
/* extract request */
|
|
m_copydata(m, 0, sizeof(cp), &cp);
|
|
m_adj(m, sizeof(cp));
|
|
|
|
/* init response */
|
|
memset(&rp, 0, sizeof(rp));
|
|
rp.scid = cp.scid;
|
|
|
|
memset(&laddr, 0, sizeof(struct sockaddr_bt));
|
|
laddr.bt_len = sizeof(struct sockaddr_bt);
|
|
laddr.bt_family = AF_BLUETOOTH;
|
|
laddr.bt_psm = le16toh(cp.psm);
|
|
bdaddr_copy(&laddr.bt_bdaddr, &link->hl_unit->hci_bdaddr);
|
|
|
|
memset(&raddr, 0, sizeof(struct sockaddr_bt));
|
|
raddr.bt_len = sizeof(struct sockaddr_bt);
|
|
raddr.bt_family = AF_BLUETOOTH;
|
|
raddr.bt_psm = le16toh(cp.psm);
|
|
bdaddr_copy(&raddr.bt_bdaddr, &link->hl_bdaddr);
|
|
|
|
LIST_FOREACH(chan, &l2cap_listen_list, lc_ncid) {
|
|
if (chan->lc_laddr.bt_psm != laddr.bt_psm
|
|
&& chan->lc_laddr.bt_psm != L2CAP_PSM_ANY)
|
|
continue;
|
|
|
|
if (!bdaddr_same(&laddr.bt_bdaddr, &chan->lc_laddr.bt_bdaddr)
|
|
&& bdaddr_any(&chan->lc_laddr.bt_bdaddr) == 0)
|
|
continue;
|
|
|
|
new= (*chan->lc_proto->newconn)(chan->lc_upper, &laddr, &raddr);
|
|
if (new == NULL)
|
|
continue;
|
|
|
|
err = l2cap_cid_alloc(new);
|
|
if (err) {
|
|
rp.result = htole16(L2CAP_NO_RESOURCES);
|
|
l2cap_send_signal(link, L2CAP_CONNECT_RSP,
|
|
cmd.ident, sizeof(rp), &rp);
|
|
(*new->lc_proto->disconnected)(new->lc_upper, err);
|
|
return;
|
|
}
|
|
|
|
new->lc_link = hci_acl_open(link->hl_unit, &link->hl_bdaddr);
|
|
KASSERT(new->lc_link == link);
|
|
|
|
new->lc_rcid = le16toh(cp.scid);
|
|
|
|
memcpy(&new->lc_laddr, &laddr, sizeof(struct sockaddr_bt));
|
|
memcpy(&new->lc_raddr, &raddr, sizeof(struct sockaddr_bt));
|
|
|
|
rp.dcid = htole16(new->lc_lcid);
|
|
rp.result = htole16(L2CAP_SUCCESS);
|
|
l2cap_send_signal(link, L2CAP_CONNECT_RSP, cmd.ident,
|
|
sizeof(rp), &rp);
|
|
|
|
new->lc_state = L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP;
|
|
l2cap_send_config_req(new);
|
|
return;
|
|
}
|
|
|
|
rp.result = htole16(L2CAP_PSM_NOT_SUPPORTED);
|
|
l2cap_send_signal(link, L2CAP_CONNECT_RSP, cmd.ident, sizeof(rp), &rp);
|
|
}
|
|
|
|
/*
|
|
* Process Received Connect Response.
|
|
*/
|
|
static void
|
|
l2cap_recv_connect_rsp(struct mbuf *m, struct hci_link *link)
|
|
{
|
|
l2cap_cmd_hdr_t cmd;
|
|
l2cap_con_rsp_cp cp;
|
|
struct l2cap_req *req;
|
|
struct l2cap_channel *chan;
|
|
|
|
m_copydata(m, 0, sizeof(cmd), &cmd);
|
|
m_adj(m, sizeof(cmd));
|
|
|
|
m_copydata(m, 0, sizeof(cp), &cp);
|
|
m_adj(m, sizeof(cp));
|
|
|
|
cp.scid = le16toh(cp.scid);
|
|
cp.dcid = le16toh(cp.dcid);
|
|
cp.result = le16toh(cp.result);
|
|
|
|
req = l2cap_request_lookup(link, cmd.ident);
|
|
if (req == NULL || req->lr_code != L2CAP_CONNECT_REQ)
|
|
return;
|
|
|
|
chan = req->lr_chan;
|
|
if (chan != NULL && chan->lc_lcid != cp.scid)
|
|
return;
|
|
|
|
if (chan == NULL || chan->lc_state != L2CAP_WAIT_CONNECT_RSP) {
|
|
l2cap_request_free(req);
|
|
return;
|
|
}
|
|
|
|
switch (cp.result) {
|
|
case L2CAP_SUCCESS:
|
|
/*
|
|
* Ok, at this point we have a connection to the other party. We
|
|
* could indicate upstream that we are ready for business and
|
|
* wait for a "Configure Channel Request" but I'm not so sure
|
|
* that is required in our case - we will proceed directly to
|
|
* sending our config request. We set two state bits because in
|
|
* the config state we are waiting for requests and responses.
|
|
*/
|
|
l2cap_request_free(req);
|
|
chan->lc_rcid = cp.dcid;
|
|
chan->lc_state = L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP;
|
|
l2cap_send_config_req(chan);
|
|
break;
|
|
|
|
case L2CAP_PENDING:
|
|
// dont release request, should start eRTX timeout?
|
|
(*chan->lc_proto->connecting)(chan->lc_upper);
|
|
break;
|
|
|
|
case L2CAP_PSM_NOT_SUPPORTED:
|
|
case L2CAP_SECURITY_BLOCK:
|
|
case L2CAP_NO_RESOURCES:
|
|
default:
|
|
l2cap_request_free(req);
|
|
l2cap_close(chan, ECONNREFUSED);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Process Received Config Reqest.
|
|
*/
|
|
static void
|
|
l2cap_recv_config_req(struct mbuf *m, struct hci_link *link)
|
|
{
|
|
uint8_t buf[L2CAP_MTU_MINIMUM];
|
|
l2cap_cmd_hdr_t cmd;
|
|
l2cap_cfg_req_cp cp;
|
|
l2cap_cfg_opt_t *opt;
|
|
l2cap_cfg_opt_val_t *val;
|
|
l2cap_cfg_rsp_cp *rp;
|
|
struct l2cap_channel *chan;
|
|
int left, len;
|
|
|
|
m_copydata(m, 0, sizeof(cmd), &cmd);
|
|
m_adj(m, sizeof(cmd));
|
|
left = le16toh(cmd.length);
|
|
|
|
if (left < sizeof(cp))
|
|
goto reject;
|
|
|
|
m_copydata(m, 0, sizeof(cp), &cp);
|
|
m_adj(m, sizeof(cp));
|
|
left -= sizeof(cp);
|
|
|
|
cp.dcid = le16toh(cp.dcid);
|
|
cp.flags = le16toh(cp.flags);
|
|
|
|
chan = l2cap_cid_lookup(cp.dcid);
|
|
if (chan == NULL || (chan->lc_state & L2CAP_WAIT_CONFIG_REQ) == 0) {
|
|
l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
|
|
L2CAP_NULL_CID, cp.dcid);
|
|
goto out;
|
|
}
|
|
|
|
/* ready our response packet */
|
|
rp = (l2cap_cfg_rsp_cp *)buf;
|
|
rp->scid = chan->lc_rcid;
|
|
rp->flags = 0; /* "No Continuation" */
|
|
rp->result = L2CAP_SUCCESS;
|
|
len = sizeof(*rp);
|
|
|
|
/*
|
|
* Process the packet. We build the return packet on the fly adding any
|
|
* unacceptable parameters as we go. As we can only return one result,
|
|
* unknown option takes precedence so we start our return packet anew
|
|
* and ignore option values thereafter as they will be re-sent.
|
|
*
|
|
* Since we do not support enough options to make overflowing the min
|
|
* MTU size an issue in normal use, we just reject config requests that
|
|
* would make that happen. This could be if:
|
|
*
|
|
* 1. too many unknown option types (>40!)
|
|
* 2. repeated options we found unacceptable
|
|
* 3. corrupted packet looks like above
|
|
*/
|
|
while (left > 0) {
|
|
if (left < sizeof(*opt) || len + sizeof(*opt) > sizeof(buf))
|
|
goto reject;
|
|
|
|
opt = (l2cap_cfg_opt_t *)&buf[len];
|
|
m_copydata(m, 0, sizeof(*opt), opt);
|
|
m_adj(m, sizeof(*opt));
|
|
left -= sizeof(*opt);
|
|
|
|
if (left < opt->length)
|
|
goto reject;
|
|
|
|
val = (l2cap_cfg_opt_val_t *)&buf[len + sizeof(*opt)];
|
|
|
|
switch(opt->type & L2CAP_OPT_HINT_MASK) {
|
|
case L2CAP_OPT_MTU:
|
|
if (rp->result == L2CAP_UNKNOWN_OPTION)
|
|
break;
|
|
|
|
if (opt->length != L2CAP_OPT_MTU_SIZE
|
|
|| len + sizeof(*opt) + L2CAP_OPT_MTU_SIZE > sizeof(buf))
|
|
goto reject;
|
|
|
|
m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, val);
|
|
val->mtu = le16toh(val->mtu);
|
|
|
|
/*
|
|
* XXX how do we know what the minimum acceptable MTU is
|
|
* for a channel? Spec says some profiles have a higher
|
|
* minimum but I have no way to find that out at this
|
|
* juncture..
|
|
*/
|
|
if (val->mtu < L2CAP_MTU_MINIMUM) {
|
|
rp->result = L2CAP_UNACCEPTABLE_PARAMS;
|
|
val->mtu = htole16(L2CAP_MTU_MINIMUM);
|
|
len += sizeof(*opt) + L2CAP_OPT_MTU_SIZE;
|
|
} else
|
|
chan->lc_omtu = val->mtu;
|
|
|
|
break;
|
|
|
|
case L2CAP_OPT_FLUSH_TIMO:
|
|
if (rp->result == L2CAP_UNKNOWN_OPTION)
|
|
break;
|
|
|
|
if (opt->length != L2CAP_OPT_FLUSH_TIMO_SIZE
|
|
|| len + sizeof(*opt) + L2CAP_OPT_FLUSH_TIMO_SIZE > sizeof(buf))
|
|
goto reject;
|
|
|
|
/*
|
|
* I think that this is informational only - he is
|
|
* informing us of the flush timeout he will be using.
|
|
* I dont think this affects us in any significant way,
|
|
* so just ignore this value for now.
|
|
*/
|
|
break;
|
|
|
|
case L2CAP_OPT_QOS:
|
|
default:
|
|
/* ignore hints */
|
|
if (opt->type & L2CAP_OPT_HINT_BIT)
|
|
break;
|
|
|
|
/* unknown options supercede all else */
|
|
if (rp->result != L2CAP_UNKNOWN_OPTION) {
|
|
rp->result = L2CAP_UNKNOWN_OPTION;
|
|
len = sizeof(*rp);
|
|
}
|
|
|
|
buf[len++] = opt->type;
|
|
buf[len++] = 0; // no need to return his data
|
|
break;
|
|
}
|
|
|
|
m_adj(m, opt->length);
|
|
left -= opt->length;
|
|
}
|
|
|
|
rp->result = htole16(rp->result);
|
|
l2cap_send_signal(link, L2CAP_CONFIG_RSP, cmd.ident, len, buf);
|
|
|
|
if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0
|
|
&& rp->result == L2CAP_SUCCESS) {
|
|
|
|
chan->lc_state &= ~L2CAP_WAIT_CONFIG_REQ;
|
|
|
|
if ((chan->lc_state & L2CAP_WAIT_CONFIG_RSP) == 0) {
|
|
chan->lc_state = L2CAP_OPEN;
|
|
// XXX how to distinguish REconfiguration?
|
|
(*chan->lc_proto->connected)(chan->lc_upper);
|
|
}
|
|
}
|
|
return;
|
|
|
|
reject:
|
|
l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
|
|
out:
|
|
m_adj(m, left);
|
|
}
|
|
|
|
/*
|
|
* Process Received Config Response.
|
|
*/
|
|
static void
|
|
l2cap_recv_config_rsp(struct mbuf *m, struct hci_link *link)
|
|
{
|
|
l2cap_cmd_hdr_t cmd;
|
|
l2cap_cfg_rsp_cp cp;
|
|
l2cap_cfg_opt_t opt;
|
|
l2cap_cfg_opt_val_t val;
|
|
struct l2cap_req *req;
|
|
struct l2cap_channel *chan;
|
|
int left;
|
|
|
|
m_copydata(m, 0, sizeof(cmd), &cmd);
|
|
m_adj(m, sizeof(cmd));
|
|
left = le16toh(cmd.length);
|
|
|
|
if (left < sizeof(cp))
|
|
goto out;
|
|
|
|
m_copydata(m, 0, sizeof(cp), &cp);
|
|
m_adj(m, sizeof(cp));
|
|
left -= sizeof(cp);
|
|
|
|
cp.scid = le16toh(cp.scid);
|
|
cp.flags = le16toh(cp.flags);
|
|
cp.result = le16toh(cp.result);
|
|
|
|
req = l2cap_request_lookup(link, cmd.ident);
|
|
if (req == NULL || req->lr_code != L2CAP_CONFIG_REQ)
|
|
goto out;
|
|
|
|
chan = req->lr_chan;
|
|
if (chan != NULL && chan->lc_lcid != cp.scid)
|
|
goto out;
|
|
|
|
l2cap_request_free(req);
|
|
|
|
if (chan == NULL || (chan->lc_state & L2CAP_WAIT_CONFIG_RSP) == 0)
|
|
goto out;
|
|
|
|
if ((cp.flags & L2CAP_OPT_CFLAG_BIT)) {
|
|
l2cap_cfg_req_cp rp;
|
|
|
|
/*
|
|
* They have more to tell us and want another ID to
|
|
* use, so send an empty config request
|
|
*/
|
|
if (l2cap_request_alloc(chan, L2CAP_CONFIG_REQ))
|
|
goto discon;
|
|
|
|
rp.dcid = htole16(cp.scid);
|
|
rp.flags = 0;
|
|
|
|
if (l2cap_send_signal(link, L2CAP_CONFIG_REQ, link->hl_lastid,
|
|
sizeof(rp), &rp))
|
|
goto discon;
|
|
}
|
|
|
|
switch(cp.result) {
|
|
case L2CAP_SUCCESS:
|
|
/*
|
|
* If continuation flag was not set, our config request was
|
|
* accepted. We may have to wait for their config request to
|
|
* complete, so check that but otherwise we are open
|
|
*
|
|
* There may be 'advisory' values in the packet but we just
|
|
* ignore those..
|
|
*/
|
|
if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) {
|
|
chan->lc_state &= ~L2CAP_WAIT_CONFIG_RSP;
|
|
|
|
if ((chan->lc_state & L2CAP_WAIT_CONFIG_REQ) == 0) {
|
|
chan->lc_state = L2CAP_OPEN;
|
|
// XXX how to distinguish REconfiguration?
|
|
(*chan->lc_proto->connected)(chan->lc_upper);
|
|
}
|
|
}
|
|
goto out;
|
|
|
|
case L2CAP_UNACCEPTABLE_PARAMS:
|
|
/*
|
|
* Packet contains unacceptable parameters with preferred values
|
|
*/
|
|
while (left > 0) {
|
|
if (left < sizeof(opt))
|
|
goto discon;
|
|
|
|
m_copydata(m, 0, sizeof(opt), &opt);
|
|
m_adj(m, sizeof(opt));
|
|
left -= sizeof(opt);
|
|
|
|
if (left < opt.length)
|
|
goto discon;
|
|
|
|
switch (opt.type) {
|
|
case L2CAP_OPT_MTU:
|
|
if (opt.length != L2CAP_OPT_MTU_SIZE)
|
|
goto discon;
|
|
|
|
m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val);
|
|
chan->lc_imtu = le16toh(val.mtu);
|
|
if (chan->lc_imtu < L2CAP_MTU_MINIMUM)
|
|
chan->lc_imtu = L2CAP_MTU_DEFAULT;
|
|
break;
|
|
|
|
case L2CAP_OPT_FLUSH_TIMO:
|
|
if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE)
|
|
goto discon;
|
|
|
|
/*
|
|
* Spec says: If we cannot honor proposed value,
|
|
* either disconnect or try again with original
|
|
* value. I can't really see why they want to
|
|
* interfere with OUR flush timeout in any case
|
|
* so we just punt for now.
|
|
*/
|
|
goto discon;
|
|
|
|
case L2CAP_OPT_QOS:
|
|
break;
|
|
|
|
default:
|
|
UNKNOWN(opt.type);
|
|
goto discon;
|
|
}
|
|
|
|
m_adj(m, opt.length);
|
|
left -= opt.length;
|
|
}
|
|
|
|
if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
|
|
l2cap_send_config_req(chan); // no state change
|
|
|
|
goto out;
|
|
|
|
case L2CAP_REJECT:
|
|
goto discon;
|
|
|
|
case L2CAP_UNKNOWN_OPTION:
|
|
/*
|
|
* Packet contains options not understood. Turn off unknown
|
|
* options by setting them to default values (means they will
|
|
* not be requested again).
|
|
*
|
|
* If our option was already off then fail (paranoia?)
|
|
*
|
|
* XXX Should we consider that options were set for a reason?
|
|
*/
|
|
while (left > 0) {
|
|
if (left < sizeof(opt))
|
|
goto discon;
|
|
|
|
m_copydata(m, 0, sizeof(opt), &opt);
|
|
m_adj(m, sizeof(opt));
|
|
left -= sizeof(opt);
|
|
|
|
if (left < opt.length)
|
|
goto discon;
|
|
|
|
m_adj(m, opt.length);
|
|
left -= opt.length;
|
|
|
|
switch(opt.type) {
|
|
case L2CAP_OPT_MTU:
|
|
if (chan->lc_imtu == L2CAP_MTU_DEFAULT)
|
|
goto discon;
|
|
|
|
chan->lc_imtu = L2CAP_MTU_DEFAULT;
|
|
break;
|
|
|
|
case L2CAP_OPT_FLUSH_TIMO:
|
|
if (chan->lc_flush == L2CAP_FLUSH_TIMO_DEFAULT)
|
|
goto discon;
|
|
|
|
chan->lc_flush = L2CAP_FLUSH_TIMO_DEFAULT;
|
|
break;
|
|
|
|
case L2CAP_OPT_QOS:
|
|
break;
|
|
|
|
default:
|
|
UNKNOWN(opt.type);
|
|
goto discon;
|
|
}
|
|
}
|
|
|
|
if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
|
|
l2cap_send_config_req(chan); /* no state change */
|
|
|
|
goto out;
|
|
|
|
default:
|
|
UNKNOWN(cp.result);
|
|
goto discon;
|
|
}
|
|
|
|
DPRINTF("how did I get here!?\n");
|
|
|
|
discon:
|
|
l2cap_send_disconnect_req(chan);
|
|
l2cap_close(chan, ECONNABORTED);
|
|
|
|
out:
|
|
m_adj(m, left);
|
|
}
|
|
|
|
/*
|
|
* Process Received Disconnect Request. We must validate scid and dcid
|
|
* just in case but otherwise this connection is finished.
|
|
*/
|
|
static void
|
|
l2cap_recv_disconnect_req(struct mbuf *m, struct hci_link *link)
|
|
{
|
|
l2cap_cmd_hdr_t cmd;
|
|
l2cap_discon_req_cp cp;
|
|
l2cap_discon_rsp_cp rp;
|
|
struct l2cap_channel *chan;
|
|
|
|
m_copydata(m, 0, sizeof(cmd), &cmd);
|
|
m_adj(m, sizeof(cmd));
|
|
|
|
m_copydata(m, 0, sizeof(cp), &cp);
|
|
m_adj(m, sizeof(cp));
|
|
|
|
cp.scid = le16toh(cp.scid);
|
|
cp.dcid = le16toh(cp.dcid);
|
|
|
|
chan = l2cap_cid_lookup(cp.dcid);
|
|
if (chan == NULL || chan->lc_rcid != cp.scid) {
|
|
l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
|
|
cp.dcid, cp.scid);
|
|
return;
|
|
}
|
|
|
|
rp.dcid = htole16(chan->lc_lcid);
|
|
rp.scid = htole16(chan->lc_rcid);
|
|
l2cap_send_signal(link, L2CAP_DISCONNECT_RSP, cmd.ident,
|
|
sizeof(rp), &rp);
|
|
|
|
if (chan->lc_state != L2CAP_CLOSED)
|
|
l2cap_close(chan, ECONNRESET);
|
|
}
|
|
|
|
/*
|
|
* Process Received Disconnect Response. We must validate scid and dcid but
|
|
* unless we were waiting for this signal, ignore it.
|
|
*/
|
|
static void
|
|
l2cap_recv_disconnect_rsp(struct mbuf *m, struct hci_link *link)
|
|
{
|
|
l2cap_cmd_hdr_t cmd;
|
|
l2cap_discon_rsp_cp cp;
|
|
struct l2cap_req *req;
|
|
struct l2cap_channel *chan;
|
|
|
|
m_copydata(m, 0, sizeof(cmd), &cmd);
|
|
m_adj(m, sizeof(cmd));
|
|
|
|
m_copydata(m, 0, sizeof(cp), &cp);
|
|
m_adj(m, sizeof(cp));
|
|
|
|
cp.scid = le16toh(cp.scid);
|
|
cp.dcid = le16toh(cp.dcid);
|
|
|
|
req = l2cap_request_lookup(link, cmd.ident);
|
|
if (req == NULL || req->lr_code != L2CAP_DISCONNECT_REQ)
|
|
return;
|
|
|
|
chan = req->lr_chan;
|
|
if (chan == NULL
|
|
|| chan->lc_lcid != cp.scid
|
|
|| chan->lc_rcid != cp.dcid)
|
|
return;
|
|
|
|
l2cap_request_free(req);
|
|
|
|
if (chan->lc_state != L2CAP_WAIT_DISCONNECT)
|
|
return;
|
|
|
|
l2cap_close(chan, 0);
|
|
}
|
|
|
|
/*
|
|
* Process Received Info Request. We must respond but alas dont
|
|
* support anything as yet so thats easy.
|
|
*/
|
|
static void
|
|
l2cap_recv_info_req(struct mbuf *m, struct hci_link *link)
|
|
{
|
|
l2cap_cmd_hdr_t cmd;
|
|
l2cap_info_req_cp cp;
|
|
l2cap_info_rsp_cp rp;
|
|
|
|
m_copydata(m, 0, sizeof(cmd), &cmd);
|
|
m_adj(m, sizeof(cmd));
|
|
|
|
m_copydata(m, 0, sizeof(cp), &cp);
|
|
m_adj(m, sizeof(cp));
|
|
|
|
switch(le16toh(cp.type)) {
|
|
case L2CAP_CONNLESS_MTU:
|
|
case L2CAP_EXTENDED_FEATURES:
|
|
default:
|
|
rp.type = cp.type;
|
|
rp.result = htole16(L2CAP_NOT_SUPPORTED);
|
|
|
|
l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident,
|
|
sizeof(rp), &rp);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Construct signal and wrap in C-Frame for link.
|
|
*/
|
|
static int
|
|
l2cap_send_signal(struct hci_link *link, uint8_t code, uint8_t ident,
|
|
uint16_t length, void *data)
|
|
{
|
|
struct mbuf *m;
|
|
l2cap_hdr_t *hdr;
|
|
l2cap_cmd_hdr_t *cmd;
|
|
|
|
#ifdef DIAGNOSTIC
|
|
if (link == NULL)
|
|
return ENETDOWN;
|
|
|
|
if (sizeof(l2cap_cmd_hdr_t) + length > link->hl_mtu)
|
|
printf("(%s) exceeding L2CAP Signal MTU for link!\n",
|
|
link->hl_unit->hci_devname);
|
|
#endif
|
|
|
|
m = m_gethdr(M_DONTWAIT, MT_DATA);
|
|
if (m == NULL)
|
|
return ENOMEM;
|
|
|
|
hdr = mtod(m, l2cap_hdr_t *);
|
|
cmd = (l2cap_cmd_hdr_t *)(hdr + 1);
|
|
|
|
m->m_len = m->m_pkthdr.len = MHLEN;
|
|
|
|
/* Command Data */
|
|
if (length > 0)
|
|
m_copyback(m, sizeof(hdr) + sizeof(cmd), length, data);
|
|
|
|
/* Command Header */
|
|
cmd->code = code;
|
|
cmd->ident = ident;
|
|
cmd->length = htole16(length);
|
|
length += sizeof(cmd);
|
|
|
|
/* C-Frame Header */
|
|
hdr->length = htole16(length);
|
|
hdr->dcid = htole16(L2CAP_SIGNAL_CID);
|
|
length += sizeof(hdr);
|
|
|
|
if (m->m_pkthdr.len != MAX(MHLEN, length)) {
|
|
m_freem(m);
|
|
return ENOMEM;
|
|
}
|
|
|
|
m->m_pkthdr.len = length;
|
|
m->m_len = MIN(length, MHLEN);
|
|
|
|
DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
|
|
link->hl_unit->hci_devname, code, ident, length);
|
|
|
|
return hci_acl_send(m, link, NULL);
|
|
}
|
|
|
|
/*
|
|
* Send Command Reject packet.
|
|
*/
|
|
static int
|
|
l2cap_send_command_rej(struct hci_link *link, uint8_t ident,
|
|
uint16_t reason, ...)
|
|
{
|
|
l2cap_cmd_rej_cp cp;
|
|
int len = 0;
|
|
va_list ap;
|
|
|
|
va_start(ap, reason);
|
|
|
|
cp.reason = htole16(reason);
|
|
|
|
switch (reason) {
|
|
case L2CAP_REJ_NOT_UNDERSTOOD:
|
|
len = 2;
|
|
break;
|
|
|
|
case L2CAP_REJ_MTU_EXCEEDED:
|
|
len = 4;
|
|
cp.data[0] = va_arg(ap, int); /* SigMTU */
|
|
cp.data[0] = htole16(cp.data[0]);
|
|
break;
|
|
|
|
case L2CAP_REJ_INVALID_CID:
|
|
len = 6;
|
|
cp.data[0] = va_arg(ap, int); /* dcid */
|
|
cp.data[0] = htole16(cp.data[0]);
|
|
cp.data[1] = va_arg(ap, int); /* scid */
|
|
cp.data[1] = htole16(cp.data[1]);
|
|
break;
|
|
|
|
default:
|
|
UNKNOWN(reason);
|
|
return EINVAL;
|
|
}
|
|
|
|
va_end(ap);
|
|
|
|
return l2cap_send_signal(link, L2CAP_COMMAND_REJ, ident, len, &cp);
|
|
}
|
|
|
|
/*
|
|
* Send Connect Request
|
|
*/
|
|
int
|
|
l2cap_send_connect_req(struct l2cap_channel *chan)
|
|
{
|
|
l2cap_con_req_cp cp;
|
|
int err;
|
|
|
|
err = l2cap_request_alloc(chan, L2CAP_CONNECT_REQ);
|
|
if (err)
|
|
return err;
|
|
|
|
cp.psm = htole16(chan->lc_raddr.bt_psm);
|
|
cp.scid = htole16(chan->lc_lcid);
|
|
|
|
return l2cap_send_signal(chan->lc_link, L2CAP_CONNECT_REQ,
|
|
chan->lc_link->hl_lastid, sizeof(cp), &cp);
|
|
}
|
|
|
|
/*
|
|
* Send Config Request
|
|
*
|
|
* For outgoing config request, we only put options in the packet if they
|
|
* differ from the default and would have to be actioned. We dont support
|
|
* enough option types to make overflowing SigMTU an issue so it can all
|
|
* go in one packet.
|
|
*/
|
|
int
|
|
l2cap_send_config_req(struct l2cap_channel *chan)
|
|
{
|
|
l2cap_cfg_req_cp *cp;
|
|
l2cap_cfg_opt_t *opt;
|
|
l2cap_cfg_opt_val_t *val;
|
|
uint8_t *next, buf[L2CAP_MTU_MINIMUM];
|
|
int err;
|
|
|
|
err = l2cap_request_alloc(chan, L2CAP_CONFIG_REQ);
|
|
if (err)
|
|
return err;
|
|
|
|
/* Config Header (4 octets) */
|
|
cp = (l2cap_cfg_req_cp *)buf;
|
|
cp->dcid = htole16(chan->lc_rcid);
|
|
cp->flags = 0; /* "No Continuation" */
|
|
|
|
next = buf + sizeof(l2cap_cfg_req_cp);
|
|
|
|
/* Incoming MTU (4 octets) */
|
|
if (chan->lc_imtu != L2CAP_MTU_DEFAULT) {
|
|
opt = (l2cap_cfg_opt_t *)next;
|
|
opt->type = L2CAP_OPT_MTU;
|
|
opt->length = L2CAP_OPT_MTU_SIZE;
|
|
|
|
val = (l2cap_cfg_opt_val_t *)(opt + 1);
|
|
val->mtu = htole16(chan->lc_imtu);
|
|
|
|
next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_MTU_SIZE;
|
|
}
|
|
|
|
/* Flush Timeout (4 octets) */
|
|
if (chan->lc_flush != L2CAP_FLUSH_TIMO_DEFAULT) {
|
|
opt = (l2cap_cfg_opt_t *)next;
|
|
opt->type = L2CAP_OPT_FLUSH_TIMO;
|
|
opt->length = L2CAP_OPT_FLUSH_TIMO_SIZE;
|
|
|
|
val = (l2cap_cfg_opt_val_t *)(opt + 1);
|
|
val->flush_timo = htole16(chan->lc_flush);
|
|
|
|
next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_FLUSH_TIMO_SIZE;
|
|
}
|
|
|
|
/* Outgoing QoS Flow (24 octets) */
|
|
/* Retransmission & Flow Control (11 octets) */
|
|
/*
|
|
* From here we need to start paying attention to SigMTU as we have
|
|
* possibly overflowed the minimum supported..
|
|
*/
|
|
|
|
return l2cap_send_signal(chan->lc_link, L2CAP_CONFIG_REQ,
|
|
chan->lc_link->hl_lastid, (int)(next - buf), buf);
|
|
}
|
|
|
|
/*
|
|
* Send Disconnect Request
|
|
*/
|
|
int
|
|
l2cap_send_disconnect_req(struct l2cap_channel *chan)
|
|
{
|
|
l2cap_discon_req_cp cp;
|
|
int err;
|
|
|
|
err = l2cap_request_alloc(chan, L2CAP_DISCONNECT_REQ);
|
|
if (err)
|
|
return err;
|
|
|
|
cp.dcid = htole16(chan->lc_rcid);
|
|
cp.scid = htole16(chan->lc_lcid);
|
|
|
|
return l2cap_send_signal(chan->lc_link, L2CAP_DISCONNECT_REQ,
|
|
chan->lc_link->hl_lastid, sizeof(cp), &cp);
|
|
}
|