/*
* Receive data, and do something with it.
* Actually we receive a queue item which holds the data.
* If we free the item it will also free the data unless we have
* previously disassociated it using the NGI_GET_M() macro.
* Possibly send it out on another link after processing.
* Possibly do something different if it comes from different
* hooks. The caller will never free m, so if we use up this data or
* abort we must free it.
*
* If we want, we may decide to force this data to be queued and reprocessed
* at the netgraph NETISR time.
* We would do that by setting the HK_QUEUE flag on our hook. We would do that
* in the connect() method.
*/
static int
ng_xxx_rcvdata(hook_p hook, item_p item )
{
const xxx_p xxxp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
int chan = -2;
int dlci = -2;
int error;
struct mbuf *m;
NGI_GET_M(item, m);
if (NG_HOOK_PRIVATE(hook)) {
dlci = ((struct XXX_hookinfo *) NG_HOOK_PRIVATE(hook))->dlci;
chan = ((struct XXX_hookinfo *) NG_HOOK_PRIVATE(hook))->channel;
if (dlci != -1) {
/* If received on a DLCI hook process for this
* channel and pass it to the downstream module.
* Normally one would add a multiplexing header at
* the front here */
/* M_PREPEND(....) ; */
/* mtod(m, xxxxxx)->dlci = dlci; */
NG_FWD_NEW_DATA(error, item,
xxxp->downstream_hook.hook, m);
xxxp->packets_out++;
} else {
/* data came from the multiplexed link */
dlci = 1; /* get dlci from header */
/* madjust(....) *//* chop off header */
for (chan = 0; chan < XXX_NUM_DLCIS; chan++)
if (xxxp->channel[chan].dlci == dlci)
break;
if (chan == XXX_NUM_DLCIS) {
NG_FREE_ITEM(item);
NG_FREE_M(m);
return (ENETUNREACH);
}
/* If we were called at splnet, use the following:
* NG_SEND_DATA_ONLY(error, otherhook, m); if this
* node is running at some SPL other than SPLNET
* then you should use instead: error =
* ng_queueit(otherhook, m, NULL); m = NULL;
* This queues the data using the standard NETISR
* system and schedules the data to be picked
* up again once the system has moved to SPLNET and
* the processing of the data can continue. After
* these are run 'm' should be considered
* as invalid and NG_SEND_DATA actually zaps them. */
NG_FWD_NEW_DATA(error, item,
xxxp->channel[chan].hook, m);
xxxp->packets_in++;
}
} else {
/* It's the debug hook, throw it away.. */
if (hook == xxxp->downstream_hook.hook) {
NG_FREE_ITEM(item);
NG_FREE_M(m);
}
}
return 0;
}
static void
bt3c_forward(node_p node, hook_p hook, void *arg1, int arg2)
{
bt3c_softc_p sc = (bt3c_softc_p) NG_NODE_PRIVATE(node);
struct mbuf *m = NULL;
int error;
if (sc == NULL)
return;
if (sc->hook != NULL && NG_HOOK_IS_VALID(sc->hook)) {
for (;;) {
IF_DEQUEUE(&sc->inq, m);
if (m == NULL)
break;
NG_SEND_DATA_ONLY(error, sc->hook, m);
if (error != 0)
NG_BT3C_STAT_IERROR(sc->stat);
}
} else {
IF_LOCK(&sc->inq);
for (;;) {
_IF_DEQUEUE(&sc->inq, m);
if (m == NULL)
break;
NG_BT3C_STAT_IERROR(sc->stat);
NG_FREE_M(m);
}
IF_UNLOCK(&sc->inq);
}
} /* bt3c_forward */
static int
bt3c_pccard_detach(device_t dev)
{
bt3c_softc_p sc = (bt3c_softc_p) device_get_softc(dev);
if (sc == NULL)
return (0);
unregister_swi(sc->ith, SWI_TTY, -1);
sc->ith = NULL;
bus_teardown_intr(dev, sc->irq, sc->irq_cookie);
bus_release_resource(dev, SYS_RES_IRQ, sc->irq_rid, sc->irq);
sc->irq_cookie = NULL;
sc->irq = NULL;
sc->irq_rid = 0;
bus_release_resource(dev, SYS_RES_IOPORT, sc->iobase_rid, sc->iobase);
sc->iobase = NULL;
sc->iobase_rid = 0;
if (sc->node != NULL) {
NG_NODE_SET_PRIVATE(sc->node, NULL);
ng_rmnode_self(sc->node);
sc->node = NULL;
}
NG_FREE_M(sc->m);
IF_DRAIN(&sc->inq);
IF_DRAIN(&sc->outq);
return (0);
} /* bt3c_pccacd_detach */
/*
* Decode an incoming frame coming from the switch
*/
static int
ngfrm_decode(node_p node, item_p item)
{
const sc_p sc = NG_NODE_PRIVATE(node);
char *data;
int alen;
u_int dlci = 0;
int error = 0;
int ctxnum;
struct mbuf *m;
NGI_GET_M(item, m);
if (m->m_len < 4 && (m = m_pullup(m, 4)) == NULL) {
error = ENOBUFS;
goto out;
}
data = mtod(m, char *);
if ((alen = sc->addrlen) == 0) {
sc->addrlen = alen = ngfrm_addrlen(data);
}
switch (alen) {
case 2:
SHIFTIN(makeup + 0, data[0], dlci);
SHIFTIN(makeup + 1, data[1], dlci);
break;
case 3:
SHIFTIN(makeup + 0, data[0], dlci);
SHIFTIN(makeup + 1, data[1], dlci);
SHIFTIN(makeup + 3, data[2], dlci); /* 3 and 2 is correct */
break;
case 4:
SHIFTIN(makeup + 0, data[0], dlci);
SHIFTIN(makeup + 1, data[1], dlci);
SHIFTIN(makeup + 2, data[2], dlci);
SHIFTIN(makeup + 3, data[3], dlci);
break;
default:
error = EINVAL;
goto out;
}
if (dlci > 1023) {
error = EINVAL;
goto out;
}
ctxnum = sc->ALT[dlci];
if ((ctxnum & CTX_VALID) && sc->channel[ctxnum &= CTX_VALUE].hook) {
/* Send it */
m_adj(m, alen);
NG_FWD_NEW_DATA(error, item, sc->channel[ctxnum].hook, m);
return (error);
} else {
error = ENETDOWN;
}
out:
NG_FREE_ITEM(item);
NG_FREE_M(m);
return (error);
}
int
ng_l2cap_l2ca_ping_rsp(ng_l2cap_con_p con, u_int32_t token, u_int16_t result,
struct mbuf *data)
{
ng_l2cap_p l2cap = con->l2cap;
struct ng_mesg *msg = NULL;
ng_l2cap_l2ca_ping_op *op = NULL;
int error = 0, size = 0;
/* Check if control hook is connected and valid */
if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) {
NG_L2CAP_WARN(
"%s: %s - unable to send L2CA_Ping response message. " \
"Hook is not connected or valid\n",
__func__, NG_NODE_NAME(l2cap->node));
error = ENOTCONN;
goto out;
}
size = (data == NULL)? 0 : data->m_pkthdr.len;
/* Create and send L2CA_Ping response message */
NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_PING,
sizeof(*op) + size, M_NOWAIT);
if (msg == NULL)
error = ENOMEM;
else {
msg->header.token = token;
msg->header.flags |= NGF_RESP;
op = (ng_l2cap_l2ca_ping_op *)(msg->data);
op->result = result;
bcopy(&con->remote, &op->bdaddr, sizeof(op->bdaddr));
if (data != NULL && size > 0) {
op->echo_size = size;
m_copydata(data, 0, size, (caddr_t) op + sizeof(*op));
}
NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0);
}
out:
NG_FREE_M(data);
return (error);
} /* ng_l2cap_l2ca_ping_rsp */
/*
* Accept data from the hook and queue it for output.
*/
Static int
ng_udbp_rcvdata(hook_p hook, item_p item)
{
const udbp_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
int error;
struct ifqueue *xmitq_p;
int s;
struct mbuf *m;
meta_p meta;
NGI_GET_M(item, m);
NGI_GET_META(item, meta);
NG_FREE_ITEM(item);
/*
* Now queue the data for when it can be sent
*/
if (meta && meta->priority > 0) {
xmitq_p = (&sc->xmitq_hipri);
} else {
xmitq_p = (&sc->xmitq);
}
s = splusb();
IF_LOCK(xmitq_p);
if (_IF_QFULL(xmitq_p)) {
_IF_DROP(xmitq_p);
IF_UNLOCK(xmitq_p);
splx(s);
error = ENOBUFS;
goto bad;
}
_IF_ENQUEUE(xmitq_p, m);
IF_UNLOCK(xmitq_p);
if (!(sc->flags & OUT_BUSY))
udbp_setup_out_transfer(sc);
splx(s);
return (0);
bad: /*
* It was an error case.
* check if we need to free the mbuf, and then return the error
*/
NG_FREE_M(m);
NG_FREE_META(meta);
return (error);
}
static int
ubt_fwd_mbuf_up(ubt_softc_p sc, struct mbuf **m)
{
hook_p hook;
int error;
/*
* Close the race with Netgraph hook newhook/disconnect methods.
* Save the hook pointer atomically. Two cases are possible:
*
* 1) The hook pointer is NULL. It means disconnect method got
* there first. In this case we are done.
*
* 2) The hook pointer is not NULL. It means that hook pointer
* could be either in valid or invalid (i.e. in the process
* of disconnect) state. In any case grab an extra reference
* to protect the hook pointer.
*
* It is ok to pass hook in invalid state to NG_SEND_DATA_ONLY() as
* it checks for it. Drop extra reference after NG_SEND_DATA_ONLY().
*/
UBT_NG_LOCK(sc);
if ((hook = sc->sc_hook) != NULL)
NG_HOOK_REF(hook);
UBT_NG_UNLOCK(sc);
if (hook == NULL) {
NG_FREE_M(*m);
return (ENETDOWN);
}
NG_SEND_DATA_ONLY(error, hook, *m);
NG_HOOK_UNREF(hook);
if (error != 0)
UBT_STAT_IERROR(sc);
return (error);
} /* ubt_fwd_mbuf_up */
/*
* Receive data on a hook
*/
static int
ngs_rcvdata(hook_p hook, item_p item)
{
struct ngsock *const priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct ngpcb *const pcbp = priv->datasock;
struct socket *so;
struct sockaddr_ng *addr;
char *addrbuf[NG_HOOKSIZ + 4];
int addrlen;
struct mbuf *m;
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
/* If there is no data socket, black-hole it. */
if (pcbp == NULL) {
NG_FREE_M(m);
return (0);
}
so = pcbp->ng_socket;
/* Get the return address into a sockaddr. */
addrlen = strlen(NG_HOOK_NAME(hook)); /* <= NG_HOOKSIZ - 1 */
addr = (struct sockaddr_ng *) addrbuf;
addr->sg_len = addrlen + 3;
addr->sg_family = AF_NETGRAPH;
bcopy(NG_HOOK_NAME(hook), addr->sg_data, addrlen);
addr->sg_data[addrlen] = '\0';
/* Try to tell the socket which hook it came in on. */
if (sbappendaddr((struct sockbuf *)&so->so_rcv, (struct sockaddr *)addr, m, NULL) == 0) {
m_freem(m);
TRAP_ERROR;
return (ENOBUFS);
}
sorwakeup(so);
return (0);
}
static int
ng_bt3c_rcvdata(hook_p hook, item_p item)
{
bt3c_softc_p sc = (bt3c_softc_p)NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct mbuf *m = NULL;
int error = 0;
if (sc == NULL) {
error = EHOSTDOWN;
goto out;
}
if (hook != sc->hook) {
error = EINVAL;
goto out;
}
NGI_GET_M(item, m);
IF_LOCK(&sc->outq);
if (_IF_QFULL(&sc->outq)) {
NG_BT3C_ERR(sc->dev,
"Outgoing queue is full. Dropping mbuf, len=%d\n", m->m_pkthdr.len);
_IF_DROP(&sc->outq);
NG_BT3C_STAT_OERROR(sc->stat);
NG_FREE_M(m);
} else
_IF_ENQUEUE(&sc->outq, m);
IF_UNLOCK(&sc->outq);
error = ng_send_fn(sc->node, NULL, bt3c_send, NULL, 0 /* new send */);
out:
NG_FREE_ITEM(item);
return (error);
} /* ng_bt3c_rcvdata */
/*
* Receive data packet
*/
static int
ngfrm_rcvdata(hook_p hook, item_p item)
{
struct ctxinfo *const ctxp = NG_HOOK_PRIVATE(hook);
int error = 0;
int dlci;
sc_p sc;
int alen;
char *data;
struct mbuf *m;
/* Data doesn't come in from just anywhere (e.g debug hook) */
if (ctxp == NULL) {
error = ENETDOWN;
goto bad;
}
/* If coming from downstream, decode it to a channel */
dlci = ctxp->dlci;
if (dlci == -1)
return (ngfrm_decode(NG_HOOK_NODE(hook), item));
NGI_GET_M(item, m);
/* Derive the softc we will need */
sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
/* If there is no live channel, throw it away */
if ((sc->downstream.hook == NULL)
|| ((ctxp->flags & CHAN_ACTIVE) == 0)) {
error = ENETDOWN;
goto bad;
}
/* Store the DLCI on the front of the packet */
alen = sc->addrlen;
if (alen == 0)
alen = 2; /* default value for transmit */
M_PREPEND(m, alen, M_DONTWAIT);
if (m == NULL) {
error = ENOBUFS;
goto bad;
}
data = mtod(m, char *);
/*
* Shift the lowest bits into the address field untill we are done.
* First byte is MSBits of addr so work backwards.
*/
switch (alen) {
case 2:
data[0] = data[1] = '\0';
SHIFTOUT(makeup + 1, data[1], dlci);
SHIFTOUT(makeup + 0, data[0], dlci);
data[1] |= BYTEX_EA;
break;
case 3:
data[0] = data[1] = data[2] = '\0';
SHIFTOUT(makeup + 3, data[2], dlci); /* 3 and 2 is correct */
SHIFTOUT(makeup + 1, data[1], dlci);
SHIFTOUT(makeup + 0, data[0], dlci);
data[2] |= BYTEX_EA;
break;
case 4:
data[0] = data[1] = data[2] = data[3] = '\0';
SHIFTOUT(makeup + 3, data[3], dlci);
SHIFTOUT(makeup + 2, data[2], dlci);
SHIFTOUT(makeup + 1, data[1], dlci);
SHIFTOUT(makeup + 0, data[0], dlci);
data[3] |= BYTEX_EA;
break;
default:
panic(__func__);
}
/* Send it */
NG_FWD_NEW_DATA(error, item, sc->downstream.hook, m);
return (error);
bad:
NG_FREE_ITEM(item);
NG_FREE_M(m);
return (error);
}
/*
* Check that a packet is entirely kosha.
* return 1 of ok, and 0 if not.
* All data is discarded if a 0 is returned.
*/
static int
nglmi_checkdata(hook_p hook, struct mbuf *m)
{
sc_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
const u_char *data;
u_short packetlen;
unsigned short dlci;
u_char type;
u_char nextbyte;
int seq_seen = 0;
int resptype_seen = 0; /* 0 , 1 (partial) or 2 (full) */
int highest_dlci = 0;
packetlen = m->m_len;
data = mtod(m, const u_char *);
if (*data != 0x03) {
log(LOG_WARNING, "nglmi: unexpected value in LMI(%d)\n", 1);
goto reject;
}
STEPBY(1);
/* look at the protocol ID */
nextbyte = *data;
if (sc->flags & SCF_AUTO) {
SETLMITYPE(sc, SCF_NOLMI); /* start with a clean slate */
switch (nextbyte) {
case 0x8:
sc->protoID = 8;
break;
case 0x9:
SETLMITYPE(sc, SCF_GROUP4);
sc->protoID = 9;
break;
default:
log(LOG_WARNING, "nglmi: bad Protocol ID(%d)\n",
(int) nextbyte);
goto reject;
}
} else {
if (nextbyte != sc->protoID) {
log(LOG_WARNING, "nglmi: unexpected Protocol ID(%d)\n",
(int) nextbyte);
goto reject;
}
}
STEPBY(1);
/* check call reference (always null in non ISDN frame relay) */
if (*data != 0x00) {
log(LOG_WARNING, "nglmi: unexpected Call Reference (0x%x)\n",
data[-1]);
goto reject;
}
STEPBY(1);
/* check message type */
switch ((type = *data)) {
case 0x75: /* Status enquiry */
log(LOG_WARNING, "nglmi: unexpected message type(0x%x)\n",
data[-1]);
goto reject;
case 0x7D: /* Status message */
break;
default:
log(LOG_WARNING,
"nglmi: unexpected msg type(0x%x) \n", (int) type);
goto reject;
}
STEPBY(1);
/* Now check if there is a 'locking shift'. This is only seen in
* Annex D frames. Don't increment immediately as it might not be
* there. */
nextbyte = *data;
if (sc->flags & SCF_AUTO) {
if (!(GROUP4(sc))) {
if (nextbyte == 0x95) {
SETLMITYPE(sc, SCF_ANNEX_D);
STEPBY(1);
} else
SETLMITYPE(sc, SCF_ANNEX_A);
} else if (nextbyte == 0x95) {
log(LOG_WARNING, "nglmi: locking shift seen in G4\n");
goto reject;
}
} else {
if (ANNEXD(sc)) {
if (*data == 0x95)
STEPBY(1);
else {
log(LOG_WARNING,
"nglmi: locking shift missing\n");
goto reject;
}
} else if (*data == 0x95) {
log(LOG_WARNING, "nglmi: locking shift seen\n");
goto reject;
}
}
/* While there is more data in the status packet, keep processing
* status items. First make sure there is enough data for the
* segment descriptor's length field. */
while (packetlen >= 2) {
u_int segtype = data[0];
u_int segsize = data[1];
/* Now that we know how long it claims to be, make sure
* there is enough data for the next seg. */
if (packetlen < (segsize + 2)) {
log(LOG_WARNING, "nglmi: IE longer than packet\n");
break;
}
switch (segtype) {
case 0x01:
case 0x51:
/* According to MCI's HP analyser, we should just
* ignore if there is mor ethan one of these (?). */
if (resptype_seen) {
log(LOG_WARNING, "nglmi: dup MSGTYPE\n");
goto nextIE;
}
if (segsize != 1) {
log(LOG_WARNING, "nglmi: MSGTYPE wrong size\n");
goto reject;
}
/* The remote end tells us what kind of response
* this is. Only expect a type 0 or 1. if it was a
* full (type 0) check we just asked for a type
* full. */
switch (data[2]) {
case 1:/* partial */
if (sc->livs > sc->liv_per_full) {
log(LOG_WARNING,
"nglmi: LIV when FULL expected\n");
goto reject; /* need full */
}
resptype_seen = 1;
break;
case 0:/* full */
/* Full response is always acceptable */
resptype_seen = 2;
break;
default:
log(LOG_WARNING,
"nglmi: Unknown report type %d\n", data[2]);
goto reject;
}
break;
case 0x03:
case 0x53:
/* The remote tells us what it thinks the sequence
* numbers are. I would have thought that there
* needs to be one and only one of these, but MCI
* want us to just ignore extras. (?) */
if (resptype_seen == 0) {
log(LOG_WARNING, "nglmi: no TYPE before SEQ\n");
goto reject;
}
if (seq_seen != 0) /* already seen seq numbers */
goto nextIE;
if (segsize != 2) {
log(LOG_WARNING, "nglmi: bad SEQ sts size\n");
goto reject;
}
if (sc->local_seq != data[3]) {
log(LOG_WARNING, "nglmi: unexpected SEQ\n");
goto reject;
}
seq_seen = 1;
break;
case 0x07:
case 0x57:
/* The remote tells us about a DLCI that it knows
* about. There may be many of these in a single
* status response */
if (seq_seen != 1) { /* already seen seq numbers? */
log(LOG_WARNING,
"nglmi: No sequence before DLCI\n");
goto reject;
}
if (resptype_seen != 2) { /* must be full */
log(LOG_WARNING,
"nglmi: No resp type before DLCI\n");
goto reject;
}
if (GROUP4(sc)) {
if (segsize != 6) {
log(LOG_WARNING,
"nglmi: wrong IE segsize\n");
goto reject;
}
dlci = ((u_short) data[2] & 0xff) << 8;
dlci |= (data[3] & 0xff);
} else {
if (segsize != 3) {
log(LOG_WARNING,
"nglmi: DLCI headersize of %d"
" not supported\n", segsize - 1);
goto reject;
}
dlci = ((u_short) data[2] & 0x3f) << 4;
dlci |= ((data[3] & 0x78) >> 3);
}
/* async can only have one of these */
#if 0 /* async not yet accepted */
if (async && highest_dlci) {
log(LOG_WARNING,
"nglmi: Async with > 1 DLCI\n");
goto reject;
}
#endif
/* Annex D says these will always be Ascending, but
* the HP test for G4 says we should accept
* duplicates, so for now allow that. ( <= vs. < ) */
#if 0
/* MCI tests want us to accept out of order for AnxD */
if ((!GROUP4(sc)) && (dlci < highest_dlci)) {
/* duplicate or mis-ordered dlci */
/* (spec says they will increase in number) */
log(LOG_WARNING, "nglmi: DLCI out of order\n");
goto reject;
}
#endif
if (dlci > 1023) {
log(LOG_WARNING, "nglmi: DLCI out of range\n");
goto reject;
}
highest_dlci = dlci;
break;
default:
log(LOG_WARNING,
"nglmi: unknown LMI segment type %d\n", segtype);
}
nextIE:
STEPBY(segsize + 2);
}
if (packetlen != 0) { /* partial junk at end? */
log(LOG_WARNING,
"nglmi: %d bytes extra at end of packet\n", packetlen);
goto print;
}
if (resptype_seen == 0) {
log(LOG_WARNING, "nglmi: No response type seen\n");
goto reject; /* had no response type */
}
if (seq_seen == 0) {
log(LOG_WARNING, "nglmi: No sequence numbers seen\n");
goto reject; /* had no sequence numbers */
}
return (1);
print:
{
int i, j, k, pos;
char buf[100];
int loc;
const u_char *bp = mtod(m, const u_char *);
k = i = 0;
loc = (m->m_len - packetlen);
log(LOG_WARNING, "nglmi: error at location %d\n", loc);
while (k < m->m_len) {
pos = 0;
j = 0;
while ((j++ < 16) && k < m->m_len) {
pos += sprintf(buf + pos, "%c%02x",
((loc == k) ? '>' : ' '),
bp[k]);
k++;
}
if (i == 0)
log(LOG_WARNING, "nglmi: packet data:%s\n", buf);
else
log(LOG_WARNING, "%04d :%s\n", k, buf);
i++;
}
}
return (1);
reject:
{
int i, j, k, pos;
char buf[100];
int loc;
const u_char *bp = mtod(m, const u_char *);
k = i = 0;
loc = (m->m_len - packetlen);
log(LOG_WARNING, "nglmi: error at location %d\n", loc);
while (k < m->m_len) {
pos = 0;
j = 0;
while ((j++ < 16) && k < m->m_len) {
pos += sprintf(buf + pos, "%c%02x",
((loc == k) ? '>' : ' '),
bp[k]);
k++;
}
if (i == 0)
log(LOG_WARNING, "nglmi: packet data:%s\n", buf);
else
log(LOG_WARNING, "%04d :%s\n", k, buf);
i++;
}
}
NG_FREE_M(m);
return (0);
}
int
ng_l2cap_l2ca_cfg_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
ng_l2cap_l2ca_cfg_rsp_ip *ip = NULL;
ng_l2cap_chan_p ch = NULL;
ng_l2cap_cmd_p cmd = NULL;
struct mbuf *opt = NULL;
u_int16_t *mtu = NULL;
ng_l2cap_flow_p flow = NULL;
int error = 0;
/* Check message */
if (msg->header.arglen != sizeof(*ip)) {
NG_L2CAP_ALERT(
"%s: %s - invalid L2CA_ConfigRsp request message size, size=%d\n",
__func__, NG_NODE_NAME(l2cap->node),
msg->header.arglen);
error = EMSGSIZE;
goto out;
}
ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data);
/* Check if we have this channel */
ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
if (ch == NULL) {
NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_ConfigRsp request message. " \
"Channel does not exist, lcid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
error = ENOENT;
goto out;
}
/* Check channel state */
if (ch->state != NG_L2CAP_CONFIG) {
NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_ConfigRsp request message. " \
"Invalid channel state, state=%d, lcid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->state,
ch->scid);
error = EINVAL;
goto out;
}
/* Set channel settings */
if (ip->omtu != ch->omtu) {
ch->omtu = ip->omtu;
mtu = &ch->omtu;
}
if (bcmp(&ip->iflow, &ch->iflow, sizeof(ch->iflow)) != 0) {
bcopy(&ip->iflow, &ch->iflow, sizeof(ch->iflow));
flow = &ch->iflow;
}
if (mtu != NULL || flow != NULL) {
_ng_l2cap_build_cfg_options(opt, mtu, NULL, flow);
if (opt == NULL) {
error = ENOBUFS;
goto out;
}
}
/* Create L2CAP command */
cmd = ng_l2cap_new_cmd(ch->con, ch, ch->ident, NG_L2CAP_CFG_RSP,
msg->header.token);
if (cmd == NULL) {
NG_FREE_M(opt);
error = ENOMEM;
goto out;
}
_ng_l2cap_cfg_rsp(cmd->aux,cmd->ident,ch->dcid,0,NG_L2CAP_SUCCESS,opt);
if (cmd->aux == NULL) {
ng_l2cap_free_cmd(cmd);
error = ENOBUFS;
goto out;
}
/* XXX FIXME - not here ??? */
ch->cfg_state |= NG_L2CAP_CFG_OUT;
if (ch->cfg_state == NG_L2CAP_CFG_BOTH)
ch->state = NG_L2CAP_OPEN;
/* Link command to the queue */
ng_l2cap_link_cmd(ch->con, cmd);
ng_l2cap_lp_deliver(ch->con);
out:
return (error);
} /* ng_l2cap_l2ca_cfg_rsp_req */
static void
ubt_bulk_read_callback(struct usb_xfer *xfer, usb_error_t error)
{
struct ubt_softc *sc = usbd_xfer_softc(xfer);
struct mbuf *m;
ng_hci_acldata_pkt_t *hdr;
struct usb_page_cache *pc;
int len;
int actlen;
usbd_xfer_status(xfer, &actlen, NULL, NULL, NULL);
m = NULL;
switch (USB_GET_STATE(xfer)) {
case USB_ST_TRANSFERRED:
/* Allocate new mbuf */
MGETHDR(m, M_NOWAIT, MT_DATA);
if (m == NULL) {
UBT_STAT_IERROR(sc);
goto submit_next;
}
if (!(MCLGET(m, M_NOWAIT))) {
UBT_STAT_IERROR(sc);
goto submit_next;
}
/* Add HCI packet type */
*mtod(m, uint8_t *)= NG_HCI_ACL_DATA_PKT;
m->m_pkthdr.len = m->m_len = 1;
if (actlen > MCLBYTES - 1)
actlen = MCLBYTES - 1;
pc = usbd_xfer_get_frame(xfer, 0);
usbd_copy_out(pc, 0, mtod(m, uint8_t *) + 1, actlen);
m->m_pkthdr.len += actlen;
m->m_len += actlen;
UBT_INFO(sc, "got %d bytes from bulk-in pipe\n",
actlen);
/* Validate packet and send it up the stack */
if (m->m_pkthdr.len < (int)sizeof(*hdr)) {
UBT_INFO(sc, "HCI ACL packet is too short\n");
UBT_STAT_IERROR(sc);
goto submit_next;
}
hdr = mtod(m, ng_hci_acldata_pkt_t *);
len = le16toh(hdr->length);
if (len != (int)(m->m_pkthdr.len - sizeof(*hdr))) {
UBT_ERR(sc, "Invalid ACL packet size, length=%d, " \
"pktlen=%d\n", len, m->m_pkthdr.len);
UBT_STAT_IERROR(sc);
goto submit_next;
}
UBT_INFO(sc, "got complete ACL data packet, pktlen=%d, " \
"length=%d\n", m->m_pkthdr.len, len);
UBT_STAT_PCKTS_RECV(sc);
UBT_STAT_BYTES_RECV(sc, m->m_pkthdr.len);
ubt_fwd_mbuf_up(sc, &m);
/* m == NULL at this point */
/* FALLTHOUGH */
case USB_ST_SETUP:
submit_next:
NG_FREE_M(m); /* checks for m != NULL */
usbd_xfer_set_frame_len(xfer, 0, usbd_xfer_max_len(xfer));
usbd_transfer_submit(xfer);
break;
default: /* Error */
if (error != USB_ERR_CANCELLED) {
UBT_WARN(sc, "bulk-in transfer failed: %s\n",
usbd_errstr(error));
/* Try to clear stall first */
usbd_xfer_set_stall(xfer);
goto submit_next;
}
/* transfer cancelled */
break;
}
} /* ubt_bulk_read_callback */
static void
ubt_ctrl_write_callback(struct usb_xfer *xfer, usb_error_t error)
{
struct ubt_softc *sc = usbd_xfer_softc(xfer);
struct usb_device_request req;
struct mbuf *m;
struct usb_page_cache *pc;
int actlen;
usbd_xfer_status(xfer, &actlen, NULL, NULL, NULL);
switch (USB_GET_STATE(xfer)) {
case USB_ST_TRANSFERRED:
UBT_INFO(sc, "sent %d bytes to control pipe\n", actlen);
UBT_STAT_BYTES_SENT(sc, actlen);
UBT_STAT_PCKTS_SENT(sc);
/* FALLTHROUGH */
case USB_ST_SETUP:
send_next:
/* Get next command mbuf, if any */
UBT_NG_LOCK(sc);
NG_BT_MBUFQ_DEQUEUE(&sc->sc_cmdq, m);
UBT_NG_UNLOCK(sc);
if (m == NULL) {
UBT_INFO(sc, "HCI command queue is empty\n");
break; /* transfer complete */
}
/* Initialize a USB control request and then schedule it */
bzero(&req, sizeof(req));
req.bmRequestType = UBT_HCI_REQUEST;
USETW(req.wLength, m->m_pkthdr.len);
UBT_INFO(sc, "Sending control request, " \
"bmRequestType=0x%02x, wLength=%d\n",
req.bmRequestType, UGETW(req.wLength));
pc = usbd_xfer_get_frame(xfer, 0);
usbd_copy_in(pc, 0, &req, sizeof(req));
pc = usbd_xfer_get_frame(xfer, 1);
usbd_m_copy_in(pc, 0, m, 0, m->m_pkthdr.len);
usbd_xfer_set_frame_len(xfer, 0, sizeof(req));
usbd_xfer_set_frame_len(xfer, 1, m->m_pkthdr.len);
usbd_xfer_set_frames(xfer, 2);
NG_FREE_M(m);
usbd_transfer_submit(xfer);
break;
default: /* Error */
if (error != USB_ERR_CANCELLED) {
UBT_WARN(sc, "control transfer failed: %s\n",
usbd_errstr(error));
UBT_STAT_OERROR(sc);
goto send_next;
}
/* transfer cancelled */
break;
}
} /* ubt_ctrl_write_callback */
static int
ng_ubt_rcvdata(hook_p hook, item_p item)
{
struct ubt_softc *sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct mbuf *m;
struct ng_bt_mbufq *q;
int action, error = 0;
if (hook != sc->sc_hook) {
error = EINVAL;
goto done;
}
/* Deatch mbuf and get HCI frame type */
NGI_GET_M(item, m);
/*
* Minimal size of the HCI frame is 4 bytes: 1 byte frame type,
* 2 bytes connection handle and at least 1 byte of length.
* Panic on data frame that has size smaller than 4 bytes (it
* should not happen)
*/
if (m->m_pkthdr.len < 4)
panic("HCI frame size is too small! pktlen=%d\n",
m->m_pkthdr.len);
/* Process HCI frame */
switch (*mtod(m, uint8_t *)) { /* XXX call m_pullup ? */
case NG_HCI_CMD_PKT:
if (m->m_pkthdr.len - 1 > (int)UBT_CTRL_BUFFER_SIZE)
panic("HCI command frame size is too big! " \
"buffer size=%zd, packet len=%d\n",
UBT_CTRL_BUFFER_SIZE, m->m_pkthdr.len);
q = &sc->sc_cmdq;
action = UBT_FLAG_T_START_CTRL;
break;
case NG_HCI_ACL_DATA_PKT:
if (m->m_pkthdr.len - 1 > UBT_BULK_WRITE_BUFFER_SIZE)
panic("ACL data frame size is too big! " \
"buffer size=%d, packet len=%d\n",
UBT_BULK_WRITE_BUFFER_SIZE, m->m_pkthdr.len);
q = &sc->sc_aclq;
action = UBT_FLAG_T_START_BULK;
break;
case NG_HCI_SCO_DATA_PKT:
q = &sc->sc_scoq;
action = 0;
break;
default:
UBT_ERR(sc, "Dropping unsupported HCI frame, type=0x%02x, " \
"pktlen=%d\n", *mtod(m, uint8_t *), m->m_pkthdr.len);
NG_FREE_M(m);
error = EINVAL;
goto done;
/* NOT REACHED */
}
UBT_NG_LOCK(sc);
if (NG_BT_MBUFQ_FULL(q)) {
NG_BT_MBUFQ_DROP(q);
UBT_NG_UNLOCK(sc);
UBT_ERR(sc, "Dropping HCI frame 0x%02x, len=%d. Queue full\n",
*mtod(m, uint8_t *), m->m_pkthdr.len);
NG_FREE_M(m);
} else {
static void
ubt_isoc_write_callback(struct usb_xfer *xfer, usb_error_t error)
{
struct ubt_softc *sc = usbd_xfer_softc(xfer);
struct usb_page_cache *pc;
struct mbuf *m;
int n, space, offset;
int actlen, nframes;
usbd_xfer_status(xfer, &actlen, NULL, NULL, &nframes);
pc = usbd_xfer_get_frame(xfer, 0);
switch (USB_GET_STATE(xfer)) {
case USB_ST_TRANSFERRED:
UBT_INFO(sc, "sent %d bytes to isoc-out pipe\n", actlen);
UBT_STAT_BYTES_SENT(sc, actlen);
UBT_STAT_PCKTS_SENT(sc);
/* FALLTHROUGH */
case USB_ST_SETUP:
send_next:
offset = 0;
space = usbd_xfer_max_framelen(xfer) * nframes;
m = NULL;
while (space > 0) {
if (m == NULL) {
UBT_NG_LOCK(sc);
NG_BT_MBUFQ_DEQUEUE(&sc->sc_scoq, m);
UBT_NG_UNLOCK(sc);
if (m == NULL)
break;
}
n = min(space, m->m_pkthdr.len);
if (n > 0) {
usbd_m_copy_in(pc, offset, m,0, n);
m_adj(m, n);
offset += n;
space -= n;
}
if (m->m_pkthdr.len == 0)
NG_FREE_M(m); /* sets m = NULL */
}
/* Put whatever is left from mbuf back on queue */
if (m != NULL) {
UBT_NG_LOCK(sc);
NG_BT_MBUFQ_PREPEND(&sc->sc_scoq, m);
UBT_NG_UNLOCK(sc);
}
/*
* Calculate sizes for isoc frames.
* Note that offset could be 0 at this point (i.e. we have
* nothing to send). That is fine, as we have isoc. transfers
* going in both directions all the time. In this case it
* would be just empty isoc. transfer.
*/
for (n = 0; n < nframes; n ++) {
usbd_xfer_set_frame_len(xfer, n,
min(offset, usbd_xfer_max_framelen(xfer)));
offset -= usbd_xfer_frame_len(xfer, n);
}
usbd_transfer_submit(xfer);
break;
default: /* Error */
if (error != USB_ERR_CANCELLED) {
UBT_STAT_OERROR(sc);
goto send_next;
}
/* transfer cancelled */
break;
}
}
static int
ubt_isoc_read_one_frame(struct usb_xfer *xfer, int frame_no)
{
struct ubt_softc *sc = usbd_xfer_softc(xfer);
struct usb_page_cache *pc;
struct mbuf *m;
int len, want, got, total;
/* Get existing SCO reassembly buffer */
pc = usbd_xfer_get_frame(xfer, 0);
m = sc->sc_isoc_in_buffer;
total = usbd_xfer_frame_len(xfer, frame_no);
/* While we have data in the frame */
while (total > 0) {
if (m == NULL) {
/* Start new reassembly buffer */
MGETHDR(m, M_NOWAIT, MT_DATA);
if (m == NULL) {
UBT_STAT_IERROR(sc);
return (-1); /* XXX out of sync! */
}
if (!(MCLGET(m, M_NOWAIT))) {
UBT_STAT_IERROR(sc);
NG_FREE_M(m);
return (-1); /* XXX out of sync! */
}
/* Expect SCO header */
*mtod(m, uint8_t *) = NG_HCI_SCO_DATA_PKT;
m->m_pkthdr.len = m->m_len = got = 1;
want = sizeof(ng_hci_scodata_pkt_t);
} else {
/*
* Check if we have SCO header and if so
* adjust amount of data we want
*/
got = m->m_pkthdr.len;
want = sizeof(ng_hci_scodata_pkt_t);
if (got >= want)
want += mtod(m, ng_hci_scodata_pkt_t *)->length;
}
/* Append frame data to the SCO reassembly buffer */
len = total;
if (got + len > want)
len = want - got;
usbd_copy_out(pc, frame_no * usbd_xfer_max_framelen(xfer),
mtod(m, uint8_t *) + m->m_pkthdr.len, len);
m->m_pkthdr.len += len;
m->m_len += len;
total -= len;
/* Check if we got everything we wanted, if not - continue */
if (got != want)
continue;
/* If we got here then we got complete SCO frame */
UBT_INFO(sc, "got complete SCO data frame, pktlen=%d, " \
"length=%d\n", m->m_pkthdr.len,
mtod(m, ng_hci_scodata_pkt_t *)->length);
UBT_STAT_PCKTS_RECV(sc);
UBT_STAT_BYTES_RECV(sc, m->m_pkthdr.len);
ubt_fwd_mbuf_up(sc, &m);
/* m == NULL at this point */
}
/* Put SCO reassembly buffer back */
sc->sc_isoc_in_buffer = m;
return (0);
} /* ubt_isoc_read_one_frame */
int
ng_l2cap_lp_send(ng_l2cap_con_p con, u_int16_t dcid, struct mbuf *m0)
{
ng_l2cap_p l2cap = con->l2cap;
ng_l2cap_hdr_t *l2cap_hdr = NULL;
ng_hci_acldata_pkt_t *acl_hdr = NULL;
struct mbuf *m_last = NULL, *m = NULL;
int len, flag = NG_HCI_PACKET_START;
KASSERT((con->tx_pkt == NULL),
("%s: %s - another packet pending?!\n", __func__, NG_NODE_NAME(l2cap->node)));
KASSERT((l2cap->pkt_size > 0),
("%s: %s - invalid l2cap->pkt_size?!\n", __func__, NG_NODE_NAME(l2cap->node)));
/* Prepend mbuf with L2CAP header */
m0 = ng_l2cap_prepend(m0, sizeof(*l2cap_hdr));
if (m0 == NULL) {
NG_L2CAP_ALERT(
"%s: %s - ng_l2cap_prepend(%zd) failed\n",
__func__, NG_NODE_NAME(l2cap->node),
sizeof(*l2cap_hdr));
goto fail;
}
l2cap_hdr = mtod(m0, ng_l2cap_hdr_t *);
l2cap_hdr->length = htole16(m0->m_pkthdr.len - sizeof(*l2cap_hdr));
l2cap_hdr->dcid = htole16(dcid);
/*
* Segment single L2CAP packet according to the HCI layer MTU. Convert
* each segment into ACL data packet and prepend it with ACL data packet
* header. Link all segments together via m_nextpkt link.
*
* XXX BC (Broadcast flag) will always be 0 (zero).
*/
while (m0 != NULL) {
/* Check length of the packet against HCI MTU */
len = m0->m_pkthdr.len;
if (len > l2cap->pkt_size) {
m = m_split(m0, l2cap->pkt_size, M_DONTWAIT);
if (m == NULL) {
NG_L2CAP_ALERT(
"%s: %s - m_split(%d) failed\n", __func__, NG_NODE_NAME(l2cap->node),
l2cap->pkt_size);
goto fail;
}
len = l2cap->pkt_size;
}
/* Convert packet fragment into ACL data packet */
m0 = ng_l2cap_prepend(m0, sizeof(*acl_hdr));
if (m0 == NULL) {
NG_L2CAP_ALERT(
"%s: %s - ng_l2cap_prepend(%zd) failed\n",
__func__, NG_NODE_NAME(l2cap->node),
sizeof(*acl_hdr));
goto fail;
}
acl_hdr = mtod(m0, ng_hci_acldata_pkt_t *);
acl_hdr->type = NG_HCI_ACL_DATA_PKT;
acl_hdr->length = htole16(len);
acl_hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE(
con->con_handle, flag, 0));
/* Add fragment to the chain */
m0->m_nextpkt = NULL;
if (con->tx_pkt == NULL)
con->tx_pkt = m_last = m0;
else {
m_last->m_nextpkt = m0;
m_last = m0;
}
NG_L2CAP_INFO(
"%s: %s - attaching ACL packet, con_handle=%d, PB=%#x, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con->con_handle,
flag, len);
m0 = m;
m = NULL;
flag = NG_HCI_PACKET_FRAGMENT;
}
return (0);
fail:
NG_FREE_M(m0);
NG_FREE_M(m);
while (con->tx_pkt != NULL) {
m = con->tx_pkt->m_nextpkt;
m_freem(con->tx_pkt);
con->tx_pkt = m;
}
return (ENOBUFS);
} /* ng_l2cap_lp_send */
int
ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m)
{
ng_hci_acldata_pkt_t *acl_hdr = NULL;
ng_l2cap_hdr_t *l2cap_hdr = NULL;
ng_l2cap_con_p con = NULL;
u_int16_t con_handle, length, pb;
int error = 0;
/* Check ACL data packet */
if (m->m_pkthdr.len < sizeof(*acl_hdr)) {
NG_L2CAP_ERR(
"%s: %s - invalid ACL data packet. Packet too small, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len);
error = EMSGSIZE;
goto drop;
}
/* Strip ACL data packet header */
NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr));
if (m == NULL)
return (ENOBUFS);
acl_hdr = mtod(m, ng_hci_acldata_pkt_t *);
m_adj(m, sizeof(*acl_hdr));
/* Get ACL connection handle, PB flag and payload length */
acl_hdr->con_handle = le16toh(acl_hdr->con_handle);
con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle);
pb = NG_HCI_PB_FLAG(acl_hdr->con_handle);
length = le16toh(acl_hdr->length);
NG_L2CAP_INFO(
"%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length);
/* Get connection descriptor */
con = ng_l2cap_con_by_handle(l2cap, con_handle);
if (con == NULL) {
NG_L2CAP_ERR(
"%s: %s - unexpected ACL data packet. " \
"Connection does not exist, con_handle=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con_handle);
error = ENOENT;
goto drop;
}
/* Verify connection state */
if (con->state != NG_L2CAP_CON_OPEN) {
NG_L2CAP_ERR(
"%s: %s - unexpected ACL data packet. Invalid connection state=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con->state);
error = EHOSTDOWN;
goto drop;
}
/* Process packet */
if (pb == NG_HCI_PACKET_START) {
if (con->rx_pkt != NULL) {
NG_L2CAP_ERR(
"%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %d bytes\n",
__func__, NG_NODE_NAME(l2cap->node),
con->rx_pkt->m_pkthdr.len, con->rx_pkt_len);
NG_FREE_M(con->rx_pkt);
con->rx_pkt_len = 0;
}
/* Get L2CAP header */
if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node),
m->m_pkthdr.len);
error = EMSGSIZE;
goto drop;
}
NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr));
if (m == NULL)
return (ENOBUFS);
l2cap_hdr = mtod(m, ng_l2cap_hdr_t *);
NG_L2CAP_INFO(
"%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con_handle,
le16toh(l2cap_hdr->length));
/* Start new L2CAP packet */
con->rx_pkt = m;
con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr);
} else if (pb == NG_HCI_PACKET_FRAGMENT) {
static void
ubt_bulk_write_callback(struct usb_xfer *xfer, usb_error_t error)
{
struct ubt_softc *sc = usbd_xfer_softc(xfer);
struct mbuf *m;
struct usb_page_cache *pc;
int actlen;
usbd_xfer_status(xfer, &actlen, NULL, NULL, NULL);
switch (USB_GET_STATE(xfer)) {
case USB_ST_TRANSFERRED:
UBT_INFO(sc, "sent %d bytes to bulk-out pipe\n", actlen);
UBT_STAT_BYTES_SENT(sc, actlen);
UBT_STAT_PCKTS_SENT(sc);
/* FALLTHROUGH */
case USB_ST_SETUP:
send_next:
/* Get next mbuf, if any */
UBT_NG_LOCK(sc);
NG_BT_MBUFQ_DEQUEUE(&sc->sc_aclq, m);
UBT_NG_UNLOCK(sc);
if (m == NULL) {
UBT_INFO(sc, "ACL data queue is empty\n");
break; /* transfer completed */
}
/*
* Copy ACL data frame back to a linear USB transfer buffer
* and schedule transfer
*/
pc = usbd_xfer_get_frame(xfer, 0);
usbd_m_copy_in(pc, 0, m, 0, m->m_pkthdr.len);
usbd_xfer_set_frame_len(xfer, 0, m->m_pkthdr.len);
UBT_INFO(sc, "bulk-out transfer has been started, len=%d\n",
m->m_pkthdr.len);
NG_FREE_M(m);
usbd_transfer_submit(xfer);
break;
default: /* Error */
if (error != USB_ERR_CANCELLED) {
UBT_WARN(sc, "bulk-out transfer failed: %s\n",
usbd_errstr(error));
UBT_STAT_OERROR(sc);
/* try to clear stall first */
usbd_xfer_set_stall(xfer);
goto send_next;
}
/* transfer cancelled */
break;
}
} /* ubt_bulk_write_callback */
int
ng_l2cap_l2ca_receive(ng_l2cap_con_p con)
{
ng_l2cap_p l2cap = con->l2cap;
ng_l2cap_hdr_t *hdr = NULL;
ng_l2cap_chan_p ch = NULL;
int error = 0;
int idtype;
uint16_t *idp;
int silent = 0;
NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
if (con->rx_pkt == NULL)
return (ENOBUFS);
hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *);
/* Check channel */
if(hdr->dcid == NG_L2CAP_ATT_CID){
idtype = NG_L2CAP_L2CA_IDTYPE_ATT;
ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_ATT_CID,
con->con_handle);
/*
* Here,ATT channel is distinguished by
* connection handle
*/
hdr->dcid = con->con_handle;
silent = 1;
}else if(hdr->dcid == NG_L2CAP_SMP_CID){
idtype = NG_L2CAP_L2CA_IDTYPE_SMP;
ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_SMP_CID,
con->con_handle);
/*
* Here,SMP channel is distinguished by
* connection handle
*/
silent = 1;
hdr->dcid = con->con_handle;
}else{
idtype = (con->linktype==NG_HCI_LINK_ACL)?
NG_L2CAP_L2CA_IDTYPE_BREDR:
NG_L2CAP_L2CA_IDTYPE_LE;
ch = ng_l2cap_chan_by_scid(l2cap, hdr->dcid, idtype);
}
if (ch == NULL) {
if(!silent)
NG_L2CAP_ERR(
"%s: %s - unexpected L2CAP data packet. Channel does not exist, cid=%d, idtype=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->dcid, idtype);
error = ENOENT;
goto drop;
}
/* Check channel state */
if (ch->state != NG_L2CAP_OPEN) {
NG_L2CAP_WARN(
"%s: %s - unexpected L2CAP data packet. " \
"Invalid channel state, cid=%d, state=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->scid,
ch->state);
error = EHOSTDOWN; /* XXX not always - re-configuration */
goto drop;
}
/* Check payload size and channel's MTU */
if (hdr->length > ch->imtu) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP data packet. " \
"Packet too big, length=%d, imtu=%d, cid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->length,
ch->imtu, ch->scid);
error = EMSGSIZE;
goto drop;
}
/*
* If we got here then everything looks good and we can sent packet
* to the upper layer protocol.
*/
/* Check if upstream hook is connected and valid */
if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
NG_L2CAP_ERR(
"%s: %s - unable to send L2CAP data packet. " \
"Hook is not connected or valid, psm=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->psm);
error = ENOTCONN;
goto drop;
}
M_PREPEND(con->rx_pkt, sizeof(uint16_t), M_NOWAIT);
if(con->rx_pkt == NULL)
goto drop;
idp = mtod(con->rx_pkt, uint16_t *);
*idp = idtype;
NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
con->rx_pkt = NULL;
drop:
NG_FREE_M(con->rx_pkt); /* checks for != NULL */
return (error);
} /* ng_l2cap_receive */
int
ng_l2cap_l2ca_receive(ng_l2cap_con_p con)
{
ng_l2cap_p l2cap = con->l2cap;
ng_l2cap_hdr_t *hdr = NULL;
ng_l2cap_chan_p ch = NULL;
int error = 0;
NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
if (con->rx_pkt == NULL)
return (ENOBUFS);
hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *);
/* Check channel */
ch = ng_l2cap_chan_by_scid(l2cap, hdr->dcid);
if (ch == NULL) {
NG_L2CAP_ERR(
"%s: %s - unexpected L2CAP data packet. Channel does not exist, cid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->dcid);
error = ENOENT;
goto drop;
}
/* Check channel state */
if (ch->state != NG_L2CAP_OPEN) {
NG_L2CAP_WARN(
"%s: %s - unexpected L2CAP data packet. " \
"Invalid channel state, cid=%d, state=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->scid,
ch->state);
error = EHOSTDOWN; /* XXX not always - re-configuration */
goto drop;
}
/* Check payload size and channel's MTU */
if (hdr->length > ch->imtu) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP data packet. " \
"Packet too big, length=%d, imtu=%d, cid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->length,
ch->imtu, ch->scid);
error = EMSGSIZE;
goto drop;
}
/*
* If we got here then everything looks good and we can sent packet
* to the upper layer protocol.
*/
/* Check if upstream hook is connected and valid */
if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
NG_L2CAP_ERR(
"%s: %s - unable to send L2CAP data packet. " \
"Hook is not connected or valid, psm=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->psm);
error = ENOTCONN;
goto drop;
}
NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
con->rx_pkt = NULL;
drop:
NG_FREE_M(con->rx_pkt); /* checks for != NULL */
return (error);
} /* ng_l2cap_receive */
int
ng_l2cap_l2ca_cfg_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
ng_l2cap_l2ca_cfg_ip *ip = NULL;
ng_l2cap_chan_p ch = NULL;
ng_l2cap_cmd_p cmd = NULL;
struct mbuf *opt = NULL;
u_int16_t *mtu = NULL, *flush_timo = NULL;
ng_l2cap_flow_p flow = NULL;
int error = 0;
/* Check message */
if (msg->header.arglen != sizeof(*ip)) {
NG_L2CAP_ALERT(
"%s: %s - Invalid L2CA_Config request message size, size=%d\n",
__func__, NG_NODE_NAME(l2cap->node),
msg->header.arglen);
error = EMSGSIZE;
goto out;
}
ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data);
/* Check if we have this channel */
ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
if (ch == NULL) {
NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_Config request message. " \
"Channel does not exist, lcid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
error = ENOENT;
goto out;
}
/* Check channel state */
if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) {
NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_Config request message. " \
"Invalid channel state, state=%d, lcid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->state,
ch->scid);
error = EINVAL;
goto out;
}
/* Set requested channel configuration options */
ch->imtu = ip->imtu;
bcopy(&ip->oflow, &ch->oflow, sizeof(ch->oflow));
ch->flush_timo = ip->flush_timo;
ch->link_timo = ip->link_timo;
/* Compare channel settings with defaults */
if (ch->imtu != NG_L2CAP_MTU_DEFAULT)
mtu = &ch->imtu;
if (ch->flush_timo != NG_L2CAP_FLUSH_TIMO_DEFAULT)
flush_timo = &ch->flush_timo;
if (bcmp(ng_l2cap_default_flow(), &ch->oflow, sizeof(ch->oflow)) != 0)
flow = &ch->oflow;
/* Create configuration options */
_ng_l2cap_build_cfg_options(opt, mtu, flush_timo, flow);
if (opt == NULL) {
error = ENOBUFS;
goto out;
}
/* Create L2CAP command descriptor */
cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con),
NG_L2CAP_CFG_REQ, msg->header.token);
if (cmd == NULL) {
NG_FREE_M(opt);
error = ENOMEM;
goto out;
}
if (cmd->ident == NG_L2CAP_NULL_IDENT) {
ng_l2cap_free_cmd(cmd);
NG_FREE_M(opt);
error = EIO;
goto out;
}
/* Create L2CAP command packet */
_ng_l2cap_cfg_req(cmd->aux, cmd->ident, ch->dcid, 0, opt);
if (cmd->aux == NULL) {
ng_l2cap_free_cmd(cmd);
error = ENOBUFS;
goto out;
}
/* Adjust channel state for re-configuration */
if (ch->state == NG_L2CAP_OPEN) {
ch->state = NG_L2CAP_CONFIG;
ch->cfg_state = 0;
}
/* Link command to the queue */
ng_l2cap_link_cmd(ch->con, cmd);
ng_l2cap_lp_deliver(ch->con);
out:
return (error);
} /* ng_l2cap_l2ca_cfg_req */
int
ng_hci_process_command_complete(ng_hci_unit_p unit, struct mbuf *e)
{
ng_hci_command_compl_ep *ep = NULL;
struct mbuf *cp = NULL;
int error = 0;
/* Get event packet and update command buffer info */
NG_HCI_M_PULLUP(e, sizeof(*ep));
if (e == NULL)
return (ENOBUFS); /* XXX this is bad */
ep = mtod(e, ng_hci_command_compl_ep *);
NG_HCI_BUFF_CMD_SET(unit->buffer, ep->num_cmd_pkts);
/* Check for special NOOP command */
if (ep->opcode == 0x0000) {
NG_FREE_M(e);
goto out;
}
/* Try to match first command item in the queue */
error = complete_command(unit, ep->opcode, &cp);
if (error != 0) {
NG_FREE_M(e);
goto out;
}
/*
* Perform post processing on command parameters and return parameters
* do it only if status is OK (status == 0). Status is the first byte
* of any command return parameters.
*/
ep->opcode = le16toh(ep->opcode);
m_adj(e, sizeof(*ep));
if (*mtod(e, u_int8_t *) == 0) { /* XXX m_pullup here? */
switch (NG_HCI_OGF(ep->opcode)) {
case NG_HCI_OGF_LINK_CONTROL:
error = process_link_control_params(unit,
NG_HCI_OCF(ep->opcode), cp, e);
break;
case NG_HCI_OGF_LINK_POLICY:
error = process_link_policy_params(unit,
NG_HCI_OCF(ep->opcode), cp, e);
break;
case NG_HCI_OGF_HC_BASEBAND:
error = process_hc_baseband_params(unit,
NG_HCI_OCF(ep->opcode), cp, e);
break;
case NG_HCI_OGF_INFO:
error = process_info_params(unit,
NG_HCI_OCF(ep->opcode), cp, e);
break;
case NG_HCI_OGF_STATUS:
error = process_status_params(unit,
NG_HCI_OCF(ep->opcode), cp, e);
break;
case NG_HCI_OGF_TESTING:
error = process_testing_params(unit,
NG_HCI_OCF(ep->opcode), cp, e);
break;
case NG_HCI_OGF_BT_LOGO:
case NG_HCI_OGF_VENDOR:
NG_FREE_M(cp);
NG_FREE_M(e);
break;
default:
NG_FREE_M(cp);
NG_FREE_M(e);
error = EINVAL;
break;
}
} else {
int
ng_l2cap_l2ca_write_req(ng_l2cap_p l2cap, struct mbuf *m)
{
ng_l2cap_l2ca_hdr_t *l2ca_hdr = NULL;
ng_l2cap_chan_p ch = NULL;
ng_l2cap_cmd_p cmd = NULL;
int error = 0;
u_int32_t token = 0;
/* Make sure we can access L2CA data packet header */
if (m->m_pkthdr.len < sizeof(*l2ca_hdr)) {
NG_L2CAP_ERR(
"%s: %s - L2CA Data packet too small, len=%d\n",
__func__,NG_NODE_NAME(l2cap->node),m->m_pkthdr.len);
error = EMSGSIZE;
goto drop;
}
/* Get L2CA data packet header */
NG_L2CAP_M_PULLUP(m, sizeof(*l2ca_hdr));
if (m == NULL)
return (ENOBUFS);
l2ca_hdr = mtod(m, ng_l2cap_l2ca_hdr_t *);
token = l2ca_hdr->token;
m_adj(m, sizeof(*l2ca_hdr));
/* Verify payload size */
if (l2ca_hdr->length != m->m_pkthdr.len) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CA Data packet. " \
"Payload length does not match, length=%d, len=%d\n",
__func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->length,
m->m_pkthdr.len);
error = EMSGSIZE;
goto drop;
}
/* Check channel ID */
if (l2ca_hdr->lcid < NG_L2CAP_FIRST_CID) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CA Data packet. Inavlid channel ID, cid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid);
error = EINVAL;
goto drop;
}
/* Verify that we have the channel and make sure it is open */
ch = ng_l2cap_chan_by_scid(l2cap, l2ca_hdr->lcid);
if (ch == NULL) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CA Data packet. Channel does not exist, cid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid);
error = ENOENT;
goto drop;
}
if (ch->state != NG_L2CAP_OPEN) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CA Data packet. Invalid channel state, scid=%d, state=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->scid,
ch->state);
error = EHOSTDOWN;
goto drop; /* XXX not always - re-configure */
}
/* Create L2CAP command descriptor */
cmd = ng_l2cap_new_cmd(ch->con, ch, 0, NGM_L2CAP_L2CA_WRITE, token);
if (cmd == NULL) {
error = ENOMEM;
goto drop;
}
/* Attach data packet and link command to the queue */
cmd->aux = m;
ng_l2cap_link_cmd(ch->con, cmd);
ng_l2cap_lp_deliver(ch->con);
return (error);
drop:
NG_FREE_M(m);
return (error);
} /* ng_l2cap_l2ca_write_req */
/*
* receive data, and use it to update our status.
* Anything coming in on the debug port is discarded.
*/
static int
nglmi_rcvdata(hook_p hook, item_p item)
{
sc_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
const u_char *data;
unsigned short dlci;
u_short packetlen;
int resptype_seen = 0;
struct mbuf *m;
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
if (NG_HOOK_PRIVATE(hook) == NULL) {
goto drop;
}
packetlen = m->m_len;
/* XXX what if it's more than 1 mbuf? */
if ((packetlen > MHLEN) && !(m->m_flags & M_EXT)) {
log(LOG_WARNING, "nglmi: packetlen (%d) too big\n", packetlen);
goto drop;
}
if (m->m_len < packetlen && (m = m_pullup(m, packetlen)) == NULL) {
log(LOG_WARNING,
"nglmi: m_pullup failed for %d bytes\n", packetlen);
return (0);
}
if (nglmi_checkdata(hook, m) == 0)
return (0);
/* pass the first 4 bytes (already checked in the nglmi_checkdata()) */
data = mtod(m, const u_char *);
STEPBY(4);
/* Now check if there is a 'locking shift'. This is only seen in
* Annex D frames. don't bother checking, we already did that. Don't
* increment immediatly as it might not be there. */
if (ANNEXD(sc))
STEPBY(1);
/* If we get this far we should consider that it is a legitimate
* frame and we know what it is. */
if (sc->flags & SCF_AUTO) {
/* note the hook that this valid channel came from and drop
* out of auto probe mode. */
if (ANNEXA(sc))
sc->protoname = NAME_ANNEXA;
else if (ANNEXD(sc))
sc->protoname = NAME_ANNEXD;
else if (GROUP4(sc))
sc->protoname = NAME_GROUP4;
else {
log(LOG_ERR, "nglmi: No known type\n");
goto drop;
}
sc->lmi_channel = hook;
sc->flags &= ~SCF_AUTO;
log(LOG_INFO, "nglmi: auto-detected %s LMI on DLCI %d\n",
sc->protoname, hook == sc->lmi_channel0 ? 0 : 1023);
}
/* While there is more data in the status packet, keep processing
* status items. First make sure there is enough data for the
* segment descriptor's length field. */
while (packetlen >= 2) {
u_int segtype = data[0];
u_int segsize = data[1];
/* Now that we know how long it claims to be, make sure
* there is enough data for the next seg. */
if (packetlen < segsize + 2)
break;
switch (segtype) {
case 0x01:
case 0x51:
if (resptype_seen) {
log(LOG_WARNING, "nglmi: dup MSGTYPE\n");
goto nextIE;
}
resptype_seen++;
/* The remote end tells us what kind of response
* this is. Only expect a type 0 or 1. if we are a
* full status, invalidate a few DLCIs just to see
* that they are still ok. */
if (segsize != 1)
goto nextIE;
switch (data[2]) {
case 1:
/* partial status, do no extra processing */
break;
case 0:
{
int count = 0;
int idx = sc->invalidx;
for (count = 0; count < 10; count++) {
if (idx > MAXDLCI)
idx = 0;
if (sc->dlci_state[idx] == DLCI_UP)
sc->dlci_state[idx] = DLCI_DOWN;
idx++;
}
sc->invalidx = idx;
/* we got and we wanted one. relax
* now.. but don't reset to 0 if it
* was unrequested. */
if (sc->livs > sc->liv_per_full)
sc->livs = 0;
break;
}
}
break;
case 0x03:
case 0x53:
/* The remote tells us what it thinks the sequence
* numbers are. If it's not size 2, it must be a
* duplicate to have gotten this far, skip it. */
if (segsize != 2)
goto nextIE;
sc->remote_seq = data[2];
if (sc->local_seq == data[3]) {
sc->local_seq++;
sc->seq_retries = 0;
/* Note that all 3 Frame protocols seem to
* not like 0 as a sequence number. */
if (sc->local_seq == 0)
sc->local_seq = 1;
}
break;
case 0x07:
case 0x57:
/* The remote tells us about a DLCI that it knows
* about. There may be many of these in a single
* status response */
switch (segsize) {
case 6:/* only on 'group of 4' */
dlci = ((u_short) data[2] & 0xff) << 8;
dlci |= (data[3] & 0xff);
if ((dlci < 1024) && (dlci > 0)) {
/* XXX */
}
break;
case 3:
dlci = ((u_short) data[2] & 0x3f) << 4;
dlci |= ((data[3] & 0x78) >> 3);
if ((dlci < 1024) && (dlci > 0)) {
/* set up the bottom half of the
* support for that dlci if it's not
* already been done */
/* store this information somewhere */
}
break;
default:
goto nextIE;
}
if (sc->dlci_state[dlci] != DLCI_UP) {
/* bring new DLCI to life */
/* may do more here some day */
if (sc->dlci_state[dlci] != DLCI_DOWN)
log(LOG_INFO,
"nglmi: DLCI %d became active\n",
dlci);
sc->dlci_state[dlci] = DLCI_UP;
}
break;
}
nextIE:
STEPBY(segsize + 2);
}
NG_FREE_M(m);
return (0);
drop:
NG_FREE_M(m);
return (EINVAL);
}
int
ng_l2cap_l2ca_clt_receive(ng_l2cap_con_p con)
{
struct _clt_pkt {
ng_l2cap_hdr_t h;
ng_l2cap_clt_hdr_t c_h;
} __attribute__ ((packed)) *hdr = NULL;
ng_l2cap_p l2cap = con->l2cap;
int length, error = 0;
NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
if (con->rx_pkt == NULL)
return (ENOBUFS);
hdr = mtod(con->rx_pkt, struct _clt_pkt *);
/* Check packet */
length = con->rx_pkt->m_pkthdr.len - sizeof(*hdr);
if (length < 0) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP CLT data packet. Packet too small, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), length);
error = EMSGSIZE;
goto drop;
}
/* Check payload size against CLT MTU */
if (length > NG_L2CAP_MTU_DEFAULT) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP CLT data packet. Packet too big, length=%d, mtu=%d\n",
__func__, NG_NODE_NAME(l2cap->node), length,
NG_L2CAP_MTU_DEFAULT);
error = EMSGSIZE;
goto drop;
}
hdr->c_h.psm = le16toh(hdr->c_h.psm);
/*
* If we got here then everything looks good and we can sent packet
* to the upper layer protocol.
*/
/* Select upstream hook based on PSM */
switch (hdr->c_h.psm) {
case NG_L2CAP_PSM_SDP:
if (l2cap->flags & NG_L2CAP_CLT_SDP_DISABLED)
goto drop;
break;
case NG_L2CAP_PSM_RFCOMM:
if (l2cap->flags & NG_L2CAP_CLT_RFCOMM_DISABLED)
goto drop;
break;
case NG_L2CAP_PSM_TCP:
if (l2cap->flags & NG_L2CAP_CLT_TCP_DISABLED)
goto drop;
break;
}
/* Check if upstream hook is connected and valid */
if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
NG_L2CAP_ERR(
"%s: %s - unable to send L2CAP CLT data packet. " \
"Hook is not connected or valid, psm=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->c_h.psm);
error = ENOTCONN;
goto drop;
}
NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
con->rx_pkt = NULL;
drop:
NG_FREE_M(con->rx_pkt); /* checks for != NULL */
return (error);
} /* ng_l2cap_l2ca_clt_receive */
static void
bt3c_receive(bt3c_softc_p sc)
{
u_int16_t i, count, c;
/* Receive data from the card */
bt3c_read(sc, 0x7006, count);
NG_BT3C_INFO(sc->dev, "The card has %d characters\n", count);
bt3c_set_address(sc, 0x7480);
for (i = 0; i < count; i++) {
/* Allocate new mbuf if needed */
if (sc->m == NULL) {
sc->state = NG_BT3C_W4_PKT_IND;
sc->want = 1;
MGETHDR(sc->m, M_NOWAIT, MT_DATA);
if (sc->m == NULL) {
NG_BT3C_ERR(sc->dev, "Could not get mbuf\n");
NG_BT3C_STAT_IERROR(sc->stat);
break; /* XXX lost of sync */
}
if (!(MCLGET(sc->m, M_NOWAIT))) {
NG_FREE_M(sc->m);
NG_BT3C_ERR(sc->dev, "Could not get cluster\n");
NG_BT3C_STAT_IERROR(sc->stat);
break; /* XXX lost of sync */
}
sc->m->m_len = sc->m->m_pkthdr.len = 0;
}
/* Read and append character to mbuf */
bt3c_read_data(sc, c);
if (sc->m->m_pkthdr.len >= MCLBYTES) {
NG_BT3C_ERR(sc->dev, "Oversized frame\n");
NG_FREE_M(sc->m);
sc->state = NG_BT3C_W4_PKT_IND;
sc->want = 1;
break; /* XXX lost of sync */
}
mtod(sc->m, u_int8_t *)[sc->m->m_len ++] = (u_int8_t) c;
sc->m->m_pkthdr.len ++;
NG_BT3C_INFO(sc->dev,
"Got char %#x, want=%d, got=%d\n", c, sc->want, sc->m->m_pkthdr.len);
if (sc->m->m_pkthdr.len < sc->want)
continue; /* wait for more */
switch (sc->state) {
/* Got packet indicator */
case NG_BT3C_W4_PKT_IND:
NG_BT3C_INFO(sc->dev,
"Got packet indicator %#x\n", *mtod(sc->m, u_int8_t *));
sc->state = NG_BT3C_W4_PKT_HDR;
/*
* Since packet indicator included in the packet
* header just set sc->want to sizeof(packet header).
*/
switch (*mtod(sc->m, u_int8_t *)) {
case NG_HCI_ACL_DATA_PKT:
sc->want = sizeof(ng_hci_acldata_pkt_t);
break;
case NG_HCI_SCO_DATA_PKT:
sc->want = sizeof(ng_hci_scodata_pkt_t);
break;
case NG_HCI_EVENT_PKT:
sc->want = sizeof(ng_hci_event_pkt_t);
break;
default:
NG_BT3C_ERR(sc->dev,
"Ignoring unknown packet type=%#x\n", *mtod(sc->m, u_int8_t *));
NG_BT3C_STAT_IERROR(sc->stat);
NG_FREE_M(sc->m);
sc->state = NG_BT3C_W4_PKT_IND;
sc->want = 1;
break;
}
break;
/* Got packet header */
case NG_BT3C_W4_PKT_HDR:
sc->state = NG_BT3C_W4_PKT_DATA;
switch (*mtod(sc->m, u_int8_t *)) {
case NG_HCI_ACL_DATA_PKT:
c = le16toh(mtod(sc->m,
ng_hci_acldata_pkt_t *)->length);
break;
case NG_HCI_SCO_DATA_PKT:
c = mtod(sc->m, ng_hci_scodata_pkt_t*)->length;
break;
case NG_HCI_EVENT_PKT:
c = mtod(sc->m, ng_hci_event_pkt_t *)->length;
break;
default:
KASSERT(0,
("Invalid packet type=%#x\n", *mtod(sc->m, u_int8_t *)));
break;
}
NG_BT3C_INFO(sc->dev,
"Got packet header, packet type=%#x, got so far %d, payload size=%d\n",
*mtod(sc->m, u_int8_t *), sc->m->m_pkthdr.len,
c);
if (c > 0) {
sc->want += c;
break;
}
/* else FALLTHROUGH and deliver frame */
/* XXX is this true? should we deliver empty frame? */
/* Got packet data */
case NG_BT3C_W4_PKT_DATA:
NG_BT3C_INFO(sc->dev,
"Got full packet, packet type=%#x, packet size=%d\n",
*mtod(sc->m, u_int8_t *), sc->m->m_pkthdr.len);
NG_BT3C_STAT_BYTES_RECV(sc->stat, sc->m->m_pkthdr.len);
NG_BT3C_STAT_PCKTS_RECV(sc->stat);
IF_LOCK(&sc->inq);
if (_IF_QFULL(&sc->inq)) {
NG_BT3C_ERR(sc->dev,
"Incoming queue is full. Dropping mbuf, len=%d\n", sc->m->m_pkthdr.len);
NG_BT3C_STAT_IERROR(sc->stat);
NG_FREE_M(sc->m);
} else {
_IF_ENQUEUE(&sc->inq, sc->m);
sc->m = NULL;
}
IF_UNLOCK(&sc->inq);
sc->state = NG_BT3C_W4_PKT_IND;
sc->want = 1;
break;
default:
KASSERT(0,
("Invalid node state=%d", sc->state));
break;
}
}
bt3c_write(sc, 0x7006, 0x0000);
} /* bt3c_receive */
