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