[mirror_ubuntu-zesty-kernel.git] / net / mac802154 / wpan.c

/*
 * Copyright 2007-2012 Siemens AG
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2
 * as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Written by:
 * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
 * Sergey Lapin <slapin@ossfans.org>
 * Maxim Gorbachyov <maxim.gorbachev@siemens.com>
 * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
 */

#include <linux/netdevice.h>
#include <linux/module.h>
#include <linux/if_arp.h>

#include <net/rtnetlink.h>
#include <linux/nl802154.h>
#include <net/af_ieee802154.h>
#include <net/mac802154.h>
#include <net/ieee802154_netdev.h>
#include <net/ieee802154.h>
#include <net/wpan-phy.h>

#include "mac802154.h"

static inline int mac802154_fetch_skb_u8(struct sk_buff *skb, u8 *val)
{
	if (unlikely(!pskb_may_pull(skb, 1)))
		return -EINVAL;

	*val = skb->data[0];
	skb_pull(skb, 1);

	return 0;
}

static inline int mac802154_fetch_skb_u16(struct sk_buff *skb, u16 *val)
{
	if (unlikely(!pskb_may_pull(skb, 2)))
		return -EINVAL;

	*val = skb->data[0] | (skb->data[1] << 8);
	skb_pull(skb, 2);

	return 0;
}

static inline void mac802154_haddr_copy_swap(u8 *dest, const u8 *src)
{
	int i;
	for (i = 0; i < IEEE802154_ADDR_LEN; i++)
		dest[IEEE802154_ADDR_LEN - i - 1] = src[i];
}

static int
mac802154_wpan_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
{
	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	struct sockaddr_ieee802154 *sa =
		(struct sockaddr_ieee802154 *)&ifr->ifr_addr;
	int err = -ENOIOCTLCMD;

	spin_lock_bh(&priv->mib_lock);

	switch (cmd) {
	case SIOCGIFADDR:
	{
		u16 pan_id, short_addr;

		pan_id = le16_to_cpu(priv->pan_id);
		short_addr = le16_to_cpu(priv->short_addr);
		if (pan_id == IEEE802154_PANID_BROADCAST ||
		    short_addr == IEEE802154_ADDR_BROADCAST) {
			err = -EADDRNOTAVAIL;
			break;
		}

		sa->family = AF_IEEE802154;
		sa->addr.addr_type = IEEE802154_ADDR_SHORT;
		sa->addr.pan_id = pan_id;
		sa->addr.short_addr = short_addr;

		err = 0;
		break;
	}
	case SIOCSIFADDR:
		dev_warn(&dev->dev,
			 "Using DEBUGing ioctl SIOCSIFADDR isn't recommened!\n");
		if (sa->family != AF_IEEE802154 ||
		    sa->addr.addr_type != IEEE802154_ADDR_SHORT ||
		    sa->addr.pan_id == IEEE802154_PANID_BROADCAST ||
		    sa->addr.short_addr == IEEE802154_ADDR_BROADCAST ||
		    sa->addr.short_addr == IEEE802154_ADDR_UNDEF) {
			err = -EINVAL;
			break;
		}

		priv->pan_id = cpu_to_le16(sa->addr.pan_id);
		priv->short_addr = cpu_to_le16(sa->addr.short_addr);

		err = 0;
		break;
	}

	spin_unlock_bh(&priv->mib_lock);
	return err;
}

static int mac802154_wpan_mac_addr(struct net_device *dev, void *p)
{
	struct sockaddr *addr = p;

	if (netif_running(dev))
		return -EBUSY;

	/* FIXME: validate addr */
	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
	mac802154_dev_set_ieee_addr(dev);
	return 0;
}

static int mac802154_header_create(struct sk_buff *skb,
				   struct net_device *dev,
				   unsigned short type,
				   const void *_daddr,
				   const void *_saddr,
				   unsigned len)
{
	const struct ieee802154_addr_sa *saddr = _saddr;
	const struct ieee802154_addr_sa *daddr = _daddr;
	struct ieee802154_addr_sa dev_addr;
	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	int pos = 2;
	u8 head[MAC802154_FRAME_HARD_HEADER_LEN];
	u16 fc;

	if (!daddr)
		return -EINVAL;

	head[pos++] = mac_cb(skb)->seq; /* DSN/BSN */
	fc = mac_cb_type(skb);
	if (mac_cb_is_ackreq(skb))
		fc |= IEEE802154_FC_ACK_REQ;

	if (!saddr) {
		spin_lock_bh(&priv->mib_lock);

		if (priv->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) ||
		    priv->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) ||
		    priv->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
			dev_addr.addr_type = IEEE802154_ADDR_LONG;
			memcpy(dev_addr.hwaddr, dev->dev_addr,
			       IEEE802154_ADDR_LEN);
		} else {
			dev_addr.addr_type = IEEE802154_ADDR_SHORT;
			dev_addr.short_addr = le16_to_cpu(priv->short_addr);
		}

		dev_addr.pan_id = le16_to_cpu(priv->pan_id);
		saddr = &dev_addr;

		spin_unlock_bh(&priv->mib_lock);
	}

	if (daddr->addr_type != IEEE802154_ADDR_NONE) {
		fc |= (daddr->addr_type << IEEE802154_FC_DAMODE_SHIFT);

		head[pos++] = daddr->pan_id & 0xff;
		head[pos++] = daddr->pan_id >> 8;

		if (daddr->addr_type == IEEE802154_ADDR_SHORT) {
			head[pos++] = daddr->short_addr & 0xff;
			head[pos++] = daddr->short_addr >> 8;
		} else {
			mac802154_haddr_copy_swap(head + pos, daddr->hwaddr);
			pos += IEEE802154_ADDR_LEN;
		}
	}

	if (saddr->addr_type != IEEE802154_ADDR_NONE) {
		fc |= (saddr->addr_type << IEEE802154_FC_SAMODE_SHIFT);

		if ((saddr->pan_id == daddr->pan_id) &&
		    (saddr->pan_id != IEEE802154_PANID_BROADCAST)) {
			/* PANID compression/intra PAN */
			fc |= IEEE802154_FC_INTRA_PAN;
		} else {
			head[pos++] = saddr->pan_id & 0xff;
			head[pos++] = saddr->pan_id >> 8;
		}

		if (saddr->addr_type == IEEE802154_ADDR_SHORT) {
			head[pos++] = saddr->short_addr & 0xff;
			head[pos++] = saddr->short_addr >> 8;
		} else {
			mac802154_haddr_copy_swap(head + pos, saddr->hwaddr);
			pos += IEEE802154_ADDR_LEN;
		}
	}

	head[0] = fc;
	head[1] = fc >> 8;

	memcpy(skb_push(skb, pos), head, pos);
	skb_reset_mac_header(skb);
	skb->mac_len = pos;

	return pos;
}

static int
mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
{
	const u8 *hdr = skb_mac_header(skb);
	const u8 *tail = skb_tail_pointer(skb);
	struct ieee802154_addr_sa *addr = (struct ieee802154_addr_sa *)haddr;
	u16 fc;
	int da_type;

	if (hdr + 3 > tail)
		goto malformed;

	fc = hdr[0] | (hdr[1] << 8);

	hdr += 3;

	da_type = IEEE802154_FC_DAMODE(fc);
	addr->addr_type = IEEE802154_FC_SAMODE(fc);

	switch (da_type) {
	case IEEE802154_ADDR_NONE:
		if (fc & IEEE802154_FC_INTRA_PAN)
			goto malformed;
		break;
	case IEEE802154_ADDR_LONG:
		if (fc & IEEE802154_FC_INTRA_PAN) {
			if (hdr + 2 > tail)
				goto malformed;
			addr->pan_id = hdr[0] | (hdr[1] << 8);
			hdr += 2;
		}

		if (hdr + IEEE802154_ADDR_LEN > tail)
			goto malformed;

		hdr += IEEE802154_ADDR_LEN;
		break;
	case IEEE802154_ADDR_SHORT:
		if (fc & IEEE802154_FC_INTRA_PAN) {
			if (hdr + 2 > tail)
				goto malformed;
			addr->pan_id = hdr[0] | (hdr[1] << 8);
			hdr += 2;
		}

		if (hdr + 2 > tail)
			goto malformed;

		hdr += 2;
		break;
	default:
		goto malformed;

	}

	switch (addr->addr_type) {
	case IEEE802154_ADDR_NONE:
		break;
	case IEEE802154_ADDR_LONG:
		if (!(fc & IEEE802154_FC_INTRA_PAN)) {
			if (hdr + 2 > tail)
				goto malformed;
			addr->pan_id = hdr[0] | (hdr[1] << 8);
			hdr += 2;
		}

		if (hdr + IEEE802154_ADDR_LEN > tail)
			goto malformed;

		mac802154_haddr_copy_swap(addr->hwaddr, hdr);
		hdr += IEEE802154_ADDR_LEN;
		break;
	case IEEE802154_ADDR_SHORT:
		if (!(fc & IEEE802154_FC_INTRA_PAN)) {
			if (hdr + 2 > tail)
				goto malformed;
			addr->pan_id = hdr[0] | (hdr[1] << 8);
			hdr += 2;
		}

		if (hdr + 2 > tail)
			goto malformed;

		addr->short_addr = hdr[0] | (hdr[1] << 8);
		hdr += 2;
		break;
	default:
		goto malformed;
	}

	return sizeof(struct ieee802154_addr_sa);

malformed:
	pr_debug("malformed packet\n");
	return 0;
}

static netdev_tx_t
mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev)
{
	struct mac802154_sub_if_data *priv;
	u8 chan, page;

	priv = netdev_priv(dev);

	spin_lock_bh(&priv->mib_lock);
	chan = priv->chan;
	page = priv->page;
	spin_unlock_bh(&priv->mib_lock);

	if (chan == MAC802154_CHAN_NONE ||
	    page >= WPAN_NUM_PAGES ||
	    chan >= WPAN_NUM_CHANNELS) {
		kfree_skb(skb);
		return NETDEV_TX_OK;
	}

	skb->skb_iif = dev->ifindex;
	dev->stats.tx_packets++;
	dev->stats.tx_bytes += skb->len;

	return mac802154_tx(priv->hw, skb, page, chan);
}

static struct header_ops mac802154_header_ops = {
	.create		= mac802154_header_create,
	.parse		= mac802154_header_parse,
};

static const struct net_device_ops mac802154_wpan_ops = {
	.ndo_open		= mac802154_slave_open,
	.ndo_stop		= mac802154_slave_close,
	.ndo_start_xmit		= mac802154_wpan_xmit,
	.ndo_do_ioctl		= mac802154_wpan_ioctl,
	.ndo_set_mac_address	= mac802154_wpan_mac_addr,
};

void mac802154_wpan_setup(struct net_device *dev)
{
	struct mac802154_sub_if_data *priv;

	dev->addr_len		= IEEE802154_ADDR_LEN;
	memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);

	dev->hard_header_len	= MAC802154_FRAME_HARD_HEADER_LEN;
	dev->header_ops		= &mac802154_header_ops;
	dev->needed_tailroom	= 2; /* FCS */
	dev->mtu		= IEEE802154_MTU;
	dev->tx_queue_len	= 300;
	dev->type		= ARPHRD_IEEE802154;
	dev->flags		= IFF_NOARP | IFF_BROADCAST;
	dev->watchdog_timeo	= 0;

	dev->destructor		= free_netdev;
	dev->netdev_ops		= &mac802154_wpan_ops;
	dev->ml_priv		= &mac802154_mlme_wpan;

	priv = netdev_priv(dev);
	priv->type = IEEE802154_DEV_WPAN;

	priv->chan = MAC802154_CHAN_NONE;
	priv->page = 0;

	spin_lock_init(&priv->mib_lock);

	get_random_bytes(&priv->bsn, 1);
	get_random_bytes(&priv->dsn, 1);

	priv->pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
	priv->short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
}

static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb)
{
	return netif_rx_ni(skb);
}

static int
mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
{
	u16 span, sshort;

	pr_debug("getting packet via slave interface %s\n", sdata->dev->name);

	spin_lock_bh(&sdata->mib_lock);

	span = le16_to_cpu(sdata->pan_id);
	sshort = le16_to_cpu(sdata->short_addr);

	switch (mac_cb(skb)->da.addr_type) {
	case IEEE802154_ADDR_NONE:
		if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE)
			/* FIXME: check if we are PAN coordinator */
			skb->pkt_type = PACKET_OTHERHOST;
		else
			/* ACK comes with both addresses empty */
			skb->pkt_type = PACKET_HOST;
		break;
	case IEEE802154_ADDR_LONG:
		if (mac_cb(skb)->da.pan_id != span &&
		    mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
			skb->pkt_type = PACKET_OTHERHOST;
		else if (!memcmp(mac_cb(skb)->da.hwaddr, sdata->dev->dev_addr,
				 IEEE802154_ADDR_LEN))
			skb->pkt_type = PACKET_HOST;
		else
			skb->pkt_type = PACKET_OTHERHOST;
		break;
	case IEEE802154_ADDR_SHORT:
		if (mac_cb(skb)->da.pan_id != span &&
		    mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
			skb->pkt_type = PACKET_OTHERHOST;
		else if (mac_cb(skb)->da.short_addr == sshort)
			skb->pkt_type = PACKET_HOST;
		else if (mac_cb(skb)->da.short_addr ==
					IEEE802154_ADDR_BROADCAST)
			skb->pkt_type = PACKET_BROADCAST;
		else
			skb->pkt_type = PACKET_OTHERHOST;
		break;
	default:
		break;
	}

	spin_unlock_bh(&sdata->mib_lock);

	skb->dev = sdata->dev;

	sdata->dev->stats.rx_packets++;
	sdata->dev->stats.rx_bytes += skb->len;

	switch (mac_cb_type(skb)) {
	case IEEE802154_FC_TYPE_DATA:
		return mac802154_process_data(sdata->dev, skb);
	default:
		pr_warn("ieee802154: bad frame received (type = %d)\n",
			mac_cb_type(skb));
		kfree_skb(skb);
		return NET_RX_DROP;
	}
}

static int mac802154_parse_frame_start(struct sk_buff *skb)
{
	u8 *head = skb->data;
	u16 fc;

	if (mac802154_fetch_skb_u16(skb, &fc) ||
	    mac802154_fetch_skb_u8(skb, &(mac_cb(skb)->seq)))
		goto err;

	pr_debug("fc: %04x dsn: %02x\n", fc, head[2]);

	mac_cb(skb)->flags = IEEE802154_FC_TYPE(fc);
	mac_cb(skb)->sa.addr_type = IEEE802154_FC_SAMODE(fc);
	mac_cb(skb)->da.addr_type = IEEE802154_FC_DAMODE(fc);

	if (fc & IEEE802154_FC_INTRA_PAN)
		mac_cb(skb)->flags |= MAC_CB_FLAG_INTRAPAN;

	if (mac_cb(skb)->da.addr_type != IEEE802154_ADDR_NONE) {
		if (mac802154_fetch_skb_u16(skb, &(mac_cb(skb)->da.pan_id)))
			goto err;

		/* source PAN id compression */
		if (mac_cb_is_intrapan(skb))
			mac_cb(skb)->sa.pan_id = mac_cb(skb)->da.pan_id;

		pr_debug("dest PAN addr: %04x\n", mac_cb(skb)->da.pan_id);

		if (mac_cb(skb)->da.addr_type == IEEE802154_ADDR_SHORT) {
			u16 *da = &(mac_cb(skb)->da.short_addr);

			if (mac802154_fetch_skb_u16(skb, da))
				goto err;

			pr_debug("destination address is short: %04x\n",
				 mac_cb(skb)->da.short_addr);
		} else {
			if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
				goto err;

			mac802154_haddr_copy_swap(mac_cb(skb)->da.hwaddr,
						  skb->data);
			skb_pull(skb, IEEE802154_ADDR_LEN);

			pr_debug("destination address is hardware\n");
		}
	}

	if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE) {
		/* non PAN-compression, fetch source address id */
		if (!(mac_cb_is_intrapan(skb))) {
			u16 *sa_pan = &(mac_cb(skb)->sa.pan_id);

			if (mac802154_fetch_skb_u16(skb, sa_pan))
				goto err;
		}

		pr_debug("source PAN addr: %04x\n", mac_cb(skb)->da.pan_id);

		if (mac_cb(skb)->sa.addr_type == IEEE802154_ADDR_SHORT) {
			u16 *sa = &(mac_cb(skb)->sa.short_addr);

			if (mac802154_fetch_skb_u16(skb, sa))
				goto err;

			pr_debug("source address is short: %04x\n",
				 mac_cb(skb)->sa.short_addr);
		} else {
			if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
				goto err;

			mac802154_haddr_copy_swap(mac_cb(skb)->sa.hwaddr,
						  skb->data);
			skb_pull(skb, IEEE802154_ADDR_LEN);

			pr_debug("source address is hardware\n");
		}
	}

	return 0;
err:
	return -EINVAL;
}

void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb)
{
	int ret;
	struct sk_buff *sskb;
	struct mac802154_sub_if_data *sdata;

	ret = mac802154_parse_frame_start(skb);
	if (ret) {
		pr_debug("got invalid frame\n");
		return;
	}

	rcu_read_lock();
	list_for_each_entry_rcu(sdata, &priv->slaves, list) {
		if (sdata->type != IEEE802154_DEV_WPAN)
			continue;

		sskb = skb_clone(skb, GFP_ATOMIC);
		if (sskb)
			mac802154_subif_frame(sdata, sskb);
	}
	rcu_read_unlock();
}
Commit	Line	Data
32bad7e3	1	/*
	2	* Copyright 2007-2012 Siemens AG
	3	*
	4	* This program is free software; you can redistribute it and/or modify
	5	* it under the terms of the GNU General Public License version 2
	6	* as published by the Free Software Foundation.
	7	*
	8	* This program is distributed in the hope that it will be useful,
	9	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	10	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	11	* GNU General Public License for more details.
	12	*
	13	* You should have received a copy of the GNU General Public License along
	14	* with this program; if not, write to the Free Software Foundation, Inc.,
	15	* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
	16	*
	17	* Written by:
	18	* Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
	19	* Sergey Lapin <slapin@ossfans.org>
	20	* Maxim Gorbachyov <maxim.gorbachev@siemens.com>
	21	* Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
	22	*/
	23
	24	#include <linux/netdevice.h>
	25	#include <linux/module.h>
	26	#include <linux/if_arp.h>
	27
	28	#include <net/rtnetlink.h>
	29	#include <linux/nl802154.h>
	30	#include <net/af_ieee802154.h>
	31	#include <net/mac802154.h>
	32	#include <net/ieee802154_netdev.h>
	33	#include <net/ieee802154.h>
	34	#include <net/wpan-phy.h>
	35
	36	#include "mac802154.h"
	37
	38	static inline int mac802154_fetch_skb_u8(struct sk_buff skb, u8 val)
	39	{
	40	if (unlikely(!pskb_may_pull(skb, 1)))
	41	return -EINVAL;
	42
	43	*val = skb->data[0];
f458c647	44	skb_pull(skb, 1);
32bad7e3	45
	46	return 0;
	47	}
	48
	49	static inline int mac802154_fetch_skb_u16(struct sk_buff skb, u16 val)
	50	{
	51	if (unlikely(!pskb_may_pull(skb, 2)))
	52	return -EINVAL;
	53
	54	*val = skb->data[0] \| (skb->data[1] << 8);
	55	skb_pull(skb, 2);
	56
	57	return 0;
	58	}
	59
	60	static inline void mac802154_haddr_copy_swap(u8 dest, const u8 src)
	61	{
	62	int i;
	63	for (i = 0; i < IEEE802154_ADDR_LEN; i++)
	64	dest[IEEE802154_ADDR_LEN - i - 1] = src[i];
	65	}
	66
	67	static int
	68	mac802154_wpan_ioctl(struct net_device dev, struct ifreq ifr, int cmd)
	69	{
	70	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	71	struct sockaddr_ieee802154 *sa =
	72	(struct sockaddr_ieee802154 *)&ifr->ifr_addr;
	73	int err = -ENOIOCTLCMD;
	74
	75	spin_lock_bh(&priv->mib_lock);
	76
	77	switch (cmd) {
	78	case SIOCGIFADDR:
b70ab2e8 PB	79	{
	80	u16 pan_id, short_addr;
	81
	82	pan_id = le16_to_cpu(priv->pan_id);
	83	short_addr = le16_to_cpu(priv->short_addr);
	84	if (pan_id == IEEE802154_PANID_BROADCAST \|\|
	85	short_addr == IEEE802154_ADDR_BROADCAST) {
32bad7e3	86	err = -EADDRNOTAVAIL;
	87	break;
	88	}
	89
	90	sa->family = AF_IEEE802154;
	91	sa->addr.addr_type = IEEE802154_ADDR_SHORT;
b70ab2e8 PB	92	sa->addr.pan_id = pan_id;
b70ab2e8 PB	93	sa->addr.short_addr = short_addr;
32bad7e3	94
	95	err = 0;
	96	break;
b70ab2e8	97	}
32bad7e3	98	case SIOCSIFADDR:
	99	dev_warn(&dev->dev,
	100	"Using DEBUGing ioctl SIOCSIFADDR isn't recommened!\n");
	101	if (sa->family != AF_IEEE802154 \|\|
	102	sa->addr.addr_type != IEEE802154_ADDR_SHORT \|\|
	103	sa->addr.pan_id == IEEE802154_PANID_BROADCAST \|\|
	104	sa->addr.short_addr == IEEE802154_ADDR_BROADCAST \|\|
	105	sa->addr.short_addr == IEEE802154_ADDR_UNDEF) {
	106	err = -EINVAL;
	107	break;
	108	}
	109
b70ab2e8 PB	110	priv->pan_id = cpu_to_le16(sa->addr.pan_id);
b70ab2e8 PB	111	priv->short_addr = cpu_to_le16(sa->addr.short_addr);
32bad7e3	112
	113	err = 0;
	114	break;
	115	}
	116
	117	spin_unlock_bh(&priv->mib_lock);
	118	return err;
	119	}
	120
	121	static int mac802154_wpan_mac_addr(struct net_device dev, void p)
	122	{
	123	struct sockaddr *addr = p;
	124
	125	if (netif_running(dev))
	126	return -EBUSY;
	127
	128	/* FIXME: validate addr */
	129	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
	130	mac802154_dev_set_ieee_addr(dev);
	131	return 0;
	132	}
	133
	134	static int mac802154_header_create(struct sk_buff *skb,
	135	struct net_device *dev,
	136	unsigned short type,
	137	const void *_daddr,
	138	const void *_saddr,
	139	unsigned len)
	140	{
376b7bd3 PB	141	const struct ieee802154_addr_sa *saddr = _saddr;
	142	const struct ieee802154_addr_sa *daddr = _daddr;
	143	struct ieee802154_addr_sa dev_addr;
32bad7e3	144	struct mac802154_sub_if_data *priv = netdev_priv(dev);
32bad7e3	145	int pos = 2;
25060d8f	146	u8 head[MAC802154_FRAME_HARD_HEADER_LEN];
32bad7e3	147	u16 fc;
	148
	149	if (!daddr)
	150	return -EINVAL;
	151
32bad7e3	152	head[pos++] = mac_cb(skb)->seq; /* DSN/BSN */
32bad7e3	153	fc = mac_cb_type(skb);
cf692061 TC	154	if (mac_cb_is_ackreq(skb))
cf692061 TC	155	fc \|= IEEE802154_FC_ACK_REQ;
32bad7e3	156
	157	if (!saddr) {
	158	spin_lock_bh(&priv->mib_lock);
	159
b70ab2e8 PB	160	if (priv->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) \|\|
	161	priv->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) \|\|
	162	priv->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
32bad7e3	163	dev_addr.addr_type = IEEE802154_ADDR_LONG;
	164	memcpy(dev_addr.hwaddr, dev->dev_addr,
	165	IEEE802154_ADDR_LEN);
	166	} else {
	167	dev_addr.addr_type = IEEE802154_ADDR_SHORT;
b70ab2e8	168	dev_addr.short_addr = le16_to_cpu(priv->short_addr);
32bad7e3	169	}
32bad7e3	170
b70ab2e8	171	dev_addr.pan_id = le16_to_cpu(priv->pan_id);
32bad7e3	172	saddr = &dev_addr;
	173
	174	spin_unlock_bh(&priv->mib_lock);
	175	}
	176
	177	if (daddr->addr_type != IEEE802154_ADDR_NONE) {
	178	fc \|= (daddr->addr_type << IEEE802154_FC_DAMODE_SHIFT);
	179
	180	head[pos++] = daddr->pan_id & 0xff;
	181	head[pos++] = daddr->pan_id >> 8;
	182
	183	if (daddr->addr_type == IEEE802154_ADDR_SHORT) {
	184	head[pos++] = daddr->short_addr & 0xff;
	185	head[pos++] = daddr->short_addr >> 8;
	186	} else {
	187	mac802154_haddr_copy_swap(head + pos, daddr->hwaddr);
	188	pos += IEEE802154_ADDR_LEN;
	189	}
	190	}
	191
	192	if (saddr->addr_type != IEEE802154_ADDR_NONE) {
	193	fc \|= (saddr->addr_type << IEEE802154_FC_SAMODE_SHIFT);
	194
	195	if ((saddr->pan_id == daddr->pan_id) &&
	196	(saddr->pan_id != IEEE802154_PANID_BROADCAST)) {
	197	/* PANID compression/intra PAN */
	198	fc \|= IEEE802154_FC_INTRA_PAN;
	199	} else {
	200	head[pos++] = saddr->pan_id & 0xff;
	201	head[pos++] = saddr->pan_id >> 8;
	202	}
	203
	204	if (saddr->addr_type == IEEE802154_ADDR_SHORT) {
	205	head[pos++] = saddr->short_addr & 0xff;
	206	head[pos++] = saddr->short_addr >> 8;
	207	} else {
	208	mac802154_haddr_copy_swap(head + pos, saddr->hwaddr);
	209	pos += IEEE802154_ADDR_LEN;
	210	}
	211	}
	212
	213	head[0] = fc;
	214	head[1] = fc >> 8;
	215
	216	memcpy(skb_push(skb, pos), head, pos);
3e69162e AA	217	skb_reset_mac_header(skb);
3e69162e AA	218	skb->mac_len = pos;
32bad7e3	219
	220	return pos;
	221	}
	222
	223	static int
	224	mac802154_header_parse(const struct sk_buff skb, unsigned char haddr)
	225	{
	226	const u8 *hdr = skb_mac_header(skb);
	227	const u8 *tail = skb_tail_pointer(skb);
376b7bd3	228	struct ieee802154_addr_sa addr = (struct ieee802154_addr_sa )haddr;
32bad7e3	229	u16 fc;
	230	int da_type;
	231
	232	if (hdr + 3 > tail)
	233	goto malformed;
	234
	235	fc = hdr[0] \| (hdr[1] << 8);
	236
	237	hdr += 3;
	238
	239	da_type = IEEE802154_FC_DAMODE(fc);
	240	addr->addr_type = IEEE802154_FC_SAMODE(fc);
	241
	242	switch (da_type) {
	243	case IEEE802154_ADDR_NONE:
	244	if (fc & IEEE802154_FC_INTRA_PAN)
	245	goto malformed;
	246	break;
	247	case IEEE802154_ADDR_LONG:
	248	if (fc & IEEE802154_FC_INTRA_PAN) {
	249	if (hdr + 2 > tail)
	250	goto malformed;
	251	addr->pan_id = hdr[0] \| (hdr[1] << 8);
	252	hdr += 2;
	253	}
	254
	255	if (hdr + IEEE802154_ADDR_LEN > tail)
	256	goto malformed;
	257
	258	hdr += IEEE802154_ADDR_LEN;
	259	break;
	260	case IEEE802154_ADDR_SHORT:
	261	if (fc & IEEE802154_FC_INTRA_PAN) {
	262	if (hdr + 2 > tail)
	263	goto malformed;
	264	addr->pan_id = hdr[0] \| (hdr[1] << 8);
	265	hdr += 2;
	266	}
	267
	268	if (hdr + 2 > tail)
	269	goto malformed;
	270
	271	hdr += 2;
	272	break;
	273	default:
	274	goto malformed;
	275
	276	}
	277
	278	switch (addr->addr_type) {
	279	case IEEE802154_ADDR_NONE:
	280	break;
	281	case IEEE802154_ADDR_LONG:
	282	if (!(fc & IEEE802154_FC_INTRA_PAN)) {
	283	if (hdr + 2 > tail)
	284	goto malformed;
	285	addr->pan_id = hdr[0] \| (hdr[1] << 8);
	286	hdr += 2;
	287	}
	288
	289	if (hdr + IEEE802154_ADDR_LEN > tail)
	290	goto malformed;
	291
	292	mac802154_haddr_copy_swap(addr->hwaddr, hdr);
293	hdr += IEEE802154_ADDR_LEN;
294	break;
295	case IEEE802154_ADDR_SHORT:
296	if (!(fc & IEEE802154_FC_INTRA_PAN)) {
297	if (hdr + 2 > tail)
298	goto malformed;
299	addr->pan_id = hdr[0] \| (hdr[1] << 8);
300	hdr += 2;
301	}
302
303	if (hdr + 2 > tail)
304	goto malformed;
305
306	addr->short_addr = hdr[0] \| (hdr[1] << 8);
307	hdr += 2;
308	break;
309	default:
310	goto malformed;
311	}
312
376b7bd3	313	return sizeof(struct ieee802154_addr_sa);
32bad7e3	314
	315	malformed:
	316	pr_debug("malformed packet\n");
	317	return 0;
	318	}
	319
	320	static netdev_tx_t
	321	mac802154_wpan_xmit(struct sk_buff skb, struct net_device dev)
	322	{
	323	struct mac802154_sub_if_data *priv;
	324	u8 chan, page;
	325
	326	priv = netdev_priv(dev);
	327
	328	spin_lock_bh(&priv->mib_lock);
	329	chan = priv->chan;
	330	page = priv->page;
	331	spin_unlock_bh(&priv->mib_lock);
	332
	333	if (chan == MAC802154_CHAN_NONE \|\|
	334	page >= WPAN_NUM_PAGES \|\|
fcefbe9f AO	335	chan >= WPAN_NUM_CHANNELS) {
fcefbe9f AO	336	kfree_skb(skb);
32bad7e3	337	return NETDEV_TX_OK;
fcefbe9f	338	}
32bad7e3	339
	340	skb->skb_iif = dev->ifindex;
	341	dev->stats.tx_packets++;
	342	dev->stats.tx_bytes += skb->len;
	343
	344	return mac802154_tx(priv->hw, skb, page, chan);
	345	}
	346
	347	static struct header_ops mac802154_header_ops = {
	348	.create = mac802154_header_create,
	349	.parse = mac802154_header_parse,
	350	};
	351
	352	static const struct net_device_ops mac802154_wpan_ops = {
	353	.ndo_open = mac802154_slave_open,
	354	.ndo_stop = mac802154_slave_close,
	355	.ndo_start_xmit = mac802154_wpan_xmit,
	356	.ndo_do_ioctl = mac802154_wpan_ioctl,
	357	.ndo_set_mac_address = mac802154_wpan_mac_addr,
	358	};
	359
	360	void mac802154_wpan_setup(struct net_device *dev)
	361	{
	362	struct mac802154_sub_if_data *priv;
	363
	364	dev->addr_len = IEEE802154_ADDR_LEN;
	365	memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
	366
	367	dev->hard_header_len = MAC802154_FRAME_HARD_HEADER_LEN;
	368	dev->header_ops = &mac802154_header_ops;
	369	dev->needed_tailroom = 2; /* FCS */
	370	dev->mtu = IEEE802154_MTU;
e937f583	371	dev->tx_queue_len = 300;
32bad7e3	372	dev->type = ARPHRD_IEEE802154;
	373	dev->flags = IFF_NOARP \| IFF_BROADCAST;
	374	dev->watchdog_timeo = 0;
	375
	376	dev->destructor = free_netdev;
	377	dev->netdev_ops = &mac802154_wpan_ops;
	378	dev->ml_priv = &mac802154_mlme_wpan;
	379
	380	priv = netdev_priv(dev);
	381	priv->type = IEEE802154_DEV_WPAN;
	382
	383	priv->chan = MAC802154_CHAN_NONE;
	384	priv->page = 0;
	385
	386	spin_lock_init(&priv->mib_lock);
	387
	388	get_random_bytes(&priv->bsn, 1);
	389	get_random_bytes(&priv->dsn, 1);
	390
b70ab2e8 PB	391	priv->pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
b70ab2e8 PB	392	priv->short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
32bad7e3	393	}
	394
	395	static int mac802154_process_data(struct net_device dev, struct sk_buff skb)
	396	{
5ff3fec6	397	return netif_rx_ni(skb);
32bad7e3	398	}
	399
	400	static int
	401	mac802154_subif_frame(struct mac802154_sub_if_data sdata, struct sk_buff skb)
	402	{
b70ab2e8 PB	403	u16 span, sshort;
b70ab2e8 PB	404
32bad7e3	405	pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
	406
	407	spin_lock_bh(&sdata->mib_lock);
	408
b70ab2e8 PB	409	span = le16_to_cpu(sdata->pan_id);
	410	sshort = le16_to_cpu(sdata->short_addr);
	411
32bad7e3	412	switch (mac_cb(skb)->da.addr_type) {
	413	case IEEE802154_ADDR_NONE:
	414	if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE)
	415	/* FIXME: check if we are PAN coordinator */
	416	skb->pkt_type = PACKET_OTHERHOST;
	417	else
	418	/* ACK comes with both addresses empty */
	419	skb->pkt_type = PACKET_HOST;
	420	break;
	421	case IEEE802154_ADDR_LONG:
b70ab2e8	422	if (mac_cb(skb)->da.pan_id != span &&
32bad7e3	423	mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
	424	skb->pkt_type = PACKET_OTHERHOST;
	425	else if (!memcmp(mac_cb(skb)->da.hwaddr, sdata->dev->dev_addr,
	426	IEEE802154_ADDR_LEN))
	427	skb->pkt_type = PACKET_HOST;
	428	else
	429	skb->pkt_type = PACKET_OTHERHOST;
	430	break;
	431	case IEEE802154_ADDR_SHORT:
b70ab2e8	432	if (mac_cb(skb)->da.pan_id != span &&
32bad7e3	433	mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
32bad7e3	434	skb->pkt_type = PACKET_OTHERHOST;
b70ab2e8	435	else if (mac_cb(skb)->da.short_addr == sshort)
32bad7e3	436	skb->pkt_type = PACKET_HOST;
	437	else if (mac_cb(skb)->da.short_addr ==
	438	IEEE802154_ADDR_BROADCAST)
	439	skb->pkt_type = PACKET_BROADCAST;
	440	else
	441	skb->pkt_type = PACKET_OTHERHOST;
	442	break;
	443	default:
	444	break;
	445	}
	446
	447	spin_unlock_bh(&sdata->mib_lock);
	448
	449	skb->dev = sdata->dev;
	450
	451	sdata->dev->stats.rx_packets++;
	452	sdata->dev->stats.rx_bytes += skb->len;
	453
	454	switch (mac_cb_type(skb)) {
	455	case IEEE802154_FC_TYPE_DATA:
	456	return mac802154_process_data(sdata->dev, skb);
	457	default:
2cc33c7e WC	458	pr_warn("ieee802154: bad frame received (type = %d)\n",
2cc33c7e WC	459	mac_cb_type(skb));
32bad7e3	460	kfree_skb(skb);
	461	return NET_RX_DROP;
	462	}
	463	}
	464
	465	static int mac802154_parse_frame_start(struct sk_buff *skb)
	466	{
	467	u8 *head = skb->data;
	468	u16 fc;
	469
	470	if (mac802154_fetch_skb_u16(skb, &fc) \|\|
	471	mac802154_fetch_skb_u8(skb, &(mac_cb(skb)->seq)))
	472	goto err;
	473
	474	pr_debug("fc: %04x dsn: %02x\n", fc, head[2]);
	475
	476	mac_cb(skb)->flags = IEEE802154_FC_TYPE(fc);
	477	mac_cb(skb)->sa.addr_type = IEEE802154_FC_SAMODE(fc);
	478	mac_cb(skb)->da.addr_type = IEEE802154_FC_DAMODE(fc);
	479
	480	if (fc & IEEE802154_FC_INTRA_PAN)
	481	mac_cb(skb)->flags \|= MAC_CB_FLAG_INTRAPAN;
	482
	483	if (mac_cb(skb)->da.addr_type != IEEE802154_ADDR_NONE) {
	484	if (mac802154_fetch_skb_u16(skb, &(mac_cb(skb)->da.pan_id)))
	485	goto err;
	486
	487	/* source PAN id compression */
	488	if (mac_cb_is_intrapan(skb))
	489	mac_cb(skb)->sa.pan_id = mac_cb(skb)->da.pan_id;
	490
	491	pr_debug("dest PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
	492
	493	if (mac_cb(skb)->da.addr_type == IEEE802154_ADDR_SHORT) {
	494	u16 *da = &(mac_cb(skb)->da.short_addr);
	495
	496	if (mac802154_fetch_skb_u16(skb, da))
	497	goto err;
	498
	499	pr_debug("destination address is short: %04x\n",
	500	mac_cb(skb)->da.short_addr);
	501	} else {
	502	if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
	503	goto err;
	504
	505	mac802154_haddr_copy_swap(mac_cb(skb)->da.hwaddr,
	506	skb->data);
	507	skb_pull(skb, IEEE802154_ADDR_LEN);
	508
	509	pr_debug("destination address is hardware\n");
	510	}
	511	}
	512
	513	if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE) {
	514	/* non PAN-compression, fetch source address id */
	515	if (!(mac_cb_is_intrapan(skb))) {
	516	u16 *sa_pan = &(mac_cb(skb)->sa.pan_id);
	517
	518	if (mac802154_fetch_skb_u16(skb, sa_pan))
	519	goto err;
	520	}
	521
	522	pr_debug("source PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
	523
524	if (mac_cb(skb)->sa.addr_type == IEEE802154_ADDR_SHORT) {
525	u16 *sa = &(mac_cb(skb)->sa.short_addr);
526
527	if (mac802154_fetch_skb_u16(skb, sa))
528	goto err;
529
530	pr_debug("source address is short: %04x\n",
531	mac_cb(skb)->sa.short_addr);
532	} else {
533	if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
534	goto err;
535
536	mac802154_haddr_copy_swap(mac_cb(skb)->sa.hwaddr,
537	skb->data);
538	skb_pull(skb, IEEE802154_ADDR_LEN);
539
540	pr_debug("source address is hardware\n");
541	}
542	}
543
544	return 0;
545	err:
546	return -EINVAL;
547	}
548
549	void mac802154_wpans_rx(struct mac802154_priv priv, struct sk_buff skb)
550	{
551	int ret;
552	struct sk_buff *sskb;
553	struct mac802154_sub_if_data *sdata;
554
555	ret = mac802154_parse_frame_start(skb);
556	if (ret) {
557	pr_debug("got invalid frame\n");
558	return;
559	}
560
561	rcu_read_lock();
562	list_for_each_entry_rcu(sdata, &priv->slaves, list) {
563	if (sdata->type != IEEE802154_DEV_WPAN)
564	continue;
565
566	sskb = skb_clone(skb, GFP_ATOMIC);
567	if (sskb)
568	mac802154_subif_frame(sdata, sskb);
569	}
570	rcu_read_unlock();
571	}