1 /* $NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $ */
4 * Copyright (c) 2008 Iain Hibbert
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
17 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
18 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
19 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
20 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
22 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
23 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
25 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 #include <sys/cdefs.h>
31 __RCSID("$NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $");
34 #include <bluetooth.h>
43 static bool bnep_recv_extension(packet_t *);
44 static size_t bnep_recv_control(channel_t *, uint8_t *, size_t, bool);
45 static size_t bnep_recv_control_command_not_understood(channel_t *, uint8_t *, size_t);
46 static size_t bnep_recv_setup_connection_req(channel_t *, uint8_t *, size_t);
47 static size_t bnep_recv_setup_connection_rsp(channel_t *, uint8_t *, size_t);
48 static size_t bnep_recv_filter_net_type_set(channel_t *, uint8_t *, size_t);
49 static size_t bnep_recv_filter_net_type_rsp(channel_t *, uint8_t *, size_t);
50 static size_t bnep_recv_filter_multi_addr_set(channel_t *, uint8_t *, size_t);
51 static size_t bnep_recv_filter_multi_addr_rsp(channel_t *, uint8_t *, size_t);
53 static bool bnep_pfilter(channel_t *, packet_t *);
54 static bool bnep_mfilter(channel_t *, packet_t *);
56 static uint8_t NAP_UUID[] = {
57 0x00, 0x00, 0x11, 0x16,
61 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb
64 static uint8_t GN_UUID[] = {
65 0x00, 0x00, 0x11, 0x17,
69 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,
72 static uint8_t PANU_UUID[] = {
73 0x00, 0x00, 0x11, 0x15,
77 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb
82 * return true if packet is to be forwarded
85 bnep_recv(packet_t *pkt)
96 switch (BNEP_TYPE(type)) {
97 case BNEP_GENERAL_ETHERNET:
98 if (pkt->len < (ETHER_ADDR_LEN * 2) + ETHER_TYPE_LEN) {
99 log_debug("dropped short packet (type 0x%2.2x)", type);
104 packet_adj(pkt, ETHER_ADDR_LEN);
106 packet_adj(pkt, ETHER_ADDR_LEN);
107 pkt->type = pkt->ptr;
108 packet_adj(pkt, ETHER_TYPE_LEN);
112 len = bnep_recv_control(pkt->chan, pkt->ptr, pkt->len, false);
116 packet_adj(pkt, len);
119 case BNEP_COMPRESSED_ETHERNET:
120 if (pkt->len < ETHER_TYPE_LEN) {
121 log_debug("dropped short packet (type 0x%2.2x)", type);
125 pkt->dst = pkt->chan->laddr;
126 pkt->src = pkt->chan->raddr;
127 pkt->type = pkt->ptr;
128 packet_adj(pkt, ETHER_TYPE_LEN);
131 case BNEP_COMPRESSED_ETHERNET_SRC_ONLY:
132 if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) {
133 log_debug("dropped short packet (type 0x%2.2x)", type);
137 pkt->dst = pkt->chan->laddr;
139 packet_adj(pkt, ETHER_ADDR_LEN);
140 pkt->type = pkt->ptr;
141 packet_adj(pkt, ETHER_TYPE_LEN);
144 case BNEP_COMPRESSED_ETHERNET_DST_ONLY:
145 if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) {
146 log_debug("dropped short packet (type 0x%2.2x)", type);
151 packet_adj(pkt, ETHER_ADDR_LEN);
152 pkt->src = pkt->chan->raddr;
153 pkt->type = pkt->ptr;
154 packet_adj(pkt, ETHER_TYPE_LEN);
159 * Any packet containing a reserved BNEP
160 * header packet type SHALL be dropped.
163 log_debug("dropped packet with reserved type 0x%2.2x", type);
167 if (BNEP_TYPE_EXT(type)
168 && !bnep_recv_extension(pkt))
169 return false; /* invalid extensions */
171 if (BNEP_TYPE(type) == BNEP_CONTROL
172 || pkt->chan->state != CHANNEL_OPEN)
173 return false; /* no forwarding */
179 bnep_recv_extension(packet_t *pkt)
192 if (pkt->len < size + 2)
196 case BNEP_EXTENSION_CONTROL:
197 len = bnep_recv_control(pkt->chan, pkt->ptr + 2, size, true);
199 log_err("ignored spurious data in exthdr");
204 /* Unknown extension headers in data packets */
205 /* SHALL be forwarded irrespective of any */
206 /* network protocol or multicast filter settings */
207 /* and any local filtering policy. */
209 eh = malloc(sizeof(exthdr_t));
211 log_err("exthdr malloc() failed: %m");
217 STAILQ_INSERT_TAIL(&pkt->extlist, eh, next);
221 packet_adj(pkt, size + 2);
222 } while (BNEP_TYPE_EXT(type));
228 bnep_recv_control(channel_t *chan, uint8_t *ptr, size_t size, bool isext)
239 case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
240 len = bnep_recv_control_command_not_understood(chan, ptr, size);
243 case BNEP_SETUP_CONNECTION_REQUEST:
245 return 0; /* not allowed in extension headers */
247 len = bnep_recv_setup_connection_req(chan, ptr, size);
250 case BNEP_SETUP_CONNECTION_RESPONSE:
252 return 0; /* not allowed in extension headers */
254 len = bnep_recv_setup_connection_rsp(chan, ptr, size);
257 case BNEP_FILTER_NET_TYPE_SET:
258 len = bnep_recv_filter_net_type_set(chan, ptr, size);
261 case BNEP_FILTER_NET_TYPE_RESPONSE:
262 len = bnep_recv_filter_net_type_rsp(chan, ptr, size);
265 case BNEP_FILTER_MULTI_ADDR_SET:
266 len = bnep_recv_filter_multi_addr_set(chan, ptr, size);
269 case BNEP_FILTER_MULTI_ADDR_RESPONSE:
270 len = bnep_recv_filter_multi_addr_rsp(chan, ptr, size);
279 bnep_send_control(chan, BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD, type);
285 bnep_recv_control_command_not_understood(channel_t *chan, uint8_t *ptr, size_t size)
293 log_err("received Control Command Not Understood (0x%2.2x)", type);
295 /* we didn't send any reserved commands, just cut them off */
302 bnep_recv_setup_connection_req(channel_t *chan, uint8_t *ptr, size_t size)
312 if (size < (len * 2 + 1))
315 if (chan->state != CHANNEL_WAIT_CONNECT_REQ
316 && chan->state != CHANNEL_OPEN) {
317 log_debug("ignored");
318 return (len * 2 + 1);
328 rsp = BNEP_SETUP_INVALID_UUID_SIZE;
332 if (memcmp(ptr, NAP_UUID + off, len) == 0)
333 dst = SDP_SERVICE_CLASS_NAP;
334 else if (memcmp(ptr, GN_UUID + off, len) == 0)
335 dst = SDP_SERVICE_CLASS_GN;
336 else if (memcmp(ptr, PANU_UUID + off, len) == 0)
337 dst = SDP_SERVICE_CLASS_PANU;
341 if (dst != service_class) {
342 rsp = BNEP_SETUP_INVALID_DST_UUID;
348 if (memcmp(ptr, NAP_UUID + off, len) == 0)
349 src = SDP_SERVICE_CLASS_NAP;
350 else if (memcmp(ptr, GN_UUID + off, len) == 0)
351 src = SDP_SERVICE_CLASS_GN;
352 else if (memcmp(ptr, PANU_UUID + off, len) == 0)
353 src = SDP_SERVICE_CLASS_PANU;
357 if ((dst != SDP_SERVICE_CLASS_PANU && src != SDP_SERVICE_CLASS_PANU)
359 rsp = BNEP_SETUP_INVALID_SRC_UUID;
363 rsp = BNEP_SETUP_SUCCESS;
364 chan->state = CHANNEL_OPEN;
365 channel_timeout(chan, 0);
368 log_debug("addr %s response 0x%2.2x",
369 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
371 bnep_send_control(chan, BNEP_SETUP_CONNECTION_RESPONSE, rsp);
372 return (len * 2 + 1);
376 bnep_recv_setup_connection_rsp(channel_t *chan, uint8_t *ptr, size_t size)
385 if (chan->state != CHANNEL_WAIT_CONNECT_RSP) {
386 log_debug("ignored");
390 log_debug("addr %s response 0x%2.2x",
391 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
393 if (rsp == BNEP_SETUP_SUCCESS) {
394 chan->state = CHANNEL_OPEN;
395 channel_timeout(chan, 0);
404 bnep_recv_filter_net_type_set(channel_t *chan, uint8_t *ptr, size_t size)
416 if (size < (len + 2))
419 if (chan->state != CHANNEL_OPEN) {
420 log_debug("ignored");
425 pf = malloc(nf * sizeof(pfilter_t));
427 rsp = BNEP_FILTER_TOO_MANY_FILTERS;
431 log_debug("nf = %d", nf);
433 for (i = 0; i < nf; i++) {
434 pf[i].start = be16dec(ptr);
436 pf[i].end = be16dec(ptr);
439 if (pf[i].start > pf[i].end) {
441 rsp = BNEP_FILTER_INVALID_RANGE;
445 log_debug("pf[%d] = %#4.4x, %#4.4x", i, pf[i].start, pf[i].end);
454 rsp = BNEP_FILTER_SUCCESS;
457 log_debug("addr %s response 0x%2.2x",
458 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
460 bnep_send_control(chan, BNEP_FILTER_NET_TYPE_RESPONSE, rsp);
465 bnep_recv_filter_net_type_rsp(channel_t *chan, uint8_t *ptr, size_t size)
472 if (chan->state != CHANNEL_OPEN) {
473 log_debug("ignored");
479 log_debug("addr %s response 0x%2.2x",
480 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
482 /* we did not send any filter_net_type_set message */
487 bnep_recv_filter_multi_addr_set(channel_t *chan, uint8_t *ptr, size_t size)
499 if (size < (len + 2))
502 if (chan->state != CHANNEL_OPEN) {
503 log_debug("ignored");
507 nf = len / (ETHER_ADDR_LEN * 2);
508 mf = malloc(nf * sizeof(mfilter_t));
510 rsp = BNEP_FILTER_TOO_MANY_FILTERS;
514 log_debug("nf = %d", nf);
516 for (i = 0; i < nf; i++) {
517 memcpy(mf[i].start, ptr, ETHER_ADDR_LEN);
518 ptr += ETHER_ADDR_LEN;
520 memcpy(mf[i].end, ptr, ETHER_ADDR_LEN);
521 ptr += ETHER_ADDR_LEN;
523 if (memcmp(mf[i].start, mf[i].end, ETHER_ADDR_LEN) > 0) {
525 rsp = BNEP_FILTER_INVALID_RANGE;
529 log_debug("pf[%d] = "
530 "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x, "
531 "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x", i,
532 mf[i].start[0], mf[i].start[1], mf[i].start[2],
533 mf[i].start[3], mf[i].start[4], mf[i].start[5],
534 mf[i].end[0], mf[i].end[1], mf[i].end[2],
535 mf[i].end[3], mf[i].end[4], mf[i].end[5]);
544 rsp = BNEP_FILTER_SUCCESS;
547 log_debug("addr %s response 0x%2.2x",
548 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
550 bnep_send_control(chan, BNEP_FILTER_MULTI_ADDR_RESPONSE, rsp);
555 bnep_recv_filter_multi_addr_rsp(channel_t *chan, uint8_t *ptr, size_t size)
562 if (chan->state != CHANNEL_OPEN) {
563 log_debug("ignored");
568 log_debug("addr %s response 0x%2.2x",
569 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
571 /* we did not send any filter_multi_addr_set message */
576 bnep_send_control(channel_t *chan, uint8_t type, ...)
582 assert(chan->state != CHANNEL_CLOSED);
584 pkt = packet_alloc(chan);
595 case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
596 *p++ = va_arg(ap, int);
599 case BNEP_SETUP_CONNECTION_REQUEST:
600 *p++ = va_arg(ap, int);
601 be16enc(p, va_arg(ap, int));
603 be16enc(p, va_arg(ap, int));
607 case BNEP_SETUP_CONNECTION_RESPONSE:
608 case BNEP_FILTER_NET_TYPE_RESPONSE:
609 case BNEP_FILTER_MULTI_ADDR_RESPONSE:
610 be16enc(p, va_arg(ap, int));
614 case BNEP_FILTER_NET_TYPE_SET: /* TODO */
615 case BNEP_FILTER_MULTI_ADDR_SET: /* TODO */
617 log_err("Can't send control type 0x%2.2x", type);
622 pkt->len = p - pkt->ptr;
624 channel_put(chan, pkt);
629 * BNEP send packet routine
630 * return true if packet can be removed from queue
633 bnep_send(channel_t *chan, packet_t *pkt)
636 uint8_t *p, *type, *proto;
641 if (pkt->type == NULL) {
642 iov[0].iov_base = pkt->ptr;
643 iov[0].iov_len = pkt->len;
644 iov[1].iov_base = NULL;
649 dst = (memcmp(pkt->dst, chan->raddr, ETHER_ADDR_LEN) != 0);
650 src = (memcmp(pkt->src, chan->laddr, ETHER_ADDR_LEN) != 0);
656 *type = BNEP_GENERAL_ETHERNET;
657 else if (dst && !src)
658 *type = BNEP_COMPRESSED_ETHERNET_DST_ONLY;
659 else if (!dst && src)
660 *type = BNEP_COMPRESSED_ETHERNET_SRC_ONLY;
661 else /* (!dst && !src) */
662 *type = BNEP_COMPRESSED_ETHERNET;
665 memcpy(p, pkt->dst, ETHER_ADDR_LEN);
670 memcpy(p, pkt->src, ETHER_ADDR_LEN);
675 memcpy(p, pkt->type, ETHER_TYPE_LEN);
678 STAILQ_FOREACH(eh, &pkt->extlist, next) {
679 if (p + eh->len > chan->sendbuf + chan->mtu)
685 memcpy(p, eh->ptr, eh->len);
691 iov[0].iov_base = chan->sendbuf;
692 iov[0].iov_len = (p - chan->sendbuf);
694 if ((chan->npfilter == 0 || bnep_pfilter(chan, pkt))
695 && (chan->nmfilter == 0 || bnep_mfilter(chan, pkt))) {
696 iov[1].iov_base = pkt->ptr;
697 iov[1].iov_len = pkt->len;
698 } else if (be16dec(proto) == ETHERTYPE_VLAN
699 && pkt->len >= ETHER_VLAN_ENCAP_LEN) {
700 iov[1].iov_base = pkt->ptr;
701 iov[1].iov_len = ETHER_VLAN_ENCAP_LEN;
703 iov[1].iov_base = NULL;
705 memset(proto, 0, ETHER_TYPE_LEN);
709 if (iov[0].iov_len + iov[1].iov_len > chan->mtu) {
710 log_err("packet exceeded MTU (dropped)");
714 nw = writev(chan->fd, iov, __arraycount(iov));
719 bnep_pfilter(channel_t *chan, packet_t *pkt)
723 proto = be16dec(pkt->type);
724 if (proto == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */
728 proto = be16dec(pkt->ptr + 2);
731 for (i = 0; i < chan->npfilter; i++) {
732 if (chan->pfilter[i].start <= proto
733 && chan->pfilter[i].end >=proto)
741 bnep_mfilter(channel_t *chan, packet_t *pkt)
745 if (!ETHER_IS_MULTICAST(pkt->dst))
748 for (i = 0; i < chan->nmfilter; i++) {
749 if (memcmp(pkt->dst, chan->mfilter[i].start, ETHER_ADDR_LEN) >= 0
750 && memcmp(pkt->dst, chan->mfilter[i].end, ETHER_ADDR_LEN) <= 0)