1 /* $NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $ */
2
3 /*-
4 * SPDX-License-Identifier: BSD-2-Clause-NetBSD
5 *
6 * Copyright (c) 2008 Iain Hibbert
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
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.
17 *
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.
28 */
29
30 /* $FreeBSD: stable/12/usr.sbin/bluetooth/btpand/bnep.c 326276 2017-11-27 15:37:16Z pfg $ */
31
32 #include <sys/cdefs.h>
33 __RCSID("$NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $");
34
35 #include <sys/uio.h>
36 #define L2CAP_SOCKET_CHECKED
37 #include <bluetooth.h>
38 #include <sdp.h>
39 #include <stdarg.h>
40 #include <string.h>
41 #include <unistd.h>
42
43 #include "btpand.h"
44 #include "bnep.h"
45
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);
55
56 static bool bnep_pfilter(channel_t *, packet_t *);
57 static bool bnep_mfilter(channel_t *, packet_t *);
58
59 static uint8_t NAP_UUID[] = {
60 0x00, 0x00, 0x11, 0x16,
61 0x00, 0x00,
62 0x10, 0x00,
63 0x80, 0x00,
64 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb
65 };
66
67 static uint8_t GN_UUID[] = {
68 0x00, 0x00, 0x11, 0x17,
69 0x00, 0x00,
70 0x10, 0x00,
71 0x80, 0x00,
72 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,
73 };
74
75 static uint8_t PANU_UUID[] = {
76 0x00, 0x00, 0x11, 0x15,
77 0x00, 0x00,
78 0x10, 0x00,
79 0x80, 0x00,
80 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb
81 };
82
83 /*
84 * receive BNEP packet
85 * return true if packet is to be forwarded
86 */
87 bool
bnep_recv(packet_t * pkt)88 bnep_recv(packet_t *pkt)
89 {
90 size_t len;
91 uint8_t type;
92
93 if (pkt->len < 1)
94 return false;
95
96 type = pkt->ptr[0];
97 packet_adj(pkt, 1);
98
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);
103 return false;
104 }
105
106 pkt->dst = pkt->ptr;
107 packet_adj(pkt, ETHER_ADDR_LEN);
108 pkt->src = pkt->ptr;
109 packet_adj(pkt, ETHER_ADDR_LEN);
110 pkt->type = pkt->ptr;
111 packet_adj(pkt, ETHER_TYPE_LEN);
112 break;
113
114 case BNEP_CONTROL:
115 len = bnep_recv_control(pkt->chan, pkt->ptr, pkt->len, false);
116 if (len == 0)
117 return false;
118
119 packet_adj(pkt, len);
120 break;
121
122 case BNEP_COMPRESSED_ETHERNET:
123 if (pkt->len < ETHER_TYPE_LEN) {
124 log_debug("dropped short packet (type 0x%2.2x)", type);
125 return false;
126 }
127
128 pkt->dst = pkt->chan->laddr;
129 pkt->src = pkt->chan->raddr;
130 pkt->type = pkt->ptr;
131 packet_adj(pkt, ETHER_TYPE_LEN);
132 break;
133
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);
137 return false;
138 }
139
140 pkt->dst = pkt->chan->laddr;
141 pkt->src = pkt->ptr;
142 packet_adj(pkt, ETHER_ADDR_LEN);
143 pkt->type = pkt->ptr;
144 packet_adj(pkt, ETHER_TYPE_LEN);
145 break;
146
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);
150 return false;
151 }
152
153 pkt->dst = pkt->ptr;
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);
158 break;
159
160 default:
161 /*
162 * Any packet containing a reserved BNEP
163 * header packet type SHALL be dropped.
164 */
165
166 log_debug("dropped packet with reserved type 0x%2.2x", type);
167 return false;
168 }
169
170 if (BNEP_TYPE_EXT(type)
171 && !bnep_recv_extension(pkt))
172 return false; /* invalid extensions */
173
174 if (BNEP_TYPE(type) == BNEP_CONTROL
175 || pkt->chan->state != CHANNEL_OPEN)
176 return false; /* no forwarding */
177
178 return true;
179 }
180
181 static bool
bnep_recv_extension(packet_t * pkt)182 bnep_recv_extension(packet_t *pkt)
183 {
184 exthdr_t *eh;
185 size_t len, size;
186 uint8_t type;
187
188 do {
189 if (pkt->len < 2)
190 return false;
191
192 type = pkt->ptr[0];
193 size = pkt->ptr[1];
194
195 if (pkt->len < size + 2)
196 return false;
197
198 switch (type) {
199 case BNEP_EXTENSION_CONTROL:
200 len = bnep_recv_control(pkt->chan, pkt->ptr + 2, size, true);
201 if (len != size)
202 log_err("ignored spurious data in exthdr");
203
204 break;
205
206 default:
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. */
211
212 eh = malloc(sizeof(exthdr_t));
213 if (eh == NULL) {
214 log_err("exthdr malloc() failed: %m");
215 break;
216 }
217
218 eh->ptr = pkt->ptr;
219 eh->len = size;
220 STAILQ_INSERT_TAIL(&pkt->extlist, eh, next);
221 break;
222 }
223
224 packet_adj(pkt, size + 2);
225 } while (BNEP_TYPE_EXT(type));
226
227 return true;
228 }
229
230 static size_t
bnep_recv_control(channel_t * chan,uint8_t * ptr,size_t size,bool isext)231 bnep_recv_control(channel_t *chan, uint8_t *ptr, size_t size, bool isext)
232 {
233 uint8_t type;
234 size_t len;
235
236 if (size-- < 1)
237 return 0;
238
239 type = *ptr++;
240
241 switch (type) {
242 case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
243 len = bnep_recv_control_command_not_understood(chan, ptr, size);
244 break;
245
246 case BNEP_SETUP_CONNECTION_REQUEST:
247 if (isext)
248 return 0; /* not allowed in extension headers */
249
250 len = bnep_recv_setup_connection_req(chan, ptr, size);
251 break;
252
253 case BNEP_SETUP_CONNECTION_RESPONSE:
254 if (isext)
255 return 0; /* not allowed in extension headers */
256
257 len = bnep_recv_setup_connection_rsp(chan, ptr, size);
258 break;
259
260 case BNEP_FILTER_NET_TYPE_SET:
261 len = bnep_recv_filter_net_type_set(chan, ptr, size);
262 break;
263
264 case BNEP_FILTER_NET_TYPE_RESPONSE:
265 len = bnep_recv_filter_net_type_rsp(chan, ptr, size);
266 break;
267
268 case BNEP_FILTER_MULTI_ADDR_SET:
269 len = bnep_recv_filter_multi_addr_set(chan, ptr, size);
270 break;
271
272 case BNEP_FILTER_MULTI_ADDR_RESPONSE:
273 len = bnep_recv_filter_multi_addr_rsp(chan, ptr, size);
274 break;
275
276 default:
277 len = 0;
278 break;
279 }
280
281 if (len == 0)
282 bnep_send_control(chan, BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD, type);
283
284 return len;
285 }
286
287 static size_t
bnep_recv_control_command_not_understood(channel_t * chan,uint8_t * ptr,size_t size)288 bnep_recv_control_command_not_understood(channel_t *chan, uint8_t *ptr, size_t size)
289 {
290 uint8_t type;
291
292 if (size < 1)
293 return 0;
294
295 type = *ptr++;
296 log_err("received Control Command Not Understood (0x%2.2x)", type);
297
298 /* we didn't send any reserved commands, just cut them off */
299 channel_close(chan);
300
301 return 1;
302 }
303
304 static size_t
bnep_recv_setup_connection_req(channel_t * chan,uint8_t * ptr,size_t size)305 bnep_recv_setup_connection_req(channel_t *chan, uint8_t *ptr, size_t size)
306 {
307 uint8_t off;
308 int src, dst, rsp;
309 size_t len;
310
311 if (size < 1)
312 return 0;
313
314 len = *ptr++;
315 if (size < (len * 2 + 1))
316 return 0;
317
318 if (chan->state != CHANNEL_WAIT_CONNECT_REQ
319 && chan->state != CHANNEL_OPEN) {
320 log_debug("ignored");
321 return (len * 2 + 1);
322 }
323
324 if (len == 2)
325 off = 2;
326 else if (len == 4)
327 off = 0;
328 else if (len == 16)
329 off = 0;
330 else {
331 rsp = BNEP_SETUP_INVALID_UUID_SIZE;
332 goto done;
333 }
334
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;
341 else
342 dst = 0;
343
344 if (dst != service_class) {
345 rsp = BNEP_SETUP_INVALID_DST_UUID;
346 goto done;
347 }
348
349 ptr += len;
350
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;
357 else
358 src = 0;
359
360 if ((dst != SDP_SERVICE_CLASS_PANU && src != SDP_SERVICE_CLASS_PANU)
361 || src == 0) {
362 rsp = BNEP_SETUP_INVALID_SRC_UUID;
363 goto done;
364 }
365
366 rsp = BNEP_SETUP_SUCCESS;
367 chan->state = CHANNEL_OPEN;
368 channel_timeout(chan, 0);
369
370 done:
371 log_debug("addr %s response 0x%2.2x",
372 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
373
374 bnep_send_control(chan, BNEP_SETUP_CONNECTION_RESPONSE, rsp);
375 return (len * 2 + 1);
376 }
377
378 static size_t
bnep_recv_setup_connection_rsp(channel_t * chan,uint8_t * ptr,size_t size)379 bnep_recv_setup_connection_rsp(channel_t *chan, uint8_t *ptr, size_t size)
380 {
381 int rsp;
382
383 if (size < 2)
384 return 0;
385
386 rsp = be16dec(ptr);
387
388 if (chan->state != CHANNEL_WAIT_CONNECT_RSP) {
389 log_debug("ignored");
390 return 2;
391 }
392
393 log_debug("addr %s response 0x%2.2x",
394 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
395
396 if (rsp == BNEP_SETUP_SUCCESS) {
397 chan->state = CHANNEL_OPEN;
398 channel_timeout(chan, 0);
399 } else {
400 channel_close(chan);
401 }
402
403 return 2;
404 }
405
406 static size_t
bnep_recv_filter_net_type_set(channel_t * chan,uint8_t * ptr,size_t size)407 bnep_recv_filter_net_type_set(channel_t *chan, uint8_t *ptr, size_t size)
408 {
409 pfilter_t *pf;
410 int i, nf, rsp;
411 size_t len;
412
413 if (size < 2)
414 return 0;
415
416 len = be16dec(ptr);
417 ptr += 2;
418
419 if (size < (len + 2))
420 return 0;
421
422 if (chan->state != CHANNEL_OPEN) {
423 log_debug("ignored");
424 return (len + 2);
425 }
426
427 nf = len / 4;
428 pf = malloc(nf * sizeof(pfilter_t));
429 if (pf == NULL) {
430 rsp = BNEP_FILTER_TOO_MANY_FILTERS;
431 goto done;
432 }
433
434 log_debug("nf = %d", nf);
435
436 for (i = 0; i < nf; i++) {
437 pf[i].start = be16dec(ptr);
438 ptr += 2;
439 pf[i].end = be16dec(ptr);
440 ptr += 2;
441
442 if (pf[i].start > pf[i].end) {
443 free(pf);
444 rsp = BNEP_FILTER_INVALID_RANGE;
445 goto done;
446 }
447
448 log_debug("pf[%d] = %#4.4x, %#4.4x", i, pf[i].start, pf[i].end);
449 }
450
451 if (chan->pfilter)
452 free(chan->pfilter);
453
454 chan->pfilter = pf;
455 chan->npfilter = nf;
456
457 rsp = BNEP_FILTER_SUCCESS;
458
459 done:
460 log_debug("addr %s response 0x%2.2x",
461 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
462
463 bnep_send_control(chan, BNEP_FILTER_NET_TYPE_RESPONSE, rsp);
464 return (len + 2);
465 }
466
467 static size_t
bnep_recv_filter_net_type_rsp(channel_t * chan,uint8_t * ptr,size_t size)468 bnep_recv_filter_net_type_rsp(channel_t *chan, uint8_t *ptr, size_t size)
469 {
470 int rsp;
471
472 if (size < 2)
473 return 0;
474
475 if (chan->state != CHANNEL_OPEN) {
476 log_debug("ignored");
477 return 2;
478 }
479
480 rsp = be16dec(ptr);
481
482 log_debug("addr %s response 0x%2.2x",
483 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
484
485 /* we did not send any filter_net_type_set message */
486 return 2;
487 }
488
489 static size_t
bnep_recv_filter_multi_addr_set(channel_t * chan,uint8_t * ptr,size_t size)490 bnep_recv_filter_multi_addr_set(channel_t *chan, uint8_t *ptr, size_t size)
491 {
492 mfilter_t *mf;
493 int i, nf, rsp;
494 size_t len;
495
496 if (size < 2)
497 return 0;
498
499 len = be16dec(ptr);
500 ptr += 2;
501
502 if (size < (len + 2))
503 return 0;
504
505 if (chan->state != CHANNEL_OPEN) {
506 log_debug("ignored");
507 return (len + 2);
508 }
509
510 nf = len / (ETHER_ADDR_LEN * 2);
511 mf = malloc(nf * sizeof(mfilter_t));
512 if (mf == NULL) {
513 rsp = BNEP_FILTER_TOO_MANY_FILTERS;
514 goto done;
515 }
516
517 log_debug("nf = %d", nf);
518
519 for (i = 0; i < nf; i++) {
520 memcpy(mf[i].start, ptr, ETHER_ADDR_LEN);
521 ptr += ETHER_ADDR_LEN;
522
523 memcpy(mf[i].end, ptr, ETHER_ADDR_LEN);
524 ptr += ETHER_ADDR_LEN;
525
526 if (memcmp(mf[i].start, mf[i].end, ETHER_ADDR_LEN) > 0) {
527 free(mf);
528 rsp = BNEP_FILTER_INVALID_RANGE;
529 goto done;
530 }
531
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]);
539 }
540
541 if (chan->mfilter)
542 free(chan->mfilter);
543
544 chan->mfilter = mf;
545 chan->nmfilter = nf;
546
547 rsp = BNEP_FILTER_SUCCESS;
548
549 done:
550 log_debug("addr %s response 0x%2.2x",
551 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
552
553 bnep_send_control(chan, BNEP_FILTER_MULTI_ADDR_RESPONSE, rsp);
554 return (len + 2);
555 }
556
557 static size_t
bnep_recv_filter_multi_addr_rsp(channel_t * chan,uint8_t * ptr,size_t size)558 bnep_recv_filter_multi_addr_rsp(channel_t *chan, uint8_t *ptr, size_t size)
559 {
560 int rsp;
561
562 if (size < 2)
563 return false;
564
565 if (chan->state != CHANNEL_OPEN) {
566 log_debug("ignored");
567 return 2;
568 }
569
570 rsp = be16dec(ptr);
571 log_debug("addr %s response 0x%2.2x",
572 ether_ntoa((struct ether_addr *)chan->raddr), rsp);
573
574 /* we did not send any filter_multi_addr_set message */
575 return 2;
576 }
577
578 void
bnep_send_control(channel_t * chan,unsigned type,...)579 bnep_send_control(channel_t *chan, unsigned type, ...)
580 {
581 packet_t *pkt;
582 uint8_t *p;
583 va_list ap;
584
585 assert(chan->state != CHANNEL_CLOSED);
586
587 pkt = packet_alloc(chan);
588 if (pkt == NULL)
589 return;
590
591 p = pkt->ptr;
592 va_start(ap, type);
593
594 *p++ = BNEP_CONTROL;
595 *p++ = (uint8_t)type;
596
597 switch(type) {
598 case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
599 *p++ = va_arg(ap, int);
600 break;
601
602 case BNEP_SETUP_CONNECTION_REQUEST:
603 *p++ = va_arg(ap, int);
604 be16enc(p, va_arg(ap, int));
605 p += 2;
606 be16enc(p, va_arg(ap, int));
607 p += 2;
608 break;
609
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));
614 p += 2;
615 break;
616
617 case BNEP_FILTER_NET_TYPE_SET: /* TODO */
618 case BNEP_FILTER_MULTI_ADDR_SET: /* TODO */
619 default:
620 log_err("Can't send control type 0x%2.2x", type);
621 break;
622 }
623
624 va_end(ap);
625 pkt->len = p - pkt->ptr;
626
627 channel_put(chan, pkt);
628 packet_free(pkt);
629 }
630
631 /*
632 * BNEP send packet routine
633 * return true if packet can be removed from queue
634 */
635 bool
bnep_send(channel_t * chan,packet_t * pkt)636 bnep_send(channel_t *chan, packet_t *pkt)
637 {
638 struct iovec iov[2];
639 uint8_t *p, *type, *proto;
640 exthdr_t *eh;
641 bool src, dst;
642 size_t nw;
643
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;
648 iov[1].iov_len = 0;
649 } else {
650 p = chan->sendbuf;
651
652 dst = (memcmp(pkt->dst, chan->raddr, ETHER_ADDR_LEN) != 0);
653 src = (memcmp(pkt->src, chan->laddr, ETHER_ADDR_LEN) != 0);
654
655 type = p;
656 p += 1;
657
658 if (dst && src)
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;
666
667 if (dst) {
668 memcpy(p, pkt->dst, ETHER_ADDR_LEN);
669 p += ETHER_ADDR_LEN;
670 }
671
672 if (src) {
673 memcpy(p, pkt->src, ETHER_ADDR_LEN);
674 p += ETHER_ADDR_LEN;
675 }
676
677 proto = p;
678 memcpy(p, pkt->type, ETHER_TYPE_LEN);
679 p += ETHER_TYPE_LEN;
680
681 STAILQ_FOREACH(eh, &pkt->extlist, next) {
682 if (p + eh->len > chan->sendbuf + chan->mtu)
683 break;
684
685 *type |= BNEP_EXT;
686 type = p;
687
688 memcpy(p, eh->ptr, eh->len);
689 p += eh->len;
690 }
691
692 *type &= ~BNEP_EXT;
693
694 iov[0].iov_base = chan->sendbuf;
695 iov[0].iov_len = (p - chan->sendbuf);
696
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;
705 } else {
706 iov[1].iov_base = NULL;
707 iov[1].iov_len = 0;
708 memset(proto, 0, ETHER_TYPE_LEN);
709 }
710 }
711
712 if (iov[0].iov_len + iov[1].iov_len > chan->mtu) {
713 log_err("packet exceeded MTU (dropped)");
714 return false;
715 }
716
717 nw = writev(chan->fd, iov, __arraycount(iov));
718 return (nw > 0);
719 }
720
721 static bool
bnep_pfilter(channel_t * chan,packet_t * pkt)722 bnep_pfilter(channel_t *chan, packet_t *pkt)
723 {
724 int proto, i;
725
726 proto = be16dec(pkt->type);
727 if (proto == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */
728 if (pkt->len < 4)
729 return false;
730
731 proto = be16dec(pkt->ptr + 2);
732 }
733
734 for (i = 0; i < chan->npfilter; i++) {
735 if (chan->pfilter[i].start <= proto
736 && chan->pfilter[i].end >=proto)
737 return true;
738 }
739
740 return false;
741 }
742
743 static bool
bnep_mfilter(channel_t * chan,packet_t * pkt)744 bnep_mfilter(channel_t *chan, packet_t *pkt)
745 {
746 int i;
747
748 if (!ETHER_IS_MULTICAST(pkt->dst))
749 return true;
750
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)
754 return true;
755 }
756
757 return false;
758 }
759