[mirror_ubuntu-jammy-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 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;
}

int mac802154_set_mac_params(struct net_device *dev,
			     const struct ieee802154_mac_params *params)
{
	struct mac802154_sub_if_data *priv = netdev_priv(dev);

	mutex_lock(&priv->hw->slaves_mtx);
	priv->mac_params = *params;
	mutex_unlock(&priv->hw->slaves_mtx);

	return 0;
}

void mac802154_get_mac_params(struct net_device *dev,
			      struct ieee802154_mac_params *params)
{
	struct mac802154_sub_if_data *priv = netdev_priv(dev);

	mutex_lock(&priv->hw->slaves_mtx);
	*params = priv->mac_params;
	mutex_unlock(&priv->hw->slaves_mtx);
}

int mac802154_wpan_open(struct net_device *dev)
{
	int rc;
	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	struct wpan_phy *phy = priv->hw->phy;

	rc = mac802154_slave_open(dev);
	if (rc < 0)
		return rc;

	mutex_lock(&phy->pib_lock);

	if (phy->set_txpower) {
		rc = phy->set_txpower(phy, priv->mac_params.transmit_power);
		if (rc < 0)
			goto out;
	}

	if (phy->set_lbt) {
		rc = phy->set_lbt(phy, priv->mac_params.lbt);
		if (rc < 0)
			goto out;
	}

	if (phy->set_cca_mode) {
		rc = phy->set_cca_mode(phy, priv->mac_params.cca_mode);
		if (rc < 0)
			goto out;
	}

	if (phy->set_cca_ed_level) {
		rc = phy->set_cca_ed_level(phy, priv->mac_params.cca_ed_level);
		if (rc < 0)
			goto out;
	}

	if (phy->set_csma_params) {
		rc = phy->set_csma_params(phy, priv->mac_params.min_be,
					  priv->mac_params.max_be,
					  priv->mac_params.csma_retries);
		if (rc < 0)
			goto out;
	}

	if (phy->set_frame_retries) {
		rc = phy->set_frame_retries(phy,
					    priv->mac_params.frame_retries);
		if (rc < 0)
			goto out;
	}

	mutex_unlock(&phy->pib_lock);
	return 0;

out:
	mutex_unlock(&phy->pib_lock);
	return rc;
}

static int mac802154_header_create(struct sk_buff *skb,
				   struct net_device *dev,
				   unsigned short type,
				   const void *daddr,
				   const void *saddr,
				   unsigned len)
{
	struct ieee802154_hdr hdr;
	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	int hlen;

	if (!daddr)
		return -EINVAL;

	memset(&hdr.fc, 0, sizeof(hdr.fc));
	hdr.fc.type = mac_cb_type(skb);
	hdr.fc.security_enabled = mac_cb_is_secen(skb);
	hdr.fc.ack_request = mac_cb_is_ackreq(skb);

	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)) {
			hdr.source.mode = IEEE802154_ADDR_LONG;
			hdr.source.extended_addr = priv->extended_addr;
		} else {
			hdr.source.mode = IEEE802154_ADDR_SHORT;
			hdr.source.short_addr = priv->short_addr;
		}

		hdr.source.pan_id = priv->pan_id;

		spin_unlock_bh(&priv->mib_lock);
	} else {
		hdr.source = *(const struct ieee802154_addr *)saddr;
	}

	hdr.dest = *(const struct ieee802154_addr *)daddr;

	hlen = ieee802154_hdr_push(skb, &hdr);
	if (hlen < 0)
		return -EINVAL;

	skb_reset_mac_header(skb);
	skb->mac_len = hlen;

	if (hlen + len + 2 > dev->mtu)
		return -EMSGSIZE;

	return hlen;
}

static int
mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
{
	struct ieee802154_hdr hdr;
	struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr;

	if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0) {
		pr_debug("malformed packet\n");
		return 0;
	}

	*addr = hdr.source;
	return sizeof(*addr);
}

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_wpan_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);

	/* defaults per 802.15.4-2011 */
	priv->mac_params.min_be = 3;
	priv->mac_params.max_be = 5;
	priv->mac_params.csma_retries = 4;
	priv->mac_params.frame_retries = -1; /* for compatibility, actual default is 3 */

	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)
{
	__le16 span, sshort;

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

	spin_lock_bh(&sdata->mib_lock);

	span = sdata->pan_id;
	sshort = sdata->short_addr;

	switch (mac_cb(skb)->dest.mode) {
	case IEEE802154_ADDR_NONE:
		if (mac_cb(skb)->dest.mode != 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)->dest.pan_id != span &&
		    mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
			skb->pkt_type = PACKET_OTHERHOST;
		else if (mac_cb(skb)->dest.extended_addr == sdata->extended_addr)
			skb->pkt_type = PACKET_HOST;
		else
			skb->pkt_type = PACKET_OTHERHOST;
		break;
	case IEEE802154_ADDR_SHORT:
		if (mac_cb(skb)->dest.pan_id != span &&
		    mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
			skb->pkt_type = PACKET_OTHERHOST;
		else if (mac_cb(skb)->dest.short_addr == sshort)
			skb->pkt_type = PACKET_HOST;
		else if (mac_cb(skb)->dest.short_addr ==
			  cpu_to_le16(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 void mac802154_print_addr(const char *name,
				 const struct ieee802154_addr *addr)
{
	if (addr->mode == IEEE802154_ADDR_NONE)
		pr_debug("%s not present\n", name);

	pr_debug("%s PAN ID: %04x\n", name, le16_to_cpu(addr->pan_id));
	if (addr->mode == IEEE802154_ADDR_SHORT) {
		pr_debug("%s is short: %04x\n", name,
			 le16_to_cpu(addr->short_addr));
	} else {
		u64 hw = swab64((__force u64) addr->extended_addr);

		pr_debug("%s is hardware: %8phC\n", name, &hw);
	}
}

static int mac802154_parse_frame_start(struct sk_buff *skb)
{
	int hlen;
	struct ieee802154_hdr hdr;

	hlen = ieee802154_hdr_pull(skb, &hdr);
	if (hlen < 0)
		return -EINVAL;

	skb->mac_len = hlen;

	pr_debug("fc: %04x dsn: %02x\n", le16_to_cpup((__le16 *)&hdr.fc),
		 hdr.seq);

	mac_cb(skb)->flags = hdr.fc.type;

	if (hdr.fc.ack_request)
		mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
	if (hdr.fc.security_enabled)
		mac_cb(skb)->flags |= MAC_CB_FLAG_SECEN;

	mac802154_print_addr("destination", &hdr.dest);
	mac802154_print_addr("source", &hdr.source);

	mac_cb(skb)->source = hdr.source;
	mac_cb(skb)->dest = hdr.dest;

	if (hdr.fc.security_enabled) {
		u64 key;

		pr_debug("seclevel %i\n", hdr.sec.level);

		switch (hdr.sec.key_id_mode) {
		case IEEE802154_SCF_KEY_IMPLICIT:
			pr_debug("implicit key\n");
			break;

		case IEEE802154_SCF_KEY_INDEX:
			pr_debug("key %02x\n", hdr.sec.key_id);
			break;

		case IEEE802154_SCF_KEY_SHORT_INDEX:
			pr_debug("key %04x:%04x %02x\n",
				 le32_to_cpu(hdr.sec.short_src) >> 16,
				 le32_to_cpu(hdr.sec.short_src) & 0xffff,
				 hdr.sec.key_id);
			break;

		case IEEE802154_SCF_KEY_HW_INDEX:
			key = swab64((__force u64) hdr.sec.extended_src);
			pr_debug("key source %8phC %02x\n", &key,
				 hdr.sec.key_id);
			break;
		}

		return -EINVAL;
	}

	return 0;
}

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
32bad7e3	38	static int
	39	mac802154_wpan_ioctl(struct net_device dev, struct ifreq ifr, int cmd)
	40	{
	41	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	42	struct sockaddr_ieee802154 *sa =
	43	(struct sockaddr_ieee802154 *)&ifr->ifr_addr;
	44	int err = -ENOIOCTLCMD;
	45
	46	spin_lock_bh(&priv->mib_lock);
	47
	48	switch (cmd) {
	49	case SIOCGIFADDR:
b70ab2e8 PB	50	{
	51	u16 pan_id, short_addr;
	52
	53	pan_id = le16_to_cpu(priv->pan_id);
	54	short_addr = le16_to_cpu(priv->short_addr);
	55	if (pan_id == IEEE802154_PANID_BROADCAST \|\|
	56	short_addr == IEEE802154_ADDR_BROADCAST) {
32bad7e3	57	err = -EADDRNOTAVAIL;
	58	break;
	59	}
	60
	61	sa->family = AF_IEEE802154;
	62	sa->addr.addr_type = IEEE802154_ADDR_SHORT;
b70ab2e8 PB	63	sa->addr.pan_id = pan_id;
b70ab2e8 PB	64	sa->addr.short_addr = short_addr;
32bad7e3	65
	66	err = 0;
	67	break;
b70ab2e8	68	}
32bad7e3	69	case SIOCSIFADDR:
	70	dev_warn(&dev->dev,
	71	"Using DEBUGing ioctl SIOCSIFADDR isn't recommened!\n");
	72	if (sa->family != AF_IEEE802154 \|\|
	73	sa->addr.addr_type != IEEE802154_ADDR_SHORT \|\|
	74	sa->addr.pan_id == IEEE802154_PANID_BROADCAST \|\|
	75	sa->addr.short_addr == IEEE802154_ADDR_BROADCAST \|\|
	76	sa->addr.short_addr == IEEE802154_ADDR_UNDEF) {
	77	err = -EINVAL;
	78	break;
	79	}
	80
b70ab2e8 PB	81	priv->pan_id = cpu_to_le16(sa->addr.pan_id);
b70ab2e8 PB	82	priv->short_addr = cpu_to_le16(sa->addr.short_addr);
32bad7e3	83
	84	err = 0;
	85	break;
	86	}
	87
	88	spin_unlock_bh(&priv->mib_lock);
	89	return err;
	90	}
	91
	92	static int mac802154_wpan_mac_addr(struct net_device dev, void p)
	93	{
	94	struct sockaddr *addr = p;
	95
	96	if (netif_running(dev))
	97	return -EBUSY;
	98
	99	/* FIXME: validate addr */
	100	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
	101	mac802154_dev_set_ieee_addr(dev);
	102	return 0;
	103	}
	104
e462ded6 PB	105	int mac802154_set_mac_params(struct net_device *dev,
	106	const struct ieee802154_mac_params *params)
	107	{
	108	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	109
	110	mutex_lock(&priv->hw->slaves_mtx);
	111	priv->mac_params = *params;
	112	mutex_unlock(&priv->hw->slaves_mtx);
	113
	114	return 0;
	115	}
	116
	117	void mac802154_get_mac_params(struct net_device *dev,
	118	struct ieee802154_mac_params *params)
	119	{
	120	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	121
	122	mutex_lock(&priv->hw->slaves_mtx);
	123	*params = priv->mac_params;
	124	mutex_unlock(&priv->hw->slaves_mtx);
	125	}
	126
	127	int mac802154_wpan_open(struct net_device *dev)
	128	{
	129	int rc;
	130	struct mac802154_sub_if_data *priv = netdev_priv(dev);
	131	struct wpan_phy *phy = priv->hw->phy;
	132
	133	rc = mac802154_slave_open(dev);
	134	if (rc < 0)
	135	return rc;
	136
	137	mutex_lock(&phy->pib_lock);
	138
	139	if (phy->set_txpower) {
	140	rc = phy->set_txpower(phy, priv->mac_params.transmit_power);
	141	if (rc < 0)
	142	goto out;
	143	}
	144
	145	if (phy->set_lbt) {
	146	rc = phy->set_lbt(phy, priv->mac_params.lbt);
	147	if (rc < 0)
	148	goto out;
	149	}
	150
	151	if (phy->set_cca_mode) {
	152	rc = phy->set_cca_mode(phy, priv->mac_params.cca_mode);
	153	if (rc < 0)
	154	goto out;
	155	}
	156
	157	if (phy->set_cca_ed_level) {
	158	rc = phy->set_cca_ed_level(phy, priv->mac_params.cca_ed_level);
	159	if (rc < 0)
	160	goto out;
	161	}
	162
	163	if (phy->set_csma_params) {
	164	rc = phy->set_csma_params(phy, priv->mac_params.min_be,
	165	priv->mac_params.max_be,
	166	priv->mac_params.csma_retries);
	167	if (rc < 0)
	168	goto out;
169	}
170
171	if (phy->set_frame_retries) {
172	rc = phy->set_frame_retries(phy,
173	priv->mac_params.frame_retries);
174	if (rc < 0)
175	goto out;
176	}
177
178	mutex_unlock(&phy->pib_lock);
179	return 0;
180
181	out:
182	mutex_unlock(&phy->pib_lock);
183	return rc;
184	}
185
32bad7e3	186	static int mac802154_header_create(struct sk_buff *skb,
	187	struct net_device *dev,
	188	unsigned short type,
e6278d92 PB	189	const void *daddr,
e6278d92 PB	190	const void *saddr,
32bad7e3	191	unsigned len)
32bad7e3	192	{
e6278d92	193	struct ieee802154_hdr hdr;
32bad7e3	194	struct mac802154_sub_if_data *priv = netdev_priv(dev);
e6278d92	195	int hlen;
32bad7e3	196
	197	if (!daddr)
	198	return -EINVAL;
	199
e6278d92 PB	200	memset(&hdr.fc, 0, sizeof(hdr.fc));
	201	hdr.fc.type = mac_cb_type(skb);
	202	hdr.fc.security_enabled = mac_cb_is_secen(skb);
	203	hdr.fc.ack_request = mac_cb_is_ackreq(skb);
32bad7e3	204
	205	if (!saddr) {
	206	spin_lock_bh(&priv->mib_lock);
	207
b70ab2e8 PB	208	if (priv->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) \|\|
	209	priv->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) \|\|
	210	priv->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
e6278d92 PB	211	hdr.source.mode = IEEE802154_ADDR_LONG;
e6278d92 PB	212	hdr.source.extended_addr = priv->extended_addr;
32bad7e3	213	} else {
e6278d92 PB	214	hdr.source.mode = IEEE802154_ADDR_SHORT;
e6278d92 PB	215	hdr.source.short_addr = priv->short_addr;
32bad7e3	216	}
32bad7e3	217
e6278d92	218	hdr.source.pan_id = priv->pan_id;
32bad7e3	219
32bad7e3	220	spin_unlock_bh(&priv->mib_lock);
e6278d92 PB	221	} else {
e6278d92 PB	222	hdr.source = (const struct ieee802154_addr )saddr;
32bad7e3	223	}
32bad7e3	224
e6278d92	225	hdr.dest = (const struct ieee802154_addr )daddr;
32bad7e3	226
e6278d92 PB	227	hlen = ieee802154_hdr_push(skb, &hdr);
	228	if (hlen < 0)
	229	return -EINVAL;
32bad7e3	230
3e69162e	231	skb_reset_mac_header(skb);
e6278d92	232	skb->mac_len = hlen;
32bad7e3	233
d1d7358e PB	234	if (hlen + len + 2 > dev->mtu)
	235	return -EMSGSIZE;
	236
e6278d92	237	return hlen;
32bad7e3	238	}
	239
	240	static int
	241	mac802154_header_parse(const struct sk_buff skb, unsigned char haddr)
	242	{
e6278d92 PB	243	struct ieee802154_hdr hdr;
e6278d92 PB	244	struct ieee802154_addr addr = (struct ieee802154_addr )haddr;
32bad7e3	245
e6278d92 PB	246	if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0) {
	247	pr_debug("malformed packet\n");
	248	return 0;
32bad7e3	249	}
32bad7e3	250
e6278d92 PB	251	*addr = hdr.source;
e6278d92 PB	252	return sizeof(*addr);
32bad7e3	253	}
	254
	255	static netdev_tx_t
	256	mac802154_wpan_xmit(struct sk_buff skb, struct net_device dev)
	257	{
	258	struct mac802154_sub_if_data *priv;
	259	u8 chan, page;
	260
	261	priv = netdev_priv(dev);
	262
	263	spin_lock_bh(&priv->mib_lock);
	264	chan = priv->chan;
	265	page = priv->page;
	266	spin_unlock_bh(&priv->mib_lock);
	267
	268	if (chan == MAC802154_CHAN_NONE \|\|
	269	page >= WPAN_NUM_PAGES \|\|
fcefbe9f AO	270	chan >= WPAN_NUM_CHANNELS) {
fcefbe9f AO	271	kfree_skb(skb);
32bad7e3	272	return NETDEV_TX_OK;
fcefbe9f	273	}
32bad7e3	274
	275	skb->skb_iif = dev->ifindex;
	276	dev->stats.tx_packets++;
	277	dev->stats.tx_bytes += skb->len;
	278
	279	return mac802154_tx(priv->hw, skb, page, chan);
	280	}
	281
	282	static struct header_ops mac802154_header_ops = {
	283	.create = mac802154_header_create,
	284	.parse = mac802154_header_parse,
	285	};
	286
	287	static const struct net_device_ops mac802154_wpan_ops = {
e462ded6	288	.ndo_open = mac802154_wpan_open,
32bad7e3	289	.ndo_stop = mac802154_slave_close,
	290	.ndo_start_xmit = mac802154_wpan_xmit,
	291	.ndo_do_ioctl = mac802154_wpan_ioctl,
	292	.ndo_set_mac_address = mac802154_wpan_mac_addr,
	293	};
	294
	295	void mac802154_wpan_setup(struct net_device *dev)
	296	{
	297	struct mac802154_sub_if_data *priv;
	298
	299	dev->addr_len = IEEE802154_ADDR_LEN;
	300	memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
	301
	302	dev->hard_header_len = MAC802154_FRAME_HARD_HEADER_LEN;
	303	dev->header_ops = &mac802154_header_ops;
	304	dev->needed_tailroom = 2; /* FCS */
	305	dev->mtu = IEEE802154_MTU;
e937f583	306	dev->tx_queue_len = 300;
32bad7e3	307	dev->type = ARPHRD_IEEE802154;
	308	dev->flags = IFF_NOARP \| IFF_BROADCAST;
	309	dev->watchdog_timeo = 0;
	310
	311	dev->destructor = free_netdev;
	312	dev->netdev_ops = &mac802154_wpan_ops;
	313	dev->ml_priv = &mac802154_mlme_wpan;
	314
	315	priv = netdev_priv(dev);
	316	priv->type = IEEE802154_DEV_WPAN;
	317
	318	priv->chan = MAC802154_CHAN_NONE;
	319	priv->page = 0;
	320
	321	spin_lock_init(&priv->mib_lock);
	322
	323	get_random_bytes(&priv->bsn, 1);
	324	get_random_bytes(&priv->dsn, 1);
	325
e462ded6 PB	326	/* defaults per 802.15.4-2011 */
	327	priv->mac_params.min_be = 3;
	328	priv->mac_params.max_be = 5;
	329	priv->mac_params.csma_retries = 4;
	330	priv->mac_params.frame_retries = -1; /* for compatibility, actual default is 3 */
	331
b70ab2e8 PB	332	priv->pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
b70ab2e8 PB	333	priv->short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
32bad7e3	334	}
	335
	336	static int mac802154_process_data(struct net_device dev, struct sk_buff skb)
	337	{
5ff3fec6	338	return netif_rx_ni(skb);
32bad7e3	339	}
	340
	341	static int
	342	mac802154_subif_frame(struct mac802154_sub_if_data sdata, struct sk_buff skb)
	343	{
ae531b94	344	__le16 span, sshort;
b70ab2e8	345
32bad7e3	346	pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
	347
	348	spin_lock_bh(&sdata->mib_lock);
	349
ae531b94 PB	350	span = sdata->pan_id;
ae531b94 PB	351	sshort = sdata->short_addr;
b70ab2e8	352
ae531b94	353	switch (mac_cb(skb)->dest.mode) {
32bad7e3	354	case IEEE802154_ADDR_NONE:
ae531b94	355	if (mac_cb(skb)->dest.mode != IEEE802154_ADDR_NONE)
32bad7e3	356	/* FIXME: check if we are PAN coordinator */
	357	skb->pkt_type = PACKET_OTHERHOST;
	358	else
	359	/* ACK comes with both addresses empty */
	360	skb->pkt_type = PACKET_HOST;
	361	break;
	362	case IEEE802154_ADDR_LONG:
ae531b94 PB	363	if (mac_cb(skb)->dest.pan_id != span &&
ae531b94 PB	364	mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
32bad7e3	365	skb->pkt_type = PACKET_OTHERHOST;
ae531b94	366	else if (mac_cb(skb)->dest.extended_addr == sdata->extended_addr)
32bad7e3	367	skb->pkt_type = PACKET_HOST;
	368	else
	369	skb->pkt_type = PACKET_OTHERHOST;
	370	break;
	371	case IEEE802154_ADDR_SHORT:
ae531b94 PB	372	if (mac_cb(skb)->dest.pan_id != span &&
ae531b94 PB	373	mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
32bad7e3	374	skb->pkt_type = PACKET_OTHERHOST;
ae531b94	375	else if (mac_cb(skb)->dest.short_addr == sshort)
32bad7e3	376	skb->pkt_type = PACKET_HOST;
ae531b94 PB	377	else if (mac_cb(skb)->dest.short_addr ==
ae531b94 PB	378	cpu_to_le16(IEEE802154_ADDR_BROADCAST))
32bad7e3	379	skb->pkt_type = PACKET_BROADCAST;
	380	else
	381	skb->pkt_type = PACKET_OTHERHOST;
	382	break;
	383	default:
	384	break;
	385	}
	386
	387	spin_unlock_bh(&sdata->mib_lock);
	388
	389	skb->dev = sdata->dev;
	390
	391	sdata->dev->stats.rx_packets++;
	392	sdata->dev->stats.rx_bytes += skb->len;
	393
	394	switch (mac_cb_type(skb)) {
	395	case IEEE802154_FC_TYPE_DATA:
	396	return mac802154_process_data(sdata->dev, skb);
	397	default:
2cc33c7e WC	398	pr_warn("ieee802154: bad frame received (type = %d)\n",
2cc33c7e WC	399	mac_cb_type(skb));
32bad7e3	400	kfree_skb(skb);
	401	return NET_RX_DROP;
	402	}
	403	}
	404
e6278d92 PB	405	static void mac802154_print_addr(const char *name,
e6278d92 PB	406	const struct ieee802154_addr *addr)
32bad7e3	407	{
e6278d92 PB	408	if (addr->mode == IEEE802154_ADDR_NONE)
e6278d92 PB	409	pr_debug("%s not present\n", name);
32bad7e3	410
e6278d92 PB	411	pr_debug("%s PAN ID: %04x\n", name, le16_to_cpu(addr->pan_id));
	412	if (addr->mode == IEEE802154_ADDR_SHORT) {
	413	pr_debug("%s is short: %04x\n", name,
	414	le16_to_cpu(addr->short_addr));
	415	} else {
	416	u64 hw = swab64((__force u64) addr->extended_addr);
32bad7e3	417
e6278d92 PB	418	pr_debug("%s is hardware: %8phC\n", name, &hw);
	419	}
	420	}
32bad7e3	421
e6278d92 PB	422	static int mac802154_parse_frame_start(struct sk_buff *skb)
e6278d92 PB	423	{
e6278d92	424	int hlen;
ae531b94	425	struct ieee802154_hdr hdr;
32bad7e3	426
e6278d92 PB	427	hlen = ieee802154_hdr_pull(skb, &hdr);
	428	if (hlen < 0)
	429	return -EINVAL;
32bad7e3	430
e6278d92	431	skb->mac_len = hlen;
32bad7e3	432
e6278d92 PB	433	pr_debug("fc: %04x dsn: %02x\n", le16_to_cpup((__le16 *)&hdr.fc),
e6278d92 PB	434	hdr.seq);
32bad7e3	435
e6278d92	436	mac_cb(skb)->flags = hdr.fc.type;
32bad7e3	437
e6278d92 PB	438	if (hdr.fc.ack_request)
	439	mac_cb(skb)->flags \|= MAC_CB_FLAG_ACKREQ;
	440	if (hdr.fc.security_enabled)
	441	mac_cb(skb)->flags \|= MAC_CB_FLAG_SECEN;
32bad7e3	442
e6278d92 PB	443	mac802154_print_addr("destination", &hdr.dest);
e6278d92 PB	444	mac802154_print_addr("source", &hdr.source);
32bad7e3	445
ae531b94 PB	446	mac_cb(skb)->source = hdr.source;
	447	mac_cb(skb)->dest = hdr.dest;
	448
e6278d92 PB	449	if (hdr.fc.security_enabled) {
e6278d92 PB	450	u64 key;
32bad7e3	451
e6278d92	452	pr_debug("seclevel %i\n", hdr.sec.level);
32bad7e3	453
e6278d92 PB	454	switch (hdr.sec.key_id_mode) {
	455	case IEEE802154_SCF_KEY_IMPLICIT:
	456	pr_debug("implicit key\n");
	457	break;
32bad7e3	458
e6278d92 PB	459	case IEEE802154_SCF_KEY_INDEX:
	460	pr_debug("key %02x\n", hdr.sec.key_id);
	461	break;
32bad7e3	462
e6278d92 PB	463	case IEEE802154_SCF_KEY_SHORT_INDEX:
	464	pr_debug("key %04x:%04x %02x\n",
	465	le32_to_cpu(hdr.sec.short_src) >> 16,
	466	le32_to_cpu(hdr.sec.short_src) & 0xffff,
	467	hdr.sec.key_id);
	468	break;
32bad7e3	469
e6278d92 PB	470	case IEEE802154_SCF_KEY_HW_INDEX:
	471	key = swab64((__force u64) hdr.sec.extended_src);
	472	pr_debug("key source %8phC %02x\n", &key,
	473	hdr.sec.key_id);
	474	break;
32bad7e3	475	}
e6278d92 PB	476
e6278d92 PB	477	return -EINVAL;
32bad7e3	478	}
	479
	480	return 0;
32bad7e3	481	}
	482
	483	void mac802154_wpans_rx(struct mac802154_priv priv, struct sk_buff skb)
	484	{
	485	int ret;
	486	struct sk_buff *sskb;
	487	struct mac802154_sub_if_data *sdata;
	488
	489	ret = mac802154_parse_frame_start(skb);
	490	if (ret) {
	491	pr_debug("got invalid frame\n");
	492	return;
	493	}
	494
	495	rcu_read_lock();
	496	list_for_each_entry_rcu(sdata, &priv->slaves, list) {
	497	if (sdata->type != IEEE802154_DEV_WPAN)
	498	continue;
	499
	500	sskb = skb_clone(skb, GFP_ATOMIC);
	501	if (sskb)
	502	mac802154_subif_frame(sdata, sskb);
	503	}
	504	rcu_read_unlock();
	505	}