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