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 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 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 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 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 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 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 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 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 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 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 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 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 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 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