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 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 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 shut them down */ 297 chan->down(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 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 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 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 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 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 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 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 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 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 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
Indexes created Sun Sep 21 16:09:51 GMT 2025