usr.sbin/btpand/bnep.c

   1 /*      $NetBSD: bnep.c,v 1.7 2009/05/12 19:57:59 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 #include <sys/cdefs.h>
  29 __RCSID("$NetBSD: bnep.c,v 1.7 2009/05/12 19:57:59 plunky 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 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 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 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         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                 chan->down(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         if (rsp != BNEP_FILTER_SUCCESS)
 480                 log_err("filter_net_type: 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         if (rsp != BNEP_FILTER_SUCCESS)
 570                 log_err("filter_multi_addr: 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, uint8_t 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++ = 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 }