1 /* $NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $ */
4 * SPDX-License-Identifier: BSD-2-Clause-NetBSD
6 * Copyright (c) 2008 Iain Hibbert
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
12 * 1. Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * 2. Redistributions in binary form must reproduce the above copyright
15 * notice, this list of conditions and the following disclaimer in the
16 * documentation and/or other materials provided with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
19 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
20 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
21 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
22 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
23 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
27 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 #include <sys/cdefs.h>
33 __RCSID("$NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $");
36 #define L2CAP_SOCKET_CHECKED
37 #include <bluetooth.h>
46 static bool bnep_recv_extension(packet_t *);
47 static size_t bnep_recv_control(channel_t *, uint8_t *, size_t, bool);
48 static size_t bnep_recv_control_command_not_understood(channel_t *, uint8_t *, size_t);
49 static size_t bnep_recv_setup_connection_req(channel_t *, uint8_t *, size_t);
50 static size_t bnep_recv_setup_connection_rsp(channel_t *, uint8_t *, size_t);
51 static size_t bnep_recv_filter_net_type_set(channel_t *, uint8_t *, size_t);
52 static size_t bnep_recv_filter_net_type_rsp(channel_t *, uint8_t *, size_t);
53 static size_t bnep_recv_filter_multi_addr_set(channel_t *, uint8_t *, size_t);
54 static size_t bnep_recv_filter_multi_addr_rsp(channel_t *, uint8_t *, size_t);
56 static bool bnep_pfilter(channel_t *, packet_t *);
57 static bool bnep_mfilter(channel_t *, packet_t *);
59 static uint8_t NAP_UUID[] = {
60 0x00, 0x00, 0x11, 0x16,
64 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb
67 static uint8_t GN_UUID[] = {
68 0x00, 0x00, 0x11, 0x17,
72 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,
75 static uint8_t PANU_UUID[] = {
76 0x00, 0x00, 0x11, 0x15,
80 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb
85 * return true if packet is to be forwarded
88 bnep_recv(packet_t *pkt)
99 switch (BNEP_TYPE(type)) {
100 case BNEP_GENERAL_ETHERNET:
101 if (pkt->len < (ETHER_ADDR_LEN * 2) + ETHER_TYPE_LEN) {
102 log_debug("dropped short packet (type 0x%2.2x)", type);
107 packet_adj(pkt, ETHER_ADDR_LEN);
109 packet_adj(pkt, ETHER_ADDR_LEN);
110 pkt->type = pkt->ptr;
111 packet_adj(pkt, ETHER_TYPE_LEN);
115 len = bnep_recv_control(pkt->chan, pkt->ptr, pkt->len, false);
119 packet_adj(pkt, len);
122 case BNEP_COMPRESSED_ETHERNET:
123 if (pkt->len < ETHER_TYPE_LEN) {
124 log_debug("dropped short packet (type 0x%2.2x)", type);
128 pkt->dst = pkt->chan->laddr;
129 pkt->src = pkt->chan->raddr;
130 pkt->type = pkt->ptr;
131 packet_adj(pkt, ETHER_TYPE_LEN);
134 case BNEP_COMPRESSED_ETHERNET_SRC_ONLY:
135 if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) {
136 log_debug("dropped short packet (type 0x%2.2x)", type);
140 pkt->dst = pkt->chan->laddr;
142 packet_adj(pkt, ETHER_ADDR_LEN);
143 pkt->type = pkt->ptr;
144 packet_adj(pkt, ETHER_TYPE_LEN);
147 case BNEP_COMPRESSED_ETHERNET_DST_ONLY:
148 if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) {
149 log_debug("dropped short packet (type 0x%2.2x)", type);
154 packet_adj(pkt, ETHER_ADDR_LEN);
155 pkt->src = pkt->chan->raddr;
156 pkt->type = pkt->ptr;
157 packet_adj(pkt, ETHER_TYPE_LEN);
162 * Any packet containing a reserved BNEP
163 * header packet type SHALL be dropped.
166 log_debug("dropped packet with reserved type 0x%2.2x", type);
170 if (BNEP_TYPE_EXT(type)
171 && !bnep_recv_extension(pkt))
172 return false; /* invalid extensions */
174 if (BNEP_TYPE(type) == BNEP_CONTROL
175 || pkt->chan->state != CHANNEL_OPEN)
176 return false; /* no forwarding */
182 bnep_recv_extension(packet_t *pkt)
195 if (pkt->len < size + 2)
199 case BNEP_EXTENSION_CONTROL:
200 len = bnep_recv_control(pkt->chan, pkt->ptr + 2, size, true);
202 log_err("ignored spurious data in exthdr");
207 /* Unknown extension headers in data packets */
208 /* SHALL be forwarded irrespective of any */
209 /* network protocol or multicast filter settings */
210 /* and any local filtering policy. */
212 eh = malloc(sizeof(exthdr_t));
214 log_err("exthdr malloc() failed: %m");
220 STAILQ_INSERT_TAIL(&pkt->extlist, eh, next);
224 packet_adj(pkt, size + 2);
225 } while (BNEP_TYPE_EXT(type));
231 bnep_recv_control(channel_t *chan, uint8_t *ptr, size_t size, bool isext)
242 case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
243 len = bnep_recv_control_command_not_understood(chan, ptr, size);
246 case BNEP_SETUP_CONNECTION_REQUEST:
248 return 0; /* not allowed in extension headers */
250 len = bnep_recv_setup_connection_req(chan, ptr, size);
253 case BNEP_SETUP_CONNECTION_RESPONSE:
255 return 0; /* not allowed in extension headers */
257 len = bnep_recv_setup_connection_rsp(chan, ptr, size);
260 case BNEP_FILTER_NET_TYPE_SET:
261 len = bnep_recv_filter_net_type_set(chan, ptr, size);
264 case BNEP_FILTER_NET_TYPE_RESPONSE:
265 len = bnep_recv_filter_net_type_rsp(chan, ptr, size);
268 case BNEP_FILTER_MULTI_ADDR_SET:
269 len = bnep_recv_filter_multi_addr_set(chan, ptr, size);
272 case BNEP_FILTER_MULTI_ADDR_RESPONSE:
273 len = bnep_recv_filter_multi_addr_rsp(chan, ptr, size);
282 bnep_send_control(chan, BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD, type);
288 bnep_recv_control_command_not_understood(channel_t *chan, uint8_t *ptr, size_t size)
296 log_err("received Control Command Not Understood (0x%2.2x)", type);
298 /* we didn't send any reserved commands, just cut them off */
305 bnep_recv_setup_connection_req(channel_t *chan, uint8_t *ptr, size_t size)
315 if (size < (len * 2 + 1))
318 if (chan->state != CHANNEL_WAIT_CONNECT_REQ
319 && chan->state != CHANNEL_OPEN) {
320 log_debug("ignored");
321 return (len * 2 + 1);
331 rsp = BNEP_SETUP_INVALID_UUID_SIZE;
335 if (memcmp(ptr, NAP_UUID + off, len) == 0)
336 dst = SDP_SERVICE_CLASS_NAP;
337 else if (memcmp(ptr, GN_UUID + off, len) == 0)
338 dst = SDP_SERVICE_CLASS_GN;
339 else if (memcmp(ptr, PANU_UUID + off, len) == 0)
340 dst = SDP_SERVICE_CLASS_PANU;
344 if (dst != service_class) {
345 rsp = BNEP_SETUP_INVALID_DST_UUID;
351 if (memcmp(ptr, NAP_UUID + off, len) == 0)
352 src = SDP_SERVICE_CLASS_NAP;
353 else if (memcmp(ptr, GN_UUID + off, len) == 0)
354 src = SDP_SERVICE_CLASS_GN;
355 else if (memcmp(ptr, PANU_UUID + off, len) == 0)
356 src = SDP_SERVICE_CLASS_PANU;
360 if ((dst != SDP_SERVICE_CLASS_PANU && src != SDP_SERVICE_CLASS_PANU)
362 rsp = BNEP_SETUP_INVALID_SRC_UUID;
366 rsp = BNEP_SETUP_SUCCESS;
367 chan->state = CHANNEL_OPEN;
368 channel_timeout(chan, 0);
371 log_debug("addr %s response 0x%2.2x",
372 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
374 bnep_send_control(chan, BNEP_SETUP_CONNECTION_RESPONSE, rsp);
375 return (len * 2 + 1);
379 bnep_recv_setup_connection_rsp(channel_t *chan, uint8_t *ptr, size_t size)
388 if (chan->state != CHANNEL_WAIT_CONNECT_RSP) {
389 log_debug("ignored");
393 log_debug("addr %s response 0x%2.2x",
394 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
396 if (rsp == BNEP_SETUP_SUCCESS) {
397 chan->state = CHANNEL_OPEN;
398 channel_timeout(chan, 0);
407 bnep_recv_filter_net_type_set(channel_t *chan, uint8_t *ptr, size_t size)
419 if (size < (len + 2))
422 if (chan->state != CHANNEL_OPEN) {
423 log_debug("ignored");
428 pf = malloc(nf * sizeof(pfilter_t));
430 rsp = BNEP_FILTER_TOO_MANY_FILTERS;
434 log_debug("nf = %d", nf);
436 for (i = 0; i < nf; i++) {
437 pf[i].start = be16dec(ptr);
439 pf[i].end = be16dec(ptr);
442 if (pf[i].start > pf[i].end) {
444 rsp = BNEP_FILTER_INVALID_RANGE;
448 log_debug("pf[%d] = %#4.4x, %#4.4x", i, pf[i].start, pf[i].end);
457 rsp = BNEP_FILTER_SUCCESS;
460 log_debug("addr %s response 0x%2.2x",
461 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
463 bnep_send_control(chan, BNEP_FILTER_NET_TYPE_RESPONSE, rsp);
468 bnep_recv_filter_net_type_rsp(channel_t *chan, uint8_t *ptr, size_t size)
475 if (chan->state != CHANNEL_OPEN) {
476 log_debug("ignored");
482 log_debug("addr %s response 0x%2.2x",
483 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
485 /* we did not send any filter_net_type_set message */
490 bnep_recv_filter_multi_addr_set(channel_t *chan, uint8_t *ptr, size_t size)
502 if (size < (len + 2))
505 if (chan->state != CHANNEL_OPEN) {
506 log_debug("ignored");
510 nf = len / (ETHER_ADDR_LEN * 2);
511 mf = malloc(nf * sizeof(mfilter_t));
513 rsp = BNEP_FILTER_TOO_MANY_FILTERS;
517 log_debug("nf = %d", nf);
519 for (i = 0; i < nf; i++) {
520 memcpy(mf[i].start, ptr, ETHER_ADDR_LEN);
521 ptr += ETHER_ADDR_LEN;
523 memcpy(mf[i].end, ptr, ETHER_ADDR_LEN);
524 ptr += ETHER_ADDR_LEN;
526 if (memcmp(mf[i].start, mf[i].end, ETHER_ADDR_LEN) > 0) {
528 rsp = BNEP_FILTER_INVALID_RANGE;
532 log_debug("pf[%d] = "
533 "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x, "
534 "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x", i,
535 mf[i].start[0], mf[i].start[1], mf[i].start[2],
536 mf[i].start[3], mf[i].start[4], mf[i].start[5],
537 mf[i].end[0], mf[i].end[1], mf[i].end[2],
538 mf[i].end[3], mf[i].end[4], mf[i].end[5]);
547 rsp = BNEP_FILTER_SUCCESS;
550 log_debug("addr %s response 0x%2.2x",
551 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
553 bnep_send_control(chan, BNEP_FILTER_MULTI_ADDR_RESPONSE, rsp);
558 bnep_recv_filter_multi_addr_rsp(channel_t *chan, uint8_t *ptr, size_t size)
565 if (chan->state != CHANNEL_OPEN) {
566 log_debug("ignored");
571 log_debug("addr %s response 0x%2.2x",
572 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
574 /* we did not send any filter_multi_addr_set message */
579 bnep_send_control(channel_t *chan, unsigned type, ...)
585 assert(chan->state != CHANNEL_CLOSED);
587 pkt = packet_alloc(chan);
595 *p++ = (uint8_t)type;
598 case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
599 *p++ = va_arg(ap, int);
602 case BNEP_SETUP_CONNECTION_REQUEST:
603 *p++ = va_arg(ap, int);
604 be16enc(p, va_arg(ap, int));
606 be16enc(p, va_arg(ap, int));
610 case BNEP_SETUP_CONNECTION_RESPONSE:
611 case BNEP_FILTER_NET_TYPE_RESPONSE:
612 case BNEP_FILTER_MULTI_ADDR_RESPONSE:
613 be16enc(p, va_arg(ap, int));
617 case BNEP_FILTER_NET_TYPE_SET: /* TODO */
618 case BNEP_FILTER_MULTI_ADDR_SET: /* TODO */
620 log_err("Can't send control type 0x%2.2x", type);
625 pkt->len = p - pkt->ptr;
627 channel_put(chan, pkt);
632 * BNEP send packet routine
633 * return true if packet can be removed from queue
636 bnep_send(channel_t *chan, packet_t *pkt)
639 uint8_t *p, *type, *proto;
644 if (pkt->type == NULL) {
645 iov[0].iov_base = pkt->ptr;
646 iov[0].iov_len = pkt->len;
647 iov[1].iov_base = NULL;
652 dst = (memcmp(pkt->dst, chan->raddr, ETHER_ADDR_LEN) != 0);
653 src = (memcmp(pkt->src, chan->laddr, ETHER_ADDR_LEN) != 0);
659 *type = BNEP_GENERAL_ETHERNET;
660 else if (dst && !src)
661 *type = BNEP_COMPRESSED_ETHERNET_DST_ONLY;
662 else if (!dst && src)
663 *type = BNEP_COMPRESSED_ETHERNET_SRC_ONLY;
664 else /* (!dst && !src) */
665 *type = BNEP_COMPRESSED_ETHERNET;
668 memcpy(p, pkt->dst, ETHER_ADDR_LEN);
673 memcpy(p, pkt->src, ETHER_ADDR_LEN);
678 memcpy(p, pkt->type, ETHER_TYPE_LEN);
681 STAILQ_FOREACH(eh, &pkt->extlist, next) {
682 if (p + eh->len > chan->sendbuf + chan->mtu)
688 memcpy(p, eh->ptr, eh->len);
694 iov[0].iov_base = chan->sendbuf;
695 iov[0].iov_len = (p - chan->sendbuf);
697 if ((chan->npfilter == 0 || bnep_pfilter(chan, pkt))
698 && (chan->nmfilter == 0 || bnep_mfilter(chan, pkt))) {
699 iov[1].iov_base = pkt->ptr;
700 iov[1].iov_len = pkt->len;
701 } else if (be16dec(proto) == ETHERTYPE_VLAN
702 && pkt->len >= ETHER_VLAN_ENCAP_LEN) {
703 iov[1].iov_base = pkt->ptr;
704 iov[1].iov_len = ETHER_VLAN_ENCAP_LEN;
706 iov[1].iov_base = NULL;
708 memset(proto, 0, ETHER_TYPE_LEN);
712 if (iov[0].iov_len + iov[1].iov_len > chan->mtu) {
713 log_err("packet exceeded MTU (dropped)");
717 nw = writev(chan->fd, iov, __arraycount(iov));
722 bnep_pfilter(channel_t *chan, packet_t *pkt)
726 proto = be16dec(pkt->type);
727 if (proto == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */
731 proto = be16dec(pkt->ptr + 2);
734 for (i = 0; i < chan->npfilter; i++) {
735 if (chan->pfilter[i].start <= proto
736 && chan->pfilter[i].end >=proto)
744 bnep_mfilter(channel_t *chan, packet_t *pkt)
748 if (!ETHER_IS_MULTICAST(pkt->dst))
751 for (i = 0; i < chan->nmfilter; i++) {
752 if (memcmp(pkt->dst, chan->mfilter[i].start, ETHER_ADDR_LEN) >= 0
753 && memcmp(pkt->dst, chan->mfilter[i].end, ETHER_ADDR_LEN) <= 0)