NetBSD/usr.sbin/btpand/bnep.c

749 lines
16 KiB
C

/* $NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $ */
/*-
* Copyright (c) 2008 Iain Hibbert
* 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.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 THE AUTHOR 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>
__RCSID("$NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $");
#include <bluetooth.h>
#include <sdp.h>
#include <stdarg.h>
#include <string.h>
#include "btpand.h"
#include "bnep.h"
static bool bnep_recv_extension(packet_t *);
static size_t bnep_recv_control(channel_t *, uint8_t *, size_t, bool);
static size_t bnep_recv_control_command_not_understood(channel_t *, uint8_t *, size_t);
static size_t bnep_recv_setup_connection_req(channel_t *, uint8_t *, size_t);
static size_t bnep_recv_setup_connection_rsp(channel_t *, uint8_t *, size_t);
static size_t bnep_recv_filter_net_type_set(channel_t *, uint8_t *, size_t);
static size_t bnep_recv_filter_net_type_rsp(channel_t *, uint8_t *, size_t);
static size_t bnep_recv_filter_multi_addr_set(channel_t *, uint8_t *, size_t);
static size_t bnep_recv_filter_multi_addr_rsp(channel_t *, uint8_t *, size_t);
static bool bnep_pfilter(channel_t *, packet_t *);
static bool bnep_mfilter(channel_t *, packet_t *);
static uint8_t NAP_UUID[] = {
0x00, 0x00, 0x11, 0x16,
0x00, 0x00,
0x10, 0x00,
0x80, 0x00,
0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb
};
static uint8_t GN_UUID[] = {
0x00, 0x00, 0x11, 0x17,
0x00, 0x00,
0x10, 0x00,
0x80, 0x00,
0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,
};
static uint8_t PANU_UUID[] = {
0x00, 0x00, 0x11, 0x15,
0x00, 0x00,
0x10, 0x00,
0x80, 0x00,
0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb
};
/*
* receive BNEP packet
* return true if packet is to be forwarded
*/
bool
bnep_recv(packet_t *pkt)
{
size_t len;
uint8_t type;
if (pkt->len < 1)
return false;
type = pkt->ptr[0];
packet_adj(pkt, 1);
switch (BNEP_TYPE(type)) {
case BNEP_GENERAL_ETHERNET:
if (pkt->len < (ETHER_ADDR_LEN * 2) + ETHER_TYPE_LEN) {
log_debug("dropped short packet (type 0x%2.2x)", type);
return false;
}
pkt->dst = pkt->ptr;
packet_adj(pkt, ETHER_ADDR_LEN);
pkt->src = pkt->ptr;
packet_adj(pkt, ETHER_ADDR_LEN);
pkt->type = pkt->ptr;
packet_adj(pkt, ETHER_TYPE_LEN);
break;
case BNEP_CONTROL:
len = bnep_recv_control(pkt->chan, pkt->ptr, pkt->len, false);
if (len == 0)
return false;
packet_adj(pkt, len);
break;
case BNEP_COMPRESSED_ETHERNET:
if (pkt->len < ETHER_TYPE_LEN) {
log_debug("dropped short packet (type 0x%2.2x)", type);
return false;
}
pkt->dst = pkt->chan->laddr;
pkt->src = pkt->chan->raddr;
pkt->type = pkt->ptr;
packet_adj(pkt, ETHER_TYPE_LEN);
break;
case BNEP_COMPRESSED_ETHERNET_SRC_ONLY:
if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) {
log_debug("dropped short packet (type 0x%2.2x)", type);
return false;
}
pkt->dst = pkt->chan->laddr;
pkt->src = pkt->ptr;
packet_adj(pkt, ETHER_ADDR_LEN);
pkt->type = pkt->ptr;
packet_adj(pkt, ETHER_TYPE_LEN);
break;
case BNEP_COMPRESSED_ETHERNET_DST_ONLY:
if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) {
log_debug("dropped short packet (type 0x%2.2x)", type);
return false;
}
pkt->dst = pkt->ptr;
packet_adj(pkt, ETHER_ADDR_LEN);
pkt->src = pkt->chan->raddr;
pkt->type = pkt->ptr;
packet_adj(pkt, ETHER_TYPE_LEN);
break;
default:
/*
* Any packet containing a reserved BNEP
* header packet type SHALL be dropped.
*/
log_debug("dropped packet with reserved type 0x%2.2x", type);
return false;
}
if (BNEP_TYPE_EXT(type)
&& !bnep_recv_extension(pkt))
return false; /* invalid extensions */
if (BNEP_TYPE(type) == BNEP_CONTROL
|| pkt->chan->state != CHANNEL_OPEN)
return false; /* no forwarding */
return true;
}
static bool
bnep_recv_extension(packet_t *pkt)
{
exthdr_t *eh;
size_t len;
uint8_t type, size;
do {
if (pkt->len < 2)
return false;
type = pkt->ptr[0];
size = pkt->ptr[1];
if (pkt->len < size + 2)
return false;
switch (type) {
case BNEP_EXTENSION_CONTROL:
len = bnep_recv_control(pkt->chan, pkt->ptr + 2, size, true);
if (len != size)
log_err("ignored spurious data in exthdr");
break;
default:
/* Unknown extension headers in data packets */
/* SHALL be forwarded irrespective of any */
/* network protocol or multicast filter settings */
/* and any local filtering policy. */
eh = malloc(sizeof(exthdr_t));
if (eh == NULL) {
log_err("exthdr malloc() failed: %m");
break;
}
eh->ptr = pkt->ptr;
eh->len = size;
STAILQ_INSERT_TAIL(&pkt->extlist, eh, next);
break;
}
packet_adj(pkt, size + 2);
} while (BNEP_TYPE_EXT(type));
return true;
}
static size_t
bnep_recv_control(channel_t *chan, uint8_t *ptr, size_t size, bool isext)
{
uint8_t type;
size_t len;
if (size-- < 1)
return 0;
type = *ptr++;
switch (type) {
case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
len = bnep_recv_control_command_not_understood(chan, ptr, size);
break;
case BNEP_SETUP_CONNECTION_REQUEST:
if (isext)
return 0; /* not allowed in extension headers */
len = bnep_recv_setup_connection_req(chan, ptr, size);
break;
case BNEP_SETUP_CONNECTION_RESPONSE:
if (isext)
return 0; /* not allowed in extension headers */
len = bnep_recv_setup_connection_rsp(chan, ptr, size);
break;
case BNEP_FILTER_NET_TYPE_SET:
len = bnep_recv_filter_net_type_set(chan, ptr, size);
break;
case BNEP_FILTER_NET_TYPE_RESPONSE:
len = bnep_recv_filter_net_type_rsp(chan, ptr, size);
break;
case BNEP_FILTER_MULTI_ADDR_SET:
len = bnep_recv_filter_multi_addr_set(chan, ptr, size);
break;
case BNEP_FILTER_MULTI_ADDR_RESPONSE:
len = bnep_recv_filter_multi_addr_rsp(chan, ptr, size);
break;
default:
len = 0;
break;
}
if (len == 0)
bnep_send_control(chan, BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD, type);
return len;
}
static size_t
bnep_recv_control_command_not_understood(channel_t *chan, uint8_t *ptr, size_t size)
{
uint8_t type;
if (size < 1)
return 0;
type = *ptr++;
log_err("received Control Command Not Understood (0x%2.2x)", type);
/* we didn't send any reserved commands, just cut them off */
channel_close(chan);
return 1;
}
static size_t
bnep_recv_setup_connection_req(channel_t *chan, uint8_t *ptr, size_t size)
{
uint8_t len, off;
int src, dst, rsp;
if (size < 1)
return 0;
len = *ptr++;
if (size < (len * 2 + 1))
return 0;
if (chan->state != CHANNEL_WAIT_CONNECT_REQ
&& chan->state != CHANNEL_OPEN) {
log_debug("ignored");
return (len * 2 + 1);
}
if (len == 2)
off = 2;
else if (len == 4)
off = 0;
else if (len == 16)
off = 0;
else {
rsp = BNEP_SETUP_INVALID_UUID_SIZE;
goto done;
}
if (memcmp(ptr, NAP_UUID + off, len) == 0)
dst = SDP_SERVICE_CLASS_NAP;
else if (memcmp(ptr, GN_UUID + off, len) == 0)
dst = SDP_SERVICE_CLASS_GN;
else if (memcmp(ptr, PANU_UUID + off, len) == 0)
dst = SDP_SERVICE_CLASS_PANU;
else
dst = 0;
if (dst != service_class) {
rsp = BNEP_SETUP_INVALID_DST_UUID;
goto done;
}
ptr += len;
if (memcmp(ptr, PANU_UUID + off, len) == 0)
src = SDP_SERVICE_CLASS_NAP;
else if (memcmp(ptr, GN_UUID + off, len) == 0)
src = SDP_SERVICE_CLASS_GN;
else if (memcmp(ptr, NAP_UUID + off, len) == 0)
src = SDP_SERVICE_CLASS_PANU;
else
src = 0;
if ((dst != SDP_SERVICE_CLASS_PANU && src != SDP_SERVICE_CLASS_PANU)
|| src == 0) {
rsp = BNEP_SETUP_INVALID_SRC_UUID;
goto done;
}
rsp = BNEP_SETUP_SUCCESS;
chan->state = CHANNEL_OPEN;
channel_timeout(chan, 0);
done:
log_debug("addr %s response 0x%2.2x",
ether_ntoa((struct ether_addr *)chan->raddr), rsp);
bnep_send_control(chan, BNEP_SETUP_CONNECTION_RESPONSE, rsp);
return (len * 2 + 1);
}
static size_t
bnep_recv_setup_connection_rsp(channel_t *chan, uint8_t *ptr, size_t size)
{
int rsp;
if (size < 2)
return 0;
rsp = be16dec(ptr);
if (chan->state != CHANNEL_WAIT_CONNECT_RSP) {
log_debug("ignored");
return 2;
}
log_debug("addr %s response 0x%2.2x",
ether_ntoa((struct ether_addr *)chan->raddr), rsp);
if (rsp == BNEP_SETUP_SUCCESS) {
chan->state = CHANNEL_OPEN;
channel_timeout(chan, 0);
} else {
channel_close(chan);
}
return 2;
}
static size_t
bnep_recv_filter_net_type_set(channel_t *chan, uint8_t *ptr, size_t size)
{
pfilter_t *pf;
int i, len, nf, rsp;
if (size < 2)
return 0;
len = be16dec(ptr);
ptr += 2;
if (size < (len + 2))
return 0;
if (chan->state != CHANNEL_OPEN) {
log_debug("ignored");
return (len + 2);
}
nf = len / 4;
pf = malloc(nf * sizeof(pfilter_t));
if (pf == NULL) {
rsp = BNEP_FILTER_TOO_MANY_FILTERS;
goto done;
}
log_debug("nf = %d", nf);
for (i = 0; i < nf; i++) {
pf[i].start = be16dec(ptr);
ptr += 2;
pf[i].end = be16dec(ptr);
ptr += 2;
if (pf[i].start > pf[i].end) {
free(pf);
rsp = BNEP_FILTER_INVALID_RANGE;
goto done;
}
log_debug("pf[%d] = %#4.4x, %#4.4x", i, pf[i].start, pf[i].end);
}
if (chan->pfilter)
free(chan->pfilter);
chan->pfilter = pf;
chan->npfilter = nf;
rsp = BNEP_FILTER_SUCCESS;
done:
log_debug("addr %s response 0x%2.2x",
ether_ntoa((struct ether_addr *)chan->raddr), rsp);
bnep_send_control(chan, BNEP_FILTER_NET_TYPE_RESPONSE, rsp);
return (len + 2);
}
static size_t
bnep_recv_filter_net_type_rsp(channel_t *chan, uint8_t *ptr, size_t size)
{
int rsp;
if (size < 2)
return 0;
if (chan->state != CHANNEL_OPEN) {
log_debug("ignored");
return 2;
}
rsp = be16dec(ptr);
log_debug("addr %s response 0x%2.2x",
ether_ntoa((struct ether_addr *)chan->raddr), rsp);
/* we did not send any filter_net_type_set message */
return 2;
}
static size_t
bnep_recv_filter_multi_addr_set(channel_t *chan, uint8_t *ptr, size_t size)
{
mfilter_t *mf;
int i, len, nf, rsp;
if (size < 2)
return 0;
len = be16dec(ptr);
ptr += 2;
if (size < (len + 2))
return 0;
if (chan->state != CHANNEL_OPEN) {
log_debug("ignored");
return (len + 2);
}
nf = len / (ETHER_ADDR_LEN * 2);
mf = malloc(nf * sizeof(mfilter_t));
if (mf == NULL) {
rsp = BNEP_FILTER_TOO_MANY_FILTERS;
goto done;
}
log_debug("nf = %d", nf);
for (i = 0; i < nf; i++) {
memcpy(mf[i].start, ptr, ETHER_ADDR_LEN);
ptr += ETHER_ADDR_LEN;
memcpy(mf[i].end, ptr, ETHER_ADDR_LEN);
ptr += ETHER_ADDR_LEN;
if (memcmp(mf[i].start, mf[i].end, ETHER_ADDR_LEN) > 0) {
free(mf);
rsp = BNEP_FILTER_INVALID_RANGE;
goto done;
}
log_debug("pf[%d] = "
"%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x, "
"%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x", i,
mf[i].start[0], mf[i].start[1], mf[i].start[2],
mf[i].start[3], mf[i].start[4], mf[i].start[5],
mf[i].end[0], mf[i].end[1], mf[i].end[2],
mf[i].end[3], mf[i].end[4], mf[i].end[5]);
}
if (chan->mfilter)
free(chan->mfilter);
chan->mfilter = mf;
chan->nmfilter = nf;
rsp = BNEP_FILTER_SUCCESS;
done:
log_debug("addr %s response 0x%2.2x",
ether_ntoa((struct ether_addr *)chan->raddr), rsp);
bnep_send_control(chan, BNEP_FILTER_MULTI_ADDR_RESPONSE, rsp);
return (len + 2);
}
static size_t
bnep_recv_filter_multi_addr_rsp(channel_t *chan, uint8_t *ptr, size_t size)
{
int rsp;
if (size < 2)
return false;
if (chan->state != CHANNEL_OPEN) {
log_debug("ignored");
return 2;
}
rsp = be16dec(ptr);
log_debug("addr %s response 0x%2.2x",
ether_ntoa((struct ether_addr *)chan->raddr), rsp);
/* we did not send any filter_multi_addr_set message */
return 2;
}
void
bnep_send_control(channel_t *chan, uint8_t type, ...)
{
packet_t *pkt;
uint8_t *p;
va_list ap;
_DIAGASSERT(chan->state != CHANNEL_CLOSED);
pkt = packet_alloc(chan);
if (pkt == NULL)
return;
p = pkt->ptr;
va_start(ap, type);
*p++ = BNEP_CONTROL;
*p++ = type;
switch(type) {
case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
*p++ = va_arg(ap, int);
break;
case BNEP_SETUP_CONNECTION_REQUEST:
*p++ = va_arg(ap, int);
be16enc(p, va_arg(ap, int));
p += 2;
be16enc(p, va_arg(ap, int));
p += 2;
break;
case BNEP_SETUP_CONNECTION_RESPONSE:
case BNEP_FILTER_NET_TYPE_RESPONSE:
case BNEP_FILTER_MULTI_ADDR_RESPONSE:
be16enc(p, va_arg(ap, int));
p += 2;
break;
case BNEP_FILTER_NET_TYPE_SET: /* TODO */
case BNEP_FILTER_MULTI_ADDR_SET: /* TODO */
default:
log_err("Can't send control type 0x%2.2x", type);
break;
}
va_end(ap);
pkt->len = p - pkt->ptr;
channel_put(chan, pkt);
packet_free(pkt);
}
/*
* BNEP send packet routine
* return true if packet can be removed from queue
*/
bool
bnep_send(channel_t *chan, packet_t *pkt)
{
struct iovec iov[2];
uint8_t *p, *type, *proto;
exthdr_t *eh;
bool src, dst;
size_t nw;
if (pkt->type == NULL) {
iov[0].iov_base = pkt->ptr;
iov[0].iov_len = pkt->len;
iov[1].iov_base = NULL;
iov[1].iov_len = 0;
} else {
p = chan->sendbuf;
dst = (memcmp(pkt->dst, chan->raddr, ETHER_ADDR_LEN) != 0);
src = (memcmp(pkt->src, chan->laddr, ETHER_ADDR_LEN) != 0);
type = p;
p += 1;
if (dst && src)
*type = BNEP_GENERAL_ETHERNET;
else if (dst && !src)
*type = BNEP_COMPRESSED_ETHERNET_DST_ONLY;
else if (!dst && src)
*type = BNEP_COMPRESSED_ETHERNET_SRC_ONLY;
else /* (!dst && !src) */
*type = BNEP_COMPRESSED_ETHERNET;
if (dst) {
memcpy(p, pkt->dst, ETHER_ADDR_LEN);
p += ETHER_ADDR_LEN;
}
if (src) {
memcpy(p, pkt->src, ETHER_ADDR_LEN);
p += ETHER_ADDR_LEN;
}
proto = p;
memcpy(p, pkt->type, ETHER_TYPE_LEN);
p += ETHER_TYPE_LEN;
STAILQ_FOREACH(eh, &pkt->extlist, next) {
if (p + eh->len > chan->sendbuf + chan->mtu)
break;
*type |= BNEP_EXT;
type = p;
memcpy(p, eh->ptr, eh->len);
p += eh->len;
}
*type &= ~BNEP_EXT;
iov[0].iov_base = chan->sendbuf;
iov[0].iov_len = (p - chan->sendbuf);
if ((chan->npfilter == 0 || bnep_pfilter(chan, pkt))
&& (chan->nmfilter == 0 || bnep_mfilter(chan, pkt))) {
iov[1].iov_base = pkt->ptr;
iov[1].iov_len = pkt->len;
} else if (be16dec(proto) == ETHERTYPE_VLAN
&& pkt->len >= ETHER_VLAN_ENCAP_LEN) {
iov[1].iov_base = pkt->ptr;
iov[1].iov_len = ETHER_VLAN_ENCAP_LEN;
} else {
iov[1].iov_base = NULL;
iov[1].iov_len = 0;
memset(proto, 0, ETHER_TYPE_LEN);
}
}
if (iov[0].iov_len + iov[1].iov_len > chan->mtu) {
log_err("packet exceeded MTU (dropped)");
return false;
}
nw = writev(chan->fd, iov, __arraycount(iov));
return (nw > 0);
}
static bool
bnep_pfilter(channel_t *chan, packet_t *pkt)
{
int proto, i;
proto = be16dec(pkt->type);
if (proto == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */
if (pkt->len < 4)
return false;
proto = be16dec(pkt->ptr + 2);
}
for (i = 0; i < chan->npfilter; i++) {
if (chan->pfilter[i].start <= proto
&& chan->pfilter[i].end >=proto)
return true;
}
return false;
}
static bool
bnep_mfilter(channel_t *chan, packet_t *pkt)
{
int i;
if (!ETHER_IS_MULTICAST(pkt->dst))
return true;
for (i = 0; i < chan->nmfilter; i++) {
if (memcmp(pkt->dst, chan->mfilter[i].start, ETHER_ADDR_LEN) >= 0
&& memcmp(pkt->dst, chan->mfilter[i].end, ETHER_ADDR_LEN) <= 0)
return true;
}
return false;
}