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 #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, unsigned 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++ = (uint8_t)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 }