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