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