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$ */ 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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