usr.sbin/bluetooth/btpand/bnep.c

   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 }