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 #define L2CAP_SOCKET_CHECKED 35 #include <bluetooth.h> 36 #include <sdp.h> 37 #include <stdarg.h> 38 #include <string.h> 39 #include <unistd.h> 40 41 #include "btpand.h" 42 #include "bnep.h" 43 44 static bool bnep_recv_extension(packet_t *); 45 static size_t bnep_recv_control(channel_t *, uint8_t *, size_t, bool); 46 static size_t bnep_recv_control_command_not_understood(channel_t *, uint8_t *, size_t); 47 static size_t bnep_recv_setup_connection_req(channel_t *, uint8_t *, size_t); 48 static size_t bnep_recv_setup_connection_rsp(channel_t *, uint8_t *, size_t); 49 static size_t bnep_recv_filter_net_type_set(channel_t *, uint8_t *, size_t); 50 static size_t bnep_recv_filter_net_type_rsp(channel_t *, uint8_t *, size_t); 51 static size_t bnep_recv_filter_multi_addr_set(channel_t *, uint8_t *, size_t); 52 static size_t bnep_recv_filter_multi_addr_rsp(channel_t *, uint8_t *, size_t); 53 54 static bool bnep_pfilter(channel_t *, packet_t *); 55 static bool bnep_mfilter(channel_t *, packet_t *); 56 57 static uint8_t NAP_UUID[] = { 58 0x00, 0x00, 0x11, 0x16, 59 0x00, 0x00, 60 0x10, 0x00, 61 0x80, 0x00, 62 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb 63 }; 64 65 static uint8_t GN_UUID[] = { 66 0x00, 0x00, 0x11, 0x17, 67 0x00, 0x00, 68 0x10, 0x00, 69 0x80, 0x00, 70 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb, 71 }; 72 73 static uint8_t PANU_UUID[] = { 74 0x00, 0x00, 0x11, 0x15, 75 0x00, 0x00, 76 0x10, 0x00, 77 0x80, 0x00, 78 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb 79 }; 80 81 /* 82 * receive BNEP packet 83 * return true if packet is to be forwarded 84 */ 85 bool 86 bnep_recv(packet_t *pkt) 87 { 88 size_t len; 89 uint8_t type; 90 91 if (pkt->len < 1) 92 return false; 93 94 type = pkt->ptr[0]; 95 packet_adj(pkt, 1); 96 97 switch (BNEP_TYPE(type)) { 98 case BNEP_GENERAL_ETHERNET: 99 if (pkt->len < (ETHER_ADDR_LEN * 2) + ETHER_TYPE_LEN) { 100 log_debug("dropped short packet (type 0x%2.2x)", type); 101 return false; 102 } 103 104 pkt->dst = pkt->ptr; 105 packet_adj(pkt, ETHER_ADDR_LEN); 106 pkt->src = pkt->ptr; 107 packet_adj(pkt, ETHER_ADDR_LEN); 108 pkt->type = pkt->ptr; 109 packet_adj(pkt, ETHER_TYPE_LEN); 110 break; 111 112 case BNEP_CONTROL: 113 len = bnep_recv_control(pkt->chan, pkt->ptr, pkt->len, false); 114 if (len == 0) 115 return false; 116 117 packet_adj(pkt, len); 118 break; 119 120 case BNEP_COMPRESSED_ETHERNET: 121 if (pkt->len < ETHER_TYPE_LEN) { 122 log_debug("dropped short packet (type 0x%2.2x)", type); 123 return false; 124 } 125 126 pkt->dst = pkt->chan->laddr; 127 pkt->src = pkt->chan->raddr; 128 pkt->type = pkt->ptr; 129 packet_adj(pkt, ETHER_TYPE_LEN); 130 break; 131 132 case BNEP_COMPRESSED_ETHERNET_SRC_ONLY: 133 if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) { 134 log_debug("dropped short packet (type 0x%2.2x)", type); 135 return false; 136 } 137 138 pkt->dst = pkt->chan->laddr; 139 pkt->src = pkt->ptr; 140 packet_adj(pkt, ETHER_ADDR_LEN); 141 pkt->type = pkt->ptr; 142 packet_adj(pkt, ETHER_TYPE_LEN); 143 break; 144 145 case BNEP_COMPRESSED_ETHERNET_DST_ONLY: 146 if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) { 147 log_debug("dropped short packet (type 0x%2.2x)", type); 148 return false; 149 } 150 151 pkt->dst = pkt->ptr; 152 packet_adj(pkt, ETHER_ADDR_LEN); 153 pkt->src = pkt->chan->raddr; 154 pkt->type = pkt->ptr; 155 packet_adj(pkt, ETHER_TYPE_LEN); 156 break; 157 158 default: 159 /* 160 * Any packet containing a reserved BNEP 161 * header packet type SHALL be dropped. 162 */ 163 164 log_debug("dropped packet with reserved type 0x%2.2x", type); 165 return false; 166 } 167 168 if (BNEP_TYPE_EXT(type) 169 && !bnep_recv_extension(pkt)) 170 return false; /* invalid extensions */ 171 172 if (BNEP_TYPE(type) == BNEP_CONTROL 173 || pkt->chan->state != CHANNEL_OPEN) 174 return false; /* no forwarding */ 175 176 return true; 177 } 178 179 static bool 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 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 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 cut them off */ 297 channel_close(chan); 298 299 return 1; 300 } 301 302 static size_t 303 bnep_recv_setup_connection_req(channel_t *chan, uint8_t *ptr, size_t size) 304 { 305 uint8_t off; 306 int src, dst, rsp; 307 size_t len; 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 return (len * 2 + 1); 374 } 375 376 static size_t 377 bnep_recv_setup_connection_rsp(channel_t *chan, uint8_t *ptr, size_t size) 378 { 379 int rsp; 380 381 if (size < 2) 382 return 0; 383 384 rsp = be16dec(ptr); 385 386 if (chan->state != CHANNEL_WAIT_CONNECT_RSP) { 387 log_debug("ignored"); 388 return 2; 389 } 390 391 log_debug("addr %s response 0x%2.2x", 392 ether_ntoa((struct ether_addr *)chan->raddr), rsp); 393 394 if (rsp == BNEP_SETUP_SUCCESS) { 395 chan->state = CHANNEL_OPEN; 396 channel_timeout(chan, 0); 397 } else { 398 channel_close(chan); 399 } 400 401 return 2; 402 } 403 404 static size_t 405 bnep_recv_filter_net_type_set(channel_t *chan, uint8_t *ptr, size_t size) 406 { 407 pfilter_t *pf; 408 int i, nf, rsp; 409 size_t len; 410 411 if (size < 2) 412 return 0; 413 414 len = be16dec(ptr); 415 ptr += 2; 416 417 if (size < (len + 2)) 418 return 0; 419 420 if (chan->state != CHANNEL_OPEN) { 421 log_debug("ignored"); 422 return (len + 2); 423 } 424 425 nf = len / 4; 426 pf = malloc(nf * sizeof(pfilter_t)); 427 if (pf == NULL) { 428 rsp = BNEP_FILTER_TOO_MANY_FILTERS; 429 goto done; 430 } 431 432 log_debug("nf = %d", nf); 433 434 for (i = 0; i < nf; i++) { 435 pf[i].start = be16dec(ptr); 436 ptr += 2; 437 pf[i].end = be16dec(ptr); 438 ptr += 2; 439 440 if (pf[i].start > pf[i].end) { 441 free(pf); 442 rsp = BNEP_FILTER_INVALID_RANGE; 443 goto done; 444 } 445 446 log_debug("pf[%d] = %#4.4x, %#4.4x", i, pf[i].start, pf[i].end); 447 } 448 449 if (chan->pfilter) 450 free(chan->pfilter); 451 452 chan->pfilter = pf; 453 chan->npfilter = nf; 454 455 rsp = BNEP_FILTER_SUCCESS; 456 457 done: 458 log_debug("addr %s response 0x%2.2x", 459 ether_ntoa((struct ether_addr *)chan->raddr), rsp); 460 461 bnep_send_control(chan, BNEP_FILTER_NET_TYPE_RESPONSE, rsp); 462 return (len + 2); 463 } 464 465 static size_t 466 bnep_recv_filter_net_type_rsp(channel_t *chan, uint8_t *ptr, size_t size) 467 { 468 int rsp; 469 470 if (size < 2) 471 return 0; 472 473 if (chan->state != CHANNEL_OPEN) { 474 log_debug("ignored"); 475 return 2; 476 } 477 478 rsp = be16dec(ptr); 479 480 log_debug("addr %s response 0x%2.2x", 481 ether_ntoa((struct ether_addr *)chan->raddr), rsp); 482 483 /* we did not send any filter_net_type_set message */ 484 return 2; 485 } 486 487 static size_t 488 bnep_recv_filter_multi_addr_set(channel_t *chan, uint8_t *ptr, size_t size) 489 { 490 mfilter_t *mf; 491 int i, nf, rsp; 492 size_t len; 493 494 if (size < 2) 495 return 0; 496 497 len = be16dec(ptr); 498 ptr += 2; 499 500 if (size < (len + 2)) 501 return 0; 502 503 if (chan->state != CHANNEL_OPEN) { 504 log_debug("ignored"); 505 return (len + 2); 506 } 507 508 nf = len / (ETHER_ADDR_LEN * 2); 509 mf = malloc(nf * sizeof(mfilter_t)); 510 if (mf == NULL) { 511 rsp = BNEP_FILTER_TOO_MANY_FILTERS; 512 goto done; 513 } 514 515 log_debug("nf = %d", nf); 516 517 for (i = 0; i < nf; i++) { 518 memcpy(mf[i].start, ptr, ETHER_ADDR_LEN); 519 ptr += ETHER_ADDR_LEN; 520 521 memcpy(mf[i].end, ptr, ETHER_ADDR_LEN); 522 ptr += ETHER_ADDR_LEN; 523 524 if (memcmp(mf[i].start, mf[i].end, ETHER_ADDR_LEN) > 0) { 525 free(mf); 526 rsp = BNEP_FILTER_INVALID_RANGE; 527 goto done; 528 } 529 530 log_debug("pf[%d] = " 531 "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x, " 532 "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x", i, 533 mf[i].start[0], mf[i].start[1], mf[i].start[2], 534 mf[i].start[3], mf[i].start[4], mf[i].start[5], 535 mf[i].end[0], mf[i].end[1], mf[i].end[2], 536 mf[i].end[3], mf[i].end[4], mf[i].end[5]); 537 } 538 539 if (chan->mfilter) 540 free(chan->mfilter); 541 542 chan->mfilter = mf; 543 chan->nmfilter = nf; 544 545 rsp = BNEP_FILTER_SUCCESS; 546 547 done: 548 log_debug("addr %s response 0x%2.2x", 549 ether_ntoa((struct ether_addr *)chan->raddr), rsp); 550 551 bnep_send_control(chan, BNEP_FILTER_MULTI_ADDR_RESPONSE, rsp); 552 return (len + 2); 553 } 554 555 static size_t 556 bnep_recv_filter_multi_addr_rsp(channel_t *chan, uint8_t *ptr, size_t size) 557 { 558 int rsp; 559 560 if (size < 2) 561 return false; 562 563 if (chan->state != CHANNEL_OPEN) { 564 log_debug("ignored"); 565 return 2; 566 } 567 568 rsp = be16dec(ptr); 569 log_debug("addr %s response 0x%2.2x", 570 ether_ntoa((struct ether_addr *)chan->raddr), rsp); 571 572 /* we did not send any filter_multi_addr_set message */ 573 return 2; 574 } 575 576 void 577 bnep_send_control(channel_t *chan, uint8_t type, ...) 578 { 579 packet_t *pkt; 580 uint8_t *p; 581 va_list ap; 582 583 assert(chan->state != CHANNEL_CLOSED); 584 585 pkt = packet_alloc(chan); 586 if (pkt == NULL) 587 return; 588 589 p = pkt->ptr; 590 va_start(ap, type); 591 592 *p++ = BNEP_CONTROL; 593 *p++ = type; 594 595 switch(type) { 596 case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD: 597 *p++ = va_arg(ap, int); 598 break; 599 600 case BNEP_SETUP_CONNECTION_REQUEST: 601 *p++ = va_arg(ap, int); 602 be16enc(p, va_arg(ap, int)); 603 p += 2; 604 be16enc(p, va_arg(ap, int)); 605 p += 2; 606 break; 607 608 case BNEP_SETUP_CONNECTION_RESPONSE: 609 case BNEP_FILTER_NET_TYPE_RESPONSE: 610 case BNEP_FILTER_MULTI_ADDR_RESPONSE: 611 be16enc(p, va_arg(ap, int)); 612 p += 2; 613 break; 614 615 case BNEP_FILTER_NET_TYPE_SET: /* TODO */ 616 case BNEP_FILTER_MULTI_ADDR_SET: /* TODO */ 617 default: 618 log_err("Can't send control type 0x%2.2x", type); 619 break; 620 } 621 622 va_end(ap); 623 pkt->len = p - pkt->ptr; 624 625 channel_put(chan, pkt); 626 packet_free(pkt); 627 } 628 629 /* 630 * BNEP send packet routine 631 * return true if packet can be removed from queue 632 */ 633 bool 634 bnep_send(channel_t *chan, packet_t *pkt) 635 { 636 struct iovec iov[2]; 637 uint8_t *p, *type, *proto; 638 exthdr_t *eh; 639 bool src, dst; 640 size_t nw; 641 642 if (pkt->type == NULL) { 643 iov[0].iov_base = pkt->ptr; 644 iov[0].iov_len = pkt->len; 645 iov[1].iov_base = NULL; 646 iov[1].iov_len = 0; 647 } else { 648 p = chan->sendbuf; 649 650 dst = (memcmp(pkt->dst, chan->raddr, ETHER_ADDR_LEN) != 0); 651 src = (memcmp(pkt->src, chan->laddr, ETHER_ADDR_LEN) != 0); 652 653 type = p; 654 p += 1; 655 656 if (dst && src) 657 *type = BNEP_GENERAL_ETHERNET; 658 else if (dst && !src) 659 *type = BNEP_COMPRESSED_ETHERNET_DST_ONLY; 660 else if (!dst && src) 661 *type = BNEP_COMPRESSED_ETHERNET_SRC_ONLY; 662 else /* (!dst && !src) */ 663 *type = BNEP_COMPRESSED_ETHERNET; 664 665 if (dst) { 666 memcpy(p, pkt->dst, ETHER_ADDR_LEN); 667 p += ETHER_ADDR_LEN; 668 } 669 670 if (src) { 671 memcpy(p, pkt->src, ETHER_ADDR_LEN); 672 p += ETHER_ADDR_LEN; 673 } 674 675 proto = p; 676 memcpy(p, pkt->type, ETHER_TYPE_LEN); 677 p += ETHER_TYPE_LEN; 678 679 STAILQ_FOREACH(eh, &pkt->extlist, next) { 680 if (p + eh->len > chan->sendbuf + chan->mtu) 681 break; 682 683 *type |= BNEP_EXT; 684 type = p; 685 686 memcpy(p, eh->ptr, eh->len); 687 p += eh->len; 688 } 689 690 *type &= ~BNEP_EXT; 691 692 iov[0].iov_base = chan->sendbuf; 693 iov[0].iov_len = (p - chan->sendbuf); 694 695 if ((chan->npfilter == 0 || bnep_pfilter(chan, pkt)) 696 && (chan->nmfilter == 0 || bnep_mfilter(chan, pkt))) { 697 iov[1].iov_base = pkt->ptr; 698 iov[1].iov_len = pkt->len; 699 } else if (be16dec(proto) == ETHERTYPE_VLAN 700 && pkt->len >= ETHER_VLAN_ENCAP_LEN) { 701 iov[1].iov_base = pkt->ptr; 702 iov[1].iov_len = ETHER_VLAN_ENCAP_LEN; 703 } else { 704 iov[1].iov_base = NULL; 705 iov[1].iov_len = 0; 706 memset(proto, 0, ETHER_TYPE_LEN); 707 } 708 } 709 710 if (iov[0].iov_len + iov[1].iov_len > chan->mtu) { 711 log_err("packet exceeded MTU (dropped)"); 712 return false; 713 } 714 715 nw = writev(chan->fd, iov, __arraycount(iov)); 716 return (nw > 0); 717 } 718 719 static bool 720 bnep_pfilter(channel_t *chan, packet_t *pkt) 721 { 722 int proto, i; 723 724 proto = be16dec(pkt->type); 725 if (proto == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */ 726 if (pkt->len < 4) 727 return false; 728 729 proto = be16dec(pkt->ptr + 2); 730 } 731 732 for (i = 0; i < chan->npfilter; i++) { 733 if (chan->pfilter[i].start <= proto 734 && chan->pfilter[i].end >=proto) 735 return true; 736 } 737 738 return false; 739 } 740 741 static bool 742 bnep_mfilter(channel_t *chan, packet_t *pkt) 743 { 744 int i; 745 746 if (!ETHER_IS_MULTICAST(pkt->dst)) 747 return true; 748 749 for (i = 0; i < chan->nmfilter; i++) { 750 if (memcmp(pkt->dst, chan->mfilter[i].start, ETHER_ADDR_LEN) >= 0 751 && memcmp(pkt->dst, chan->mfilter[i].end, ETHER_ADDR_LEN) <= 0) 752 return true; 753 } 754 755 return false; 756 } 757