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