net/mac802154/wpan.c - kernel/common - Git at Google

 /*
  * 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 <[email protected]>
  * Sergey Lapin <[email protected]>
  * Maxim Gorbachyov <[email protected]>
  * Alexander Smirnov <[email protected]>
  */

 #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:
 		if (priv->pan_id == IEEE802154_PANID_BROADCAST ||
 		    priv->short_addr == IEEE802154_ADDR_BROADCAST) {
 			err = -EADDRNOTAVAIL;
 			break;
 		}

 		sa->family = AF_IEEE802154;
 		sa->addr.addr_type = IEEE802154_ADDR_SHORT;
 		sa->addr.pan_id = priv->pan_id;
 		sa->addr.short_addr = priv->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 = sa->addr.pan_id;
 		priv->short_addr = 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 *saddr = _saddr;
 	const struct ieee802154_addr *daddr = _daddr;
 	struct ieee802154_addr dev_addr;
 	struct mac802154_sub_if_data *priv = netdev_priv(dev);
 	int pos = 2;
 	u8 *head;
 	u16 fc;

 	if (!daddr)
 		return -EINVAL;

 	head = kzalloc(MAC802154_FRAME_HARD_HEADER_LEN, GFP_KERNEL);
 	if (head == NULL)
 		return -ENOMEM;

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

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

 		if (priv->short_addr == IEEE802154_ADDR_BROADCAST ||
 		    priv->short_addr == IEEE802154_ADDR_UNDEF ||
 		    priv->pan_id == 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 = priv->short_addr;
 		}

 		dev_addr.pan_id = 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);
 	kfree(head);

 	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 *addr = (struct ieee802154_addr *)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);

 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	= 10;
 	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 = IEEE802154_PANID_BROADCAST;
 	priv->short_addr = IEEE802154_ADDR_BROADCAST;
 }

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

 static int
 mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
 {
 	pr_debug("getting packet via slave interface %s\n", sdata->dev->name);

 	spin_lock_bh(&sdata->mib_lock);

 	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 != sdata->pan_id &&
 		    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 != sdata->pan_id &&
 		    mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
 			skb->pkt_type = PACKET_OTHERHOST;
 		else if (mac_cb(skb)->da.short_addr == sdata->short_addr)
 			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_warning("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();
 }
	/*
	* 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 <[email protected]>
	* Sergey Lapin <[email protected]>
	* Maxim Gorbachyov <[email protected]>
	* Alexander Smirnov <[email protected]>
	*/

	#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:
	if (priv->pan_id == IEEE802154_PANID_BROADCAST \|\|
	priv->short_addr == IEEE802154_ADDR_BROADCAST) {
	err = -EADDRNOTAVAIL;
	break;
	}

	sa->family = AF_IEEE802154;
	sa->addr.addr_type = IEEE802154_ADDR_SHORT;
	sa->addr.pan_id = priv->pan_id;
	sa->addr.short_addr = priv->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 = sa->addr.pan_id;
	priv->short_addr = 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 *saddr = _saddr;
	const struct ieee802154_addr *daddr = _daddr;
	struct ieee802154_addr dev_addr;
	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	int pos = 2;
	u8 *head;
	u16 fc;

	if (!daddr)
	return -EINVAL;

	head = kzalloc(MAC802154_FRAME_HARD_HEADER_LEN, GFP_KERNEL);
	if (head == NULL)
	return -ENOMEM;

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

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

	if (priv->short_addr == IEEE802154_ADDR_BROADCAST \|\|
	priv->short_addr == IEEE802154_ADDR_UNDEF \|\|
	priv->pan_id == 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 = priv->short_addr;
	}

	dev_addr.pan_id = 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);
	kfree(head);

	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 addr = (struct ieee802154_addr )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);

	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 = 10;
	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 = IEEE802154_PANID_BROADCAST;
	priv->short_addr = IEEE802154_ADDR_BROADCAST;
	}

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

	static int
	mac802154_subif_frame(struct mac802154_sub_if_data sdata, struct sk_buff skb)
	{
	pr_debug("getting packet via slave interface %s\n", sdata->dev->name);

	spin_lock_bh(&sdata->mib_lock);

	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 != sdata->pan_id &&
	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 != sdata->pan_id &&
	mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
	skb->pkt_type = PACKET_OTHERHOST;
	else if (mac_cb(skb)->da.short_addr == sdata->short_addr)
	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_warning("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();
	}