usr.sbin/bluetooth/btpand/bnep.c

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