1187938Semax/* $NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $ */ 2187938Semax 3187938Semax/*- 4330449Seadler * SPDX-License-Identifier: BSD-2-Clause-NetBSD 5330449Seadler * 6187938Semax * Copyright (c) 2008 Iain Hibbert 7187938Semax * All rights reserved. 8187938Semax * 9187938Semax * Redistribution and use in source and binary forms, with or without 10187938Semax * modification, are permitted provided that the following conditions 11187938Semax * are met: 12187938Semax * 1. Redistributions of source code must retain the above copyright 13187938Semax * notice, this list of conditions and the following disclaimer. 14187938Semax * 2. Redistributions in binary form must reproduce the above copyright 15187938Semax * notice, this list of conditions and the following disclaimer in the 16187938Semax * documentation and/or other materials provided with the distribution. 17187938Semax * 18187938Semax * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 19187938Semax * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 20187938Semax * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 21187938Semax * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 22187938Semax * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 23187938Semax * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24187938Semax * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 25187938Semax * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26187938Semax * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27187938Semax * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28187938Semax */ 29187938Semax 30187938Semax/* $FreeBSD: stable/11/usr.sbin/bluetooth/btpand/bnep.c 330449 2018-03-05 07:26:05Z eadler $ */ 31187938Semax 32187938Semax#include <sys/cdefs.h> 33187938Semax__RCSID("$NetBSD: bnep.c,v 1.1 2008/08/17 13:20:57 plunky Exp $"); 34187938Semax 35187938Semax#include <sys/uio.h> 36281210Stakawata#define L2CAP_SOCKET_CHECKED 37187938Semax#include <bluetooth.h> 38187938Semax#include <sdp.h> 39187938Semax#include <stdarg.h> 40187938Semax#include <string.h> 41187938Semax#include <unistd.h> 42187938Semax 43187938Semax#include "btpand.h" 44187938Semax#include "bnep.h" 45187938Semax 46187938Semaxstatic bool bnep_recv_extension(packet_t *); 47187938Semaxstatic size_t bnep_recv_control(channel_t *, uint8_t *, size_t, bool); 48187938Semaxstatic size_t bnep_recv_control_command_not_understood(channel_t *, uint8_t *, size_t); 49187938Semaxstatic size_t bnep_recv_setup_connection_req(channel_t *, uint8_t *, size_t); 50187938Semaxstatic size_t bnep_recv_setup_connection_rsp(channel_t *, uint8_t *, size_t); 51187938Semaxstatic size_t bnep_recv_filter_net_type_set(channel_t *, uint8_t *, size_t); 52187938Semaxstatic size_t bnep_recv_filter_net_type_rsp(channel_t *, uint8_t *, size_t); 53187938Semaxstatic size_t bnep_recv_filter_multi_addr_set(channel_t *, uint8_t *, size_t); 54187938Semaxstatic size_t bnep_recv_filter_multi_addr_rsp(channel_t *, uint8_t *, size_t); 55187938Semax 56187938Semaxstatic bool bnep_pfilter(channel_t *, packet_t *); 57187938Semaxstatic bool bnep_mfilter(channel_t *, packet_t *); 58187938Semax 59187938Semaxstatic uint8_t NAP_UUID[] = { 60187938Semax 0x00, 0x00, 0x11, 0x16, 61187938Semax 0x00, 0x00, 62187938Semax 0x10, 0x00, 63187938Semax 0x80, 0x00, 64187938Semax 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb 65187938Semax}; 66187938Semax 67187938Semaxstatic uint8_t GN_UUID[] = { 68187938Semax 0x00, 0x00, 0x11, 0x17, 69187938Semax 0x00, 0x00, 70187938Semax 0x10, 0x00, 71187938Semax 0x80, 0x00, 72187938Semax 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb, 73187938Semax}; 74187938Semax 75187938Semaxstatic uint8_t PANU_UUID[] = { 76187938Semax 0x00, 0x00, 0x11, 0x15, 77187938Semax 0x00, 0x00, 78187938Semax 0x10, 0x00, 79187938Semax 0x80, 0x00, 80187938Semax 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb 81187938Semax}; 82187938Semax 83187938Semax/* 84187938Semax * receive BNEP packet 85187938Semax * return true if packet is to be forwarded 86187938Semax */ 87187938Semaxbool 88187938Semaxbnep_recv(packet_t *pkt) 89187938Semax{ 90187938Semax size_t len; 91187938Semax uint8_t type; 92187938Semax 93187938Semax if (pkt->len < 1) 94187938Semax return false; 95187938Semax 96187938Semax type = pkt->ptr[0]; 97187938Semax packet_adj(pkt, 1); 98187938Semax 99187938Semax switch (BNEP_TYPE(type)) { 100187938Semax case BNEP_GENERAL_ETHERNET: 101187938Semax if (pkt->len < (ETHER_ADDR_LEN * 2) + ETHER_TYPE_LEN) { 102187938Semax log_debug("dropped short packet (type 0x%2.2x)", type); 103187938Semax return false; 104187938Semax } 105187938Semax 106187938Semax pkt->dst = pkt->ptr; 107187938Semax packet_adj(pkt, ETHER_ADDR_LEN); 108187938Semax pkt->src = pkt->ptr; 109187938Semax packet_adj(pkt, ETHER_ADDR_LEN); 110187938Semax pkt->type = pkt->ptr; 111187938Semax packet_adj(pkt, ETHER_TYPE_LEN); 112187938Semax break; 113187938Semax 114187938Semax case BNEP_CONTROL: 115187938Semax len = bnep_recv_control(pkt->chan, pkt->ptr, pkt->len, false); 116187938Semax if (len == 0) 117187938Semax return false; 118187938Semax 119187938Semax packet_adj(pkt, len); 120187938Semax break; 121187938Semax 122187938Semax case BNEP_COMPRESSED_ETHERNET: 123187938Semax if (pkt->len < ETHER_TYPE_LEN) { 124187938Semax log_debug("dropped short packet (type 0x%2.2x)", type); 125187938Semax return false; 126187938Semax } 127187938Semax 128187938Semax pkt->dst = pkt->chan->laddr; 129187938Semax pkt->src = pkt->chan->raddr; 130187938Semax pkt->type = pkt->ptr; 131187938Semax packet_adj(pkt, ETHER_TYPE_LEN); 132187938Semax break; 133187938Semax 134187938Semax case BNEP_COMPRESSED_ETHERNET_SRC_ONLY: 135187938Semax if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) { 136187938Semax log_debug("dropped short packet (type 0x%2.2x)", type); 137187938Semax return false; 138187938Semax } 139187938Semax 140187938Semax pkt->dst = pkt->chan->laddr; 141187938Semax pkt->src = pkt->ptr; 142187938Semax packet_adj(pkt, ETHER_ADDR_LEN); 143187938Semax pkt->type = pkt->ptr; 144187938Semax packet_adj(pkt, ETHER_TYPE_LEN); 145187938Semax break; 146187938Semax 147187938Semax case BNEP_COMPRESSED_ETHERNET_DST_ONLY: 148187938Semax if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) { 149187938Semax log_debug("dropped short packet (type 0x%2.2x)", type); 150187938Semax return false; 151187938Semax } 152187938Semax 153187938Semax pkt->dst = pkt->ptr; 154187938Semax packet_adj(pkt, ETHER_ADDR_LEN); 155187938Semax pkt->src = pkt->chan->raddr; 156187938Semax pkt->type = pkt->ptr; 157187938Semax packet_adj(pkt, ETHER_TYPE_LEN); 158187938Semax break; 159187938Semax 160187938Semax default: 161187938Semax /* 162187938Semax * Any packet containing a reserved BNEP 163187938Semax * header packet type SHALL be dropped. 164187938Semax */ 165187938Semax 166187938Semax log_debug("dropped packet with reserved type 0x%2.2x", type); 167187938Semax return false; 168187938Semax } 169187938Semax 170187938Semax if (BNEP_TYPE_EXT(type) 171187938Semax && !bnep_recv_extension(pkt)) 172187938Semax return false; /* invalid extensions */ 173187938Semax 174187938Semax if (BNEP_TYPE(type) == BNEP_CONTROL 175187938Semax || pkt->chan->state != CHANNEL_OPEN) 176187938Semax return false; /* no forwarding */ 177187938Semax 178187938Semax return true; 179187938Semax} 180187938Semax 181187938Semaxstatic bool 182187938Semaxbnep_recv_extension(packet_t *pkt) 183187938Semax{ 184187938Semax exthdr_t *eh; 185187938Semax size_t len, size; 186187938Semax uint8_t type; 187187938Semax 188187938Semax do { 189187938Semax if (pkt->len < 2) 190187938Semax return false; 191187938Semax 192187938Semax type = pkt->ptr[0]; 193187938Semax size = pkt->ptr[1]; 194187938Semax 195187938Semax if (pkt->len < size + 2) 196187938Semax return false; 197187938Semax 198187938Semax switch (type) { 199187938Semax case BNEP_EXTENSION_CONTROL: 200187938Semax len = bnep_recv_control(pkt->chan, pkt->ptr + 2, size, true); 201187938Semax if (len != size) 202187938Semax log_err("ignored spurious data in exthdr"); 203187938Semax 204187938Semax break; 205187938Semax 206187938Semax default: 207187938Semax /* Unknown extension headers in data packets */ 208187938Semax /* SHALL be forwarded irrespective of any */ 209187938Semax /* network protocol or multicast filter settings */ 210187938Semax /* and any local filtering policy. */ 211187938Semax 212187938Semax eh = malloc(sizeof(exthdr_t)); 213187938Semax if (eh == NULL) { 214187938Semax log_err("exthdr malloc() failed: %m"); 215187938Semax break; 216187938Semax } 217187938Semax 218187938Semax eh->ptr = pkt->ptr; 219187938Semax eh->len = size; 220187938Semax STAILQ_INSERT_TAIL(&pkt->extlist, eh, next); 221187938Semax break; 222187938Semax } 223187938Semax 224187938Semax packet_adj(pkt, size + 2); 225187938Semax } while (BNEP_TYPE_EXT(type)); 226187938Semax 227187938Semax return true; 228187938Semax} 229187938Semax 230187938Semaxstatic size_t 231187938Semaxbnep_recv_control(channel_t *chan, uint8_t *ptr, size_t size, bool isext) 232187938Semax{ 233187938Semax uint8_t type; 234187938Semax size_t len; 235187938Semax 236187938Semax if (size-- < 1) 237187938Semax return 0; 238187938Semax 239187938Semax type = *ptr++; 240187938Semax 241187938Semax switch (type) { 242187938Semax case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD: 243187938Semax len = bnep_recv_control_command_not_understood(chan, ptr, size); 244187938Semax break; 245187938Semax 246187938Semax case BNEP_SETUP_CONNECTION_REQUEST: 247187938Semax if (isext) 248187938Semax return 0; /* not allowed in extension headers */ 249187938Semax 250187938Semax len = bnep_recv_setup_connection_req(chan, ptr, size); 251187938Semax break; 252187938Semax 253187938Semax case BNEP_SETUP_CONNECTION_RESPONSE: 254187938Semax if (isext) 255187938Semax return 0; /* not allowed in extension headers */ 256187938Semax 257187938Semax len = bnep_recv_setup_connection_rsp(chan, ptr, size); 258187938Semax break; 259187938Semax 260187938Semax case BNEP_FILTER_NET_TYPE_SET: 261187938Semax len = bnep_recv_filter_net_type_set(chan, ptr, size); 262187938Semax break; 263187938Semax 264187938Semax case BNEP_FILTER_NET_TYPE_RESPONSE: 265187938Semax len = bnep_recv_filter_net_type_rsp(chan, ptr, size); 266187938Semax break; 267187938Semax 268187938Semax case BNEP_FILTER_MULTI_ADDR_SET: 269187938Semax len = bnep_recv_filter_multi_addr_set(chan, ptr, size); 270187938Semax break; 271187938Semax 272187938Semax case BNEP_FILTER_MULTI_ADDR_RESPONSE: 273187938Semax len = bnep_recv_filter_multi_addr_rsp(chan, ptr, size); 274187938Semax break; 275187938Semax 276187938Semax default: 277187938Semax len = 0; 278187938Semax break; 279187938Semax } 280187938Semax 281187938Semax if (len == 0) 282187938Semax bnep_send_control(chan, BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD, type); 283187938Semax 284187938Semax return len; 285187938Semax} 286187938Semax 287187938Semaxstatic size_t 288187938Semaxbnep_recv_control_command_not_understood(channel_t *chan, uint8_t *ptr, size_t size) 289187938Semax{ 290187938Semax uint8_t type; 291187938Semax 292187938Semax if (size < 1) 293187938Semax return 0; 294187938Semax 295187938Semax type = *ptr++; 296187938Semax log_err("received Control Command Not Understood (0x%2.2x)", type); 297187938Semax 298187938Semax /* we didn't send any reserved commands, just cut them off */ 299187938Semax channel_close(chan); 300187938Semax 301187938Semax return 1; 302187938Semax} 303187938Semax 304187938Semaxstatic size_t 305187938Semaxbnep_recv_setup_connection_req(channel_t *chan, uint8_t *ptr, size_t size) 306187938Semax{ 307187938Semax uint8_t off; 308187938Semax int src, dst, rsp; 309187938Semax size_t len; 310187938Semax 311187938Semax if (size < 1) 312187938Semax return 0; 313187938Semax 314187938Semax len = *ptr++; 315187938Semax if (size < (len * 2 + 1)) 316187938Semax return 0; 317187938Semax 318187938Semax if (chan->state != CHANNEL_WAIT_CONNECT_REQ 319187938Semax && chan->state != CHANNEL_OPEN) { 320187938Semax log_debug("ignored"); 321187938Semax return (len * 2 + 1); 322187938Semax } 323187938Semax 324187938Semax if (len == 2) 325187938Semax off = 2; 326187938Semax else if (len == 4) 327187938Semax off = 0; 328187938Semax else if (len == 16) 329187938Semax off = 0; 330187938Semax else { 331187938Semax rsp = BNEP_SETUP_INVALID_UUID_SIZE; 332187938Semax goto done; 333187938Semax } 334187938Semax 335187938Semax if (memcmp(ptr, NAP_UUID + off, len) == 0) 336187938Semax dst = SDP_SERVICE_CLASS_NAP; 337187938Semax else if (memcmp(ptr, GN_UUID + off, len) == 0) 338187938Semax dst = SDP_SERVICE_CLASS_GN; 339187938Semax else if (memcmp(ptr, PANU_UUID + off, len) == 0) 340187938Semax dst = SDP_SERVICE_CLASS_PANU; 341187938Semax else 342187938Semax dst = 0; 343187938Semax 344187938Semax if (dst != service_class) { 345187938Semax rsp = BNEP_SETUP_INVALID_DST_UUID; 346187938Semax goto done; 347187938Semax } 348187938Semax 349187938Semax ptr += len; 350187938Semax 351187938Semax if (memcmp(ptr, NAP_UUID + off, len) == 0) 352187938Semax src = SDP_SERVICE_CLASS_NAP; 353187938Semax else if (memcmp(ptr, GN_UUID + off, len) == 0) 354187938Semax src = SDP_SERVICE_CLASS_GN; 355187938Semax else if (memcmp(ptr, PANU_UUID + off, len) == 0) 356187938Semax src = SDP_SERVICE_CLASS_PANU; 357187938Semax else 358187938Semax src = 0; 359187938Semax 360187938Semax if ((dst != SDP_SERVICE_CLASS_PANU && src != SDP_SERVICE_CLASS_PANU) 361187938Semax || src == 0) { 362187938Semax rsp = BNEP_SETUP_INVALID_SRC_UUID; 363187938Semax goto done; 364187938Semax } 365187938Semax 366187938Semax rsp = BNEP_SETUP_SUCCESS; 367187938Semax chan->state = CHANNEL_OPEN; 368187938Semax channel_timeout(chan, 0); 369187938Semax 370187938Semaxdone: 371187938Semax log_debug("addr %s response 0x%2.2x", 372187938Semax ether_ntoa((struct ether_addr *)chan->raddr), rsp); 373187938Semax 374187938Semax bnep_send_control(chan, BNEP_SETUP_CONNECTION_RESPONSE, rsp); 375187938Semax return (len * 2 + 1); 376187938Semax} 377187938Semax 378187938Semaxstatic size_t 379187938Semaxbnep_recv_setup_connection_rsp(channel_t *chan, uint8_t *ptr, size_t size) 380187938Semax{ 381187938Semax int rsp; 382187938Semax 383187938Semax if (size < 2) 384187938Semax return 0; 385187938Semax 386187938Semax rsp = be16dec(ptr); 387187938Semax 388187938Semax if (chan->state != CHANNEL_WAIT_CONNECT_RSP) { 389187938Semax log_debug("ignored"); 390187938Semax return 2; 391187938Semax } 392187938Semax 393187938Semax log_debug("addr %s response 0x%2.2x", 394187938Semax ether_ntoa((struct ether_addr *)chan->raddr), rsp); 395187938Semax 396187938Semax if (rsp == BNEP_SETUP_SUCCESS) { 397187938Semax chan->state = CHANNEL_OPEN; 398187938Semax channel_timeout(chan, 0); 399187938Semax } else { 400187938Semax channel_close(chan); 401187938Semax } 402187938Semax 403187938Semax return 2; 404187938Semax} 405187938Semax 406187938Semaxstatic size_t 407187938Semaxbnep_recv_filter_net_type_set(channel_t *chan, uint8_t *ptr, size_t size) 408187938Semax{ 409187938Semax pfilter_t *pf; 410187938Semax int i, nf, rsp; 411187938Semax size_t len; 412187938Semax 413187938Semax if (size < 2) 414187938Semax return 0; 415187938Semax 416187938Semax len = be16dec(ptr); 417187938Semax ptr += 2; 418187938Semax 419187938Semax if (size < (len + 2)) 420187938Semax return 0; 421187938Semax 422187938Semax if (chan->state != CHANNEL_OPEN) { 423187938Semax log_debug("ignored"); 424187938Semax return (len + 2); 425187938Semax } 426187938Semax 427187938Semax nf = len / 4; 428187938Semax pf = malloc(nf * sizeof(pfilter_t)); 429187938Semax if (pf == NULL) { 430187938Semax rsp = BNEP_FILTER_TOO_MANY_FILTERS; 431187938Semax goto done; 432187938Semax } 433187938Semax 434187938Semax log_debug("nf = %d", nf); 435187938Semax 436187938Semax for (i = 0; i < nf; i++) { 437187938Semax pf[i].start = be16dec(ptr); 438187938Semax ptr += 2; 439187938Semax pf[i].end = be16dec(ptr); 440187938Semax ptr += 2; 441187938Semax 442187938Semax if (pf[i].start > pf[i].end) { 443187938Semax free(pf); 444187938Semax rsp = BNEP_FILTER_INVALID_RANGE; 445187938Semax goto done; 446187938Semax } 447187938Semax 448187938Semax log_debug("pf[%d] = %#4.4x, %#4.4x", i, pf[i].start, pf[i].end); 449187938Semax } 450187938Semax 451187938Semax if (chan->pfilter) 452187938Semax free(chan->pfilter); 453187938Semax 454187938Semax chan->pfilter = pf; 455187938Semax chan->npfilter = nf; 456187938Semax 457187938Semax rsp = BNEP_FILTER_SUCCESS; 458187938Semax 459187938Semaxdone: 460187938Semax log_debug("addr %s response 0x%2.2x", 461187938Semax ether_ntoa((struct ether_addr *)chan->raddr), rsp); 462187938Semax 463187938Semax bnep_send_control(chan, BNEP_FILTER_NET_TYPE_RESPONSE, rsp); 464187938Semax return (len + 2); 465187938Semax} 466187938Semax 467187938Semaxstatic size_t 468187938Semaxbnep_recv_filter_net_type_rsp(channel_t *chan, uint8_t *ptr, size_t size) 469187938Semax{ 470187938Semax int rsp; 471187938Semax 472187938Semax if (size < 2) 473187938Semax return 0; 474187938Semax 475187938Semax if (chan->state != CHANNEL_OPEN) { 476187938Semax log_debug("ignored"); 477187938Semax return 2; 478187938Semax } 479187938Semax 480187938Semax rsp = be16dec(ptr); 481187938Semax 482187938Semax log_debug("addr %s response 0x%2.2x", 483187938Semax ether_ntoa((struct ether_addr *)chan->raddr), rsp); 484187938Semax 485187938Semax /* we did not send any filter_net_type_set message */ 486187938Semax return 2; 487187938Semax} 488187938Semax 489187938Semaxstatic size_t 490187938Semaxbnep_recv_filter_multi_addr_set(channel_t *chan, uint8_t *ptr, size_t size) 491187938Semax{ 492187938Semax mfilter_t *mf; 493187938Semax int i, nf, rsp; 494187938Semax size_t len; 495187938Semax 496187938Semax if (size < 2) 497187938Semax return 0; 498187938Semax 499187938Semax len = be16dec(ptr); 500187938Semax ptr += 2; 501187938Semax 502187938Semax if (size < (len + 2)) 503187938Semax return 0; 504187938Semax 505187938Semax if (chan->state != CHANNEL_OPEN) { 506187938Semax log_debug("ignored"); 507187938Semax return (len + 2); 508187938Semax } 509187938Semax 510187938Semax nf = len / (ETHER_ADDR_LEN * 2); 511187938Semax mf = malloc(nf * sizeof(mfilter_t)); 512187938Semax if (mf == NULL) { 513187938Semax rsp = BNEP_FILTER_TOO_MANY_FILTERS; 514187938Semax goto done; 515187938Semax } 516187938Semax 517187938Semax log_debug("nf = %d", nf); 518187938Semax 519187938Semax for (i = 0; i < nf; i++) { 520187938Semax memcpy(mf[i].start, ptr, ETHER_ADDR_LEN); 521187938Semax ptr += ETHER_ADDR_LEN; 522187938Semax 523187938Semax memcpy(mf[i].end, ptr, ETHER_ADDR_LEN); 524187938Semax ptr += ETHER_ADDR_LEN; 525187938Semax 526187938Semax if (memcmp(mf[i].start, mf[i].end, ETHER_ADDR_LEN) > 0) { 527187938Semax free(mf); 528187938Semax rsp = BNEP_FILTER_INVALID_RANGE; 529187938Semax goto done; 530187938Semax } 531187938Semax 532187938Semax log_debug("pf[%d] = " 533187938Semax "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x, " 534187938Semax "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x", i, 535187938Semax mf[i].start[0], mf[i].start[1], mf[i].start[2], 536187938Semax mf[i].start[3], mf[i].start[4], mf[i].start[5], 537187938Semax mf[i].end[0], mf[i].end[1], mf[i].end[2], 538187938Semax mf[i].end[3], mf[i].end[4], mf[i].end[5]); 539187938Semax } 540187938Semax 541187938Semax if (chan->mfilter) 542187938Semax free(chan->mfilter); 543187938Semax 544187938Semax chan->mfilter = mf; 545187938Semax chan->nmfilter = nf; 546187938Semax 547187938Semax rsp = BNEP_FILTER_SUCCESS; 548187938Semax 549187938Semaxdone: 550187938Semax log_debug("addr %s response 0x%2.2x", 551187938Semax ether_ntoa((struct ether_addr *)chan->raddr), rsp); 552187938Semax 553187938Semax bnep_send_control(chan, BNEP_FILTER_MULTI_ADDR_RESPONSE, rsp); 554187938Semax return (len + 2); 555187938Semax} 556187938Semax 557187938Semaxstatic size_t 558187938Semaxbnep_recv_filter_multi_addr_rsp(channel_t *chan, uint8_t *ptr, size_t size) 559187938Semax{ 560187938Semax int rsp; 561187938Semax 562187938Semax if (size < 2) 563187938Semax return false; 564187938Semax 565187938Semax if (chan->state != CHANNEL_OPEN) { 566187938Semax log_debug("ignored"); 567187938Semax return 2; 568187938Semax } 569187938Semax 570187938Semax rsp = be16dec(ptr); 571187938Semax log_debug("addr %s response 0x%2.2x", 572187938Semax ether_ntoa((struct ether_addr *)chan->raddr), rsp); 573187938Semax 574187938Semax /* we did not send any filter_multi_addr_set message */ 575187938Semax return 2; 576187938Semax} 577187938Semax 578187938Semaxvoid 579305287Sdimbnep_send_control(channel_t *chan, unsigned type, ...) 580187938Semax{ 581187938Semax packet_t *pkt; 582187938Semax uint8_t *p; 583187938Semax va_list ap; 584187938Semax 585187938Semax assert(chan->state != CHANNEL_CLOSED); 586187938Semax 587187938Semax pkt = packet_alloc(chan); 588187938Semax if (pkt == NULL) 589187938Semax return; 590187938Semax 591187938Semax p = pkt->ptr; 592187938Semax va_start(ap, type); 593187938Semax 594187938Semax *p++ = BNEP_CONTROL; 595305287Sdim *p++ = (uint8_t)type; 596187938Semax 597187938Semax switch(type) { 598187938Semax case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD: 599187938Semax *p++ = va_arg(ap, int); 600187938Semax break; 601187938Semax 602187938Semax case BNEP_SETUP_CONNECTION_REQUEST: 603187938Semax *p++ = va_arg(ap, int); 604187938Semax be16enc(p, va_arg(ap, int)); 605187938Semax p += 2; 606187938Semax be16enc(p, va_arg(ap, int)); 607187938Semax p += 2; 608187938Semax break; 609187938Semax 610187938Semax case BNEP_SETUP_CONNECTION_RESPONSE: 611187938Semax case BNEP_FILTER_NET_TYPE_RESPONSE: 612187938Semax case BNEP_FILTER_MULTI_ADDR_RESPONSE: 613187938Semax be16enc(p, va_arg(ap, int)); 614187938Semax p += 2; 615187938Semax break; 616187938Semax 617187938Semax case BNEP_FILTER_NET_TYPE_SET: /* TODO */ 618187938Semax case BNEP_FILTER_MULTI_ADDR_SET: /* TODO */ 619187938Semax default: 620187938Semax log_err("Can't send control type 0x%2.2x", type); 621187938Semax break; 622187938Semax } 623187938Semax 624187938Semax va_end(ap); 625187938Semax pkt->len = p - pkt->ptr; 626187938Semax 627187938Semax channel_put(chan, pkt); 628187938Semax packet_free(pkt); 629187938Semax} 630187938Semax 631187938Semax/* 632187938Semax * BNEP send packet routine 633187938Semax * return true if packet can be removed from queue 634187938Semax */ 635187938Semaxbool 636187938Semaxbnep_send(channel_t *chan, packet_t *pkt) 637187938Semax{ 638187938Semax struct iovec iov[2]; 639187938Semax uint8_t *p, *type, *proto; 640187938Semax exthdr_t *eh; 641187938Semax bool src, dst; 642187938Semax size_t nw; 643187938Semax 644187938Semax if (pkt->type == NULL) { 645187938Semax iov[0].iov_base = pkt->ptr; 646187938Semax iov[0].iov_len = pkt->len; 647187938Semax iov[1].iov_base = NULL; 648187938Semax iov[1].iov_len = 0; 649187938Semax } else { 650187938Semax p = chan->sendbuf; 651187938Semax 652187938Semax dst = (memcmp(pkt->dst, chan->raddr, ETHER_ADDR_LEN) != 0); 653187938Semax src = (memcmp(pkt->src, chan->laddr, ETHER_ADDR_LEN) != 0); 654187938Semax 655187938Semax type = p; 656187938Semax p += 1; 657187938Semax 658187938Semax if (dst && src) 659187938Semax *type = BNEP_GENERAL_ETHERNET; 660187938Semax else if (dst && !src) 661187938Semax *type = BNEP_COMPRESSED_ETHERNET_DST_ONLY; 662187938Semax else if (!dst && src) 663187938Semax *type = BNEP_COMPRESSED_ETHERNET_SRC_ONLY; 664187938Semax else /* (!dst && !src) */ 665187938Semax *type = BNEP_COMPRESSED_ETHERNET; 666187938Semax 667187938Semax if (dst) { 668187938Semax memcpy(p, pkt->dst, ETHER_ADDR_LEN); 669187938Semax p += ETHER_ADDR_LEN; 670187938Semax } 671187938Semax 672187938Semax if (src) { 673187938Semax memcpy(p, pkt->src, ETHER_ADDR_LEN); 674187938Semax p += ETHER_ADDR_LEN; 675187938Semax } 676187938Semax 677187938Semax proto = p; 678187938Semax memcpy(p, pkt->type, ETHER_TYPE_LEN); 679187938Semax p += ETHER_TYPE_LEN; 680187938Semax 681187938Semax STAILQ_FOREACH(eh, &pkt->extlist, next) { 682187938Semax if (p + eh->len > chan->sendbuf + chan->mtu) 683187938Semax break; 684187938Semax 685187938Semax *type |= BNEP_EXT; 686187938Semax type = p; 687187938Semax 688187938Semax memcpy(p, eh->ptr, eh->len); 689187938Semax p += eh->len; 690187938Semax } 691187938Semax 692187938Semax *type &= ~BNEP_EXT; 693187938Semax 694187938Semax iov[0].iov_base = chan->sendbuf; 695187938Semax iov[0].iov_len = (p - chan->sendbuf); 696187938Semax 697187938Semax if ((chan->npfilter == 0 || bnep_pfilter(chan, pkt)) 698187938Semax && (chan->nmfilter == 0 || bnep_mfilter(chan, pkt))) { 699187938Semax iov[1].iov_base = pkt->ptr; 700187938Semax iov[1].iov_len = pkt->len; 701187938Semax } else if (be16dec(proto) == ETHERTYPE_VLAN 702187938Semax && pkt->len >= ETHER_VLAN_ENCAP_LEN) { 703187938Semax iov[1].iov_base = pkt->ptr; 704187938Semax iov[1].iov_len = ETHER_VLAN_ENCAP_LEN; 705187938Semax } else { 706187938Semax iov[1].iov_base = NULL; 707187938Semax iov[1].iov_len = 0; 708187938Semax memset(proto, 0, ETHER_TYPE_LEN); 709187938Semax } 710187938Semax } 711187938Semax 712187938Semax if (iov[0].iov_len + iov[1].iov_len > chan->mtu) { 713187938Semax log_err("packet exceeded MTU (dropped)"); 714187938Semax return false; 715187938Semax } 716187938Semax 717187938Semax nw = writev(chan->fd, iov, __arraycount(iov)); 718187938Semax return (nw > 0); 719187938Semax} 720187938Semax 721187938Semaxstatic bool 722187938Semaxbnep_pfilter(channel_t *chan, packet_t *pkt) 723187938Semax{ 724187938Semax int proto, i; 725187938Semax 726187938Semax proto = be16dec(pkt->type); 727187938Semax if (proto == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */ 728187938Semax if (pkt->len < 4) 729187938Semax return false; 730187938Semax 731187938Semax proto = be16dec(pkt->ptr + 2); 732187938Semax } 733187938Semax 734187938Semax for (i = 0; i < chan->npfilter; i++) { 735187938Semax if (chan->pfilter[i].start <= proto 736187938Semax && chan->pfilter[i].end >=proto) 737187938Semax return true; 738187938Semax } 739187938Semax 740187938Semax return false; 741187938Semax} 742187938Semax 743187938Semaxstatic bool 744187938Semaxbnep_mfilter(channel_t *chan, packet_t *pkt) 745187938Semax{ 746187938Semax int i; 747187938Semax 748187938Semax if (!ETHER_IS_MULTICAST(pkt->dst)) 749187938Semax return true; 750187938Semax 751187938Semax for (i = 0; i < chan->nmfilter; i++) { 752187938Semax if (memcmp(pkt->dst, chan->mfilter[i].start, ETHER_ADDR_LEN) >= 0 753187938Semax && memcmp(pkt->dst, chan->mfilter[i].end, ETHER_ADDR_LEN) <= 0) 754187938Semax return true; 755187938Semax } 756187938Semax 757187938Semax return false; 758187938Semax} 759