[mirror_ubuntu-eoan-kernel.git] / net / mac802154 / iface.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.
 *
 * 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 <linux/ieee802154.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/cfg802154.h>

#include "ieee802154_i.h"

static int mac802154_wpan_update_llsec(struct net_device *dev)
{
	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
	int rc = 0;

	if (ops->llsec) {
		struct ieee802154_llsec_params params;
		int changed = 0;

		params.pan_id = sdata->pan_id;
		changed |= IEEE802154_LLSEC_PARAM_PAN_ID;

		params.hwaddr = sdata->extended_addr;
		changed |= IEEE802154_LLSEC_PARAM_HWADDR;

		rc = ops->llsec->set_params(dev, &params, changed);
	}

	return rc;
}

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

	spin_lock_bh(&sdata->mib_lock);

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

		pan_id = le16_to_cpu(sdata->pan_id);
		short_addr = le16_to_cpu(sdata->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 recommended!\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;
		}

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

		err = mac802154_wpan_update_llsec(dev);
		break;
	}

	spin_unlock_bh(&sdata->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 mac802154_wpan_update_llsec(dev);
}

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

	mutex_lock(&sdata->local->iflist_mtx);
	sdata->mac_params = *params;
	mutex_unlock(&sdata->local->iflist_mtx);

	return 0;
}

void mac802154_get_mac_params(struct net_device *dev,
			      struct ieee802154_mac_params *params)
{
	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);

	mutex_lock(&sdata->local->iflist_mtx);
	*params = sdata->mac_params;
	mutex_unlock(&sdata->local->iflist_mtx);
}

static int mac802154_wpan_open(struct net_device *dev)
{
	int rc;
	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
	struct wpan_phy *phy = sdata->local->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, sdata->mac_params.transmit_power);
		if (rc < 0)
			goto out;
	}

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

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

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

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

	if (phy->set_frame_retries) {
		rc = phy->set_frame_retries(phy,
					    sdata->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_set_header_security(struct ieee802154_sub_if_data *sdata,
					 struct ieee802154_hdr *hdr,
					 const struct ieee802154_mac_cb *cb)
{
	struct ieee802154_llsec_params params;
	u8 level;

	mac802154_llsec_get_params(&sdata->sec, &params);

	if (!params.enabled && cb->secen_override && cb->secen)
		return -EINVAL;
	if (!params.enabled ||
	    (cb->secen_override && !cb->secen) ||
	    !params.out_level)
		return 0;
	if (cb->seclevel_override && !cb->seclevel)
		return -EINVAL;

	level = cb->seclevel_override ? cb->seclevel : params.out_level;

	hdr->fc.security_enabled = 1;
	hdr->sec.level = level;
	hdr->sec.key_id_mode = params.out_key.mode;
	if (params.out_key.mode == IEEE802154_SCF_KEY_SHORT_INDEX)
		hdr->sec.short_src = params.out_key.short_source;
	else if (params.out_key.mode == IEEE802154_SCF_KEY_HW_INDEX)
		hdr->sec.extended_src = params.out_key.extended_source;
	hdr->sec.key_id = params.out_key.id;

	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)
{
	struct ieee802154_hdr hdr;
	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
	struct ieee802154_mac_cb *cb = mac_cb(skb);
	int hlen;

	if (!daddr)
		return -EINVAL;

	memset(&hdr.fc, 0, sizeof(hdr.fc));
	hdr.fc.type = cb->type;
	hdr.fc.security_enabled = cb->secen;
	hdr.fc.ack_request = cb->ackreq;
	hdr.seq = ieee802154_mlme_ops(dev)->get_dsn(dev);

	if (mac802154_set_header_security(sdata, &hdr, cb) < 0)
		return -EINVAL;

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

		if (sdata->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) ||
		    sdata->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) ||
		    sdata->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
			hdr.source.mode = IEEE802154_ADDR_LONG;
			hdr.source.extended_addr = sdata->extended_addr;
		} else {
			hdr.source.mode = IEEE802154_ADDR_SHORT;
			hdr.source.short_addr = sdata->short_addr;
		}

		hdr.source.pan_id = sdata->pan_id;

		spin_unlock_bh(&sdata->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 (len > ieee802154_max_payload(&hdr))
		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 ieee802154_sub_if_data *sdata;
	u8 chan, page;
	int rc;

	sdata = netdev_priv(dev);

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

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

	rc = mac802154_llsec_encrypt(&sdata->sec, skb);
	if (rc) {
		pr_warn("encryption failed: %i\n", rc);
		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(sdata->local, 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,
};

static void mac802154_wpan_free(struct net_device *dev)
{
	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);

	mac802154_llsec_destroy(&sdata->sec);

	free_netdev(dev);
}

void mac802154_wpan_setup(struct net_device *dev)
{
	struct ieee802154_sub_if_data *sdata;

	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 + 16; /* FCS + MIC */
	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		= mac802154_wpan_free;
	dev->netdev_ops		= &mac802154_wpan_ops;
	dev->ml_priv		= &mac802154_mlme_wpan;

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

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

	spin_lock_init(&sdata->mib_lock);
	mutex_init(&sdata->sec_mtx);

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

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

	sdata->pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
	sdata->short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);

	mac802154_llsec_init(&sdata->sec);
}

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

static int
mac802154_subif_frame(struct ieee802154_sub_if_data *sdata, struct sk_buff *skb,
		      const struct ieee802154_hdr *hdr)
{
	__le16 span, sshort;
	int rc;

	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:
		spin_unlock_bh(&sdata->mib_lock);
		pr_debug("invalid dest mode\n");
		kfree_skb(skb);
		return NET_RX_DROP;
	}

	spin_unlock_bh(&sdata->mib_lock);

	skb->dev = sdata->dev;

	rc = mac802154_llsec_decrypt(&sdata->sec, skb);
	if (rc) {
		pr_debug("decryption failed: %i\n", rc);
		goto fail;
	}

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

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

fail:
	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,
				       struct ieee802154_hdr *hdr)
{
	int hlen;
	struct ieee802154_mac_cb *cb = mac_cb_init(skb);

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

	cb->type = hdr->fc.type;
	cb->ackreq = hdr->fc.ack_request;
	cb->secen = hdr->fc.security_enabled;

	mac802154_print_addr("destination", &hdr->dest);
	mac802154_print_addr("source", &hdr->source);

	cb->source = hdr->source;
	cb->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 0;
}

void mac802154_wpans_rx(struct ieee802154_local *local, struct sk_buff *skb)
{
	int ret;
	struct ieee802154_sub_if_data *sdata;
	struct ieee802154_hdr hdr;

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

	rcu_read_lock();
	list_for_each_entry_rcu(sdata, &local->interfaces, list) {
		if (sdata->type != IEEE802154_DEV_WPAN ||
		    !netif_running(sdata->dev))
			continue;

		mac802154_subif_frame(sdata, skb, &hdr);
		skb = NULL;
		break;
	}
	rcu_read_unlock();

	if (skb)
		kfree_skb(skb);
}
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	*
32bad7e3	13	* Written by:
	14	* Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
	15	* Sergey Lapin <slapin@ossfans.org>
	16	* Maxim Gorbachyov <maxim.gorbachev@siemens.com>
	17	* Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
	18	*/
	19
	20	#include <linux/netdevice.h>
	21	#include <linux/module.h>
	22	#include <linux/if_arp.h>
4ca24aca	23	#include <linux/ieee802154.h>
32bad7e3	24
	25	#include <net/rtnetlink.h>
	26	#include <linux/nl802154.h>
	27	#include <net/af_ieee802154.h>
	28	#include <net/mac802154.h>
	29	#include <net/ieee802154_netdev.h>
5ad60d36	30	#include <net/cfg802154.h>
32bad7e3	31
0f1556bc	32	#include "ieee802154_i.h"
32bad7e3	33
9b0bb4a8 PB	34	static int mac802154_wpan_update_llsec(struct net_device *dev)
9b0bb4a8 PB	35	{
036562f9	36	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
9b0bb4a8 PB	37	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
	38	int rc = 0;
	39
	40	if (ops->llsec) {
	41	struct ieee802154_llsec_params params;
	42	int changed = 0;
	43
036562f9	44	params.pan_id = sdata->pan_id;
9b0bb4a8 PB	45	changed \|= IEEE802154_LLSEC_PARAM_PAN_ID;
9b0bb4a8 PB	46
036562f9	47	params.hwaddr = sdata->extended_addr;
9b0bb4a8 PB	48	changed \|= IEEE802154_LLSEC_PARAM_HWADDR;
	49
	50	rc = ops->llsec->set_params(dev, &params, changed);
	51	}
	52
	53	return rc;
	54	}
	55
32bad7e3	56	static int
	57	mac802154_wpan_ioctl(struct net_device dev, struct ifreq ifr, int cmd)
	58	{
036562f9	59	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
32bad7e3	60	struct sockaddr_ieee802154 *sa =
	61	(struct sockaddr_ieee802154 *)&ifr->ifr_addr;
	62	int err = -ENOIOCTLCMD;
	63
036562f9	64	spin_lock_bh(&sdata->mib_lock);
32bad7e3	65
	66	switch (cmd) {
	67	case SIOCGIFADDR:
b70ab2e8 PB	68	{
	69	u16 pan_id, short_addr;
	70
036562f9 AA	71	pan_id = le16_to_cpu(sdata->pan_id);
036562f9 AA	72	short_addr = le16_to_cpu(sdata->short_addr);
b70ab2e8 PB	73	if (pan_id == IEEE802154_PANID_BROADCAST \|\|
b70ab2e8 PB	74	short_addr == IEEE802154_ADDR_BROADCAST) {
32bad7e3	75	err = -EADDRNOTAVAIL;
	76	break;
	77	}
	78
	79	sa->family = AF_IEEE802154;
	80	sa->addr.addr_type = IEEE802154_ADDR_SHORT;
b70ab2e8 PB	81	sa->addr.pan_id = pan_id;
b70ab2e8 PB	82	sa->addr.short_addr = short_addr;
32bad7e3	83
	84	err = 0;
	85	break;
b70ab2e8	86	}
32bad7e3	87	case SIOCSIFADDR:
32bad7e3	88	dev_warn(&dev->dev,
9b13494c	89	"Using DEBUGing ioctl SIOCSIFADDR isn't recommended!\n");
32bad7e3	90	if (sa->family != AF_IEEE802154 \|\|
	91	sa->addr.addr_type != IEEE802154_ADDR_SHORT \|\|
	92	sa->addr.pan_id == IEEE802154_PANID_BROADCAST \|\|
	93	sa->addr.short_addr == IEEE802154_ADDR_BROADCAST \|\|
	94	sa->addr.short_addr == IEEE802154_ADDR_UNDEF) {
	95	err = -EINVAL;
	96	break;
	97	}
	98
036562f9 AA	99	sdata->pan_id = cpu_to_le16(sa->addr.pan_id);
036562f9 AA	100	sdata->short_addr = cpu_to_le16(sa->addr.short_addr);
32bad7e3	101
9b0bb4a8	102	err = mac802154_wpan_update_llsec(dev);
32bad7e3	103	break;
	104	}
	105
036562f9	106	spin_unlock_bh(&sdata->mib_lock);
32bad7e3	107	return err;
	108	}
	109
	110	static int mac802154_wpan_mac_addr(struct net_device dev, void p)
	111	{
	112	struct sockaddr *addr = p;
	113
	114	if (netif_running(dev))
	115	return -EBUSY;
	116
	117	/* FIXME: validate addr */
	118	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
	119	mac802154_dev_set_ieee_addr(dev);
9b0bb4a8	120	return mac802154_wpan_update_llsec(dev);
32bad7e3	121	}
32bad7e3	122
e462ded6 PB	123	int mac802154_set_mac_params(struct net_device *dev,
	124	const struct ieee802154_mac_params *params)
	125	{
036562f9	126	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
e462ded6	127
d98be45b	128	mutex_lock(&sdata->local->iflist_mtx);
036562f9	129	sdata->mac_params = *params;
d98be45b	130	mutex_unlock(&sdata->local->iflist_mtx);
e462ded6 PB	131
	132	return 0;
	133	}
	134
	135	void mac802154_get_mac_params(struct net_device *dev,
	136	struct ieee802154_mac_params *params)
	137	{
036562f9	138	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
e462ded6	139
d98be45b	140	mutex_lock(&sdata->local->iflist_mtx);
036562f9	141	*params = sdata->mac_params;
d98be45b	142	mutex_unlock(&sdata->local->iflist_mtx);
e462ded6 PB	143	}
e462ded6 PB	144
6ef0023a	145	static int mac802154_wpan_open(struct net_device *dev)
e462ded6 PB	146	{
e462ded6 PB	147	int rc;
036562f9	148	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
04e850fe	149	struct wpan_phy *phy = sdata->local->phy;
e462ded6 PB	150
	151	rc = mac802154_slave_open(dev);
	152	if (rc < 0)
	153	return rc;
	154
	155	mutex_lock(&phy->pib_lock);
	156
	157	if (phy->set_txpower) {
036562f9	158	rc = phy->set_txpower(phy, sdata->mac_params.transmit_power);
e462ded6 PB	159	if (rc < 0)
	160	goto out;
	161	}
	162
	163	if (phy->set_lbt) {
036562f9	164	rc = phy->set_lbt(phy, sdata->mac_params.lbt);
e462ded6 PB	165	if (rc < 0)
	166	goto out;
	167	}
	168
	169	if (phy->set_cca_mode) {
036562f9	170	rc = phy->set_cca_mode(phy, sdata->mac_params.cca_mode);
e462ded6 PB	171	if (rc < 0)
	172	goto out;
	173	}
	174
	175	if (phy->set_cca_ed_level) {
036562f9	176	rc = phy->set_cca_ed_level(phy, sdata->mac_params.cca_ed_level);
e462ded6 PB	177	if (rc < 0)
	178	goto out;
	179	}
	180
	181	if (phy->set_csma_params) {
036562f9 AA	182	rc = phy->set_csma_params(phy, sdata->mac_params.min_be,
	183	sdata->mac_params.max_be,
	184	sdata->mac_params.csma_retries);
e462ded6 PB	185	if (rc < 0)
	186	goto out;
	187	}
	188
	189	if (phy->set_frame_retries) {
	190	rc = phy->set_frame_retries(phy,
036562f9	191	sdata->mac_params.frame_retries);
e462ded6 PB	192	if (rc < 0)
	193	goto out;
	194	}
	195
	196	mutex_unlock(&phy->pib_lock);
	197	return 0;
	198
	199	out:
	200	mutex_unlock(&phy->pib_lock);
	201	return rc;
	202	}
	203
036562f9	204	static int mac802154_set_header_security(struct ieee802154_sub_if_data *sdata,
f30be4d5 PB	205	struct ieee802154_hdr *hdr,
	206	const struct ieee802154_mac_cb *cb)
	207	{
	208	struct ieee802154_llsec_params params;
	209	u8 level;
	210
036562f9	211	mac802154_llsec_get_params(&sdata->sec, &params);
f30be4d5 PB	212
	213	if (!params.enabled && cb->secen_override && cb->secen)
	214	return -EINVAL;
	215	if (!params.enabled \|\|
	216	(cb->secen_override && !cb->secen) \|\|
	217	!params.out_level)
	218	return 0;
	219	if (cb->seclevel_override && !cb->seclevel)
	220	return -EINVAL;
	221
	222	level = cb->seclevel_override ? cb->seclevel : params.out_level;
	223
	224	hdr->fc.security_enabled = 1;
	225	hdr->sec.level = level;
	226	hdr->sec.key_id_mode = params.out_key.mode;
	227	if (params.out_key.mode == IEEE802154_SCF_KEY_SHORT_INDEX)
	228	hdr->sec.short_src = params.out_key.short_source;
	229	else if (params.out_key.mode == IEEE802154_SCF_KEY_HW_INDEX)
	230	hdr->sec.extended_src = params.out_key.extended_source;
	231	hdr->sec.key_id = params.out_key.id;
	232
	233	return 0;
	234	}
	235
32bad7e3	236	static int mac802154_header_create(struct sk_buff *skb,
	237	struct net_device *dev,
	238	unsigned short type,
e6278d92 PB	239	const void *daddr,
e6278d92 PB	240	const void *saddr,
32bad7e3	241	unsigned len)
32bad7e3	242	{
e6278d92	243	struct ieee802154_hdr hdr;
036562f9	244	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
32edc40a	245	struct ieee802154_mac_cb *cb = mac_cb(skb);
e6278d92	246	int hlen;
32bad7e3	247
	248	if (!daddr)
	249	return -EINVAL;
	250
e6278d92	251	memset(&hdr.fc, 0, sizeof(hdr.fc));
32edc40a PB	252	hdr.fc.type = cb->type;
	253	hdr.fc.security_enabled = cb->secen;
	254	hdr.fc.ack_request = cb->ackreq;
	255	hdr.seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
32bad7e3	256
036562f9	257	if (mac802154_set_header_security(sdata, &hdr, cb) < 0)
f30be4d5 PB	258	return -EINVAL;
f30be4d5 PB	259
32bad7e3	260	if (!saddr) {
036562f9	261	spin_lock_bh(&sdata->mib_lock);
32bad7e3	262
036562f9 AA	263	if (sdata->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) \|\|
	264	sdata->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) \|\|
	265	sdata->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
e6278d92	266	hdr.source.mode = IEEE802154_ADDR_LONG;
036562f9	267	hdr.source.extended_addr = sdata->extended_addr;
32bad7e3	268	} else {
e6278d92	269	hdr.source.mode = IEEE802154_ADDR_SHORT;
036562f9	270	hdr.source.short_addr = sdata->short_addr;
32bad7e3	271	}
32bad7e3	272
036562f9	273	hdr.source.pan_id = sdata->pan_id;
32bad7e3	274
036562f9	275	spin_unlock_bh(&sdata->mib_lock);
e6278d92 PB	276	} else {
e6278d92 PB	277	hdr.source = (const struct ieee802154_addr )saddr;
32bad7e3	278	}
32bad7e3	279
e6278d92	280	hdr.dest = (const struct ieee802154_addr )daddr;
32bad7e3	281
e6278d92 PB	282	hlen = ieee802154_hdr_push(skb, &hdr);
	283	if (hlen < 0)
	284	return -EINVAL;
32bad7e3	285
3e69162e	286	skb_reset_mac_header(skb);
e6278d92	287	skb->mac_len = hlen;
32bad7e3	288
8c84296f	289	if (len > ieee802154_max_payload(&hdr))
d1d7358e PB	290	return -EMSGSIZE;
d1d7358e PB	291
e6278d92	292	return hlen;
32bad7e3	293	}
	294
	295	static int
	296	mac802154_header_parse(const struct sk_buff skb, unsigned char haddr)
	297	{
e6278d92 PB	298	struct ieee802154_hdr hdr;
e6278d92 PB	299	struct ieee802154_addr addr = (struct ieee802154_addr )haddr;
32bad7e3	300
e6278d92 PB	301	if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0) {
	302	pr_debug("malformed packet\n");
	303	return 0;
32bad7e3	304	}
32bad7e3	305
e6278d92 PB	306	*addr = hdr.source;
e6278d92 PB	307	return sizeof(*addr);
32bad7e3	308	}
	309
	310	static netdev_tx_t
	311	mac802154_wpan_xmit(struct sk_buff skb, struct net_device dev)
	312	{
036562f9	313	struct ieee802154_sub_if_data *sdata;
32bad7e3	314	u8 chan, page;
f30be4d5	315	int rc;
32bad7e3	316
036562f9	317	sdata = netdev_priv(dev);
32bad7e3	318
036562f9 AA	319	spin_lock_bh(&sdata->mib_lock);
	320	chan = sdata->chan;
	321	page = sdata->page;
	322	spin_unlock_bh(&sdata->mib_lock);
32bad7e3	323
	324	if (chan == MAC802154_CHAN_NONE \|\|
	325	page >= WPAN_NUM_PAGES \|\|
fcefbe9f AO	326	chan >= WPAN_NUM_CHANNELS) {
fcefbe9f AO	327	kfree_skb(skb);
32bad7e3	328	return NETDEV_TX_OK;
fcefbe9f	329	}
32bad7e3	330
036562f9	331	rc = mac802154_llsec_encrypt(&sdata->sec, skb);
f30be4d5 PB	332	if (rc) {
	333	pr_warn("encryption failed: %i\n", rc);
	334	kfree_skb(skb);
	335	return NETDEV_TX_OK;
	336	}
	337
32bad7e3	338	skb->skb_iif = dev->ifindex;
	339	dev->stats.tx_packets++;
	340	dev->stats.tx_bytes += skb->len;
	341
04e850fe	342	return mac802154_tx(sdata->local, skb, page, chan);
32bad7e3	343	}
	344
	345	static struct header_ops mac802154_header_ops = {
	346	.create = mac802154_header_create,
	347	.parse = mac802154_header_parse,
	348	};
	349
	350	static const struct net_device_ops mac802154_wpan_ops = {
e462ded6	351	.ndo_open = mac802154_wpan_open,
32bad7e3	352	.ndo_stop = mac802154_slave_close,
	353	.ndo_start_xmit = mac802154_wpan_xmit,
	354	.ndo_do_ioctl = mac802154_wpan_ioctl,
	355	.ndo_set_mac_address = mac802154_wpan_mac_addr,
	356	};
	357
f30be4d5 PB	358	static void mac802154_wpan_free(struct net_device *dev)
f30be4d5 PB	359	{
036562f9	360	struct ieee802154_sub_if_data *sdata = netdev_priv(dev);
f30be4d5	361
036562f9	362	mac802154_llsec_destroy(&sdata->sec);
f30be4d5 PB	363
	364	free_netdev(dev);
	365	}
	366
32bad7e3	367	void mac802154_wpan_setup(struct net_device *dev)
32bad7e3	368	{
036562f9	369	struct ieee802154_sub_if_data *sdata;
32bad7e3	370
	371	dev->addr_len = IEEE802154_ADDR_LEN;
	372	memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
	373
	374	dev->hard_header_len = MAC802154_FRAME_HARD_HEADER_LEN;
	375	dev->header_ops = &mac802154_header_ops;
f30be4d5	376	dev->needed_tailroom = 2 + 16; /* FCS + MIC */
32bad7e3	377	dev->mtu = IEEE802154_MTU;
e937f583	378	dev->tx_queue_len = 300;
32bad7e3	379	dev->type = ARPHRD_IEEE802154;
	380	dev->flags = IFF_NOARP \| IFF_BROADCAST;
	381	dev->watchdog_timeo = 0;
	382
f30be4d5	383	dev->destructor = mac802154_wpan_free;
32bad7e3	384	dev->netdev_ops = &mac802154_wpan_ops;
	385	dev->ml_priv = &mac802154_mlme_wpan;
	386
036562f9 AA	387	sdata = netdev_priv(dev);
036562f9 AA	388	sdata->type = IEEE802154_DEV_WPAN;
32bad7e3	389
036562f9 AA	390	sdata->chan = MAC802154_CHAN_NONE;
036562f9 AA	391	sdata->page = 0;
32bad7e3	392
036562f9 AA	393	spin_lock_init(&sdata->mib_lock);
036562f9 AA	394	mutex_init(&sdata->sec_mtx);
32bad7e3	395
036562f9 AA	396	get_random_bytes(&sdata->bsn, 1);
036562f9 AA	397	get_random_bytes(&sdata->dsn, 1);
32bad7e3	398
e462ded6	399	/* defaults per 802.15.4-2011 */
036562f9 AA	400	sdata->mac_params.min_be = 3;
	401	sdata->mac_params.max_be = 5;
	402	sdata->mac_params.csma_retries = 4;
	403	/* for compatibility, actual default is 3 */
	404	sdata->mac_params.frame_retries = -1;
e462ded6	405
036562f9 AA	406	sdata->pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
036562f9 AA	407	sdata->short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
f30be4d5	408
036562f9	409	mac802154_llsec_init(&sdata->sec);
32bad7e3	410	}
	411
	412	static int mac802154_process_data(struct net_device dev, struct sk_buff skb)
	413	{
5ff3fec6	414	return netif_rx_ni(skb);
32bad7e3	415	}
	416
	417	static int
036562f9	418	mac802154_subif_frame(struct ieee802154_sub_if_data sdata, struct sk_buff skb,
f30be4d5	419	const struct ieee802154_hdr *hdr)
32bad7e3	420	{
ae531b94	421	__le16 span, sshort;
f30be4d5	422	int rc;
b70ab2e8	423
32bad7e3	424	pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
	425
	426	spin_lock_bh(&sdata->mib_lock);
	427
ae531b94 PB	428	span = sdata->pan_id;
ae531b94 PB	429	sshort = sdata->short_addr;
b70ab2e8	430
ae531b94	431	switch (mac_cb(skb)->dest.mode) {
32bad7e3	432	case IEEE802154_ADDR_NONE:
ae531b94	433	if (mac_cb(skb)->dest.mode != IEEE802154_ADDR_NONE)
32bad7e3	434	/* FIXME: check if we are PAN coordinator */
	435	skb->pkt_type = PACKET_OTHERHOST;
	436	else
	437	/* ACK comes with both addresses empty */
	438	skb->pkt_type = PACKET_HOST;
	439	break;
	440	case IEEE802154_ADDR_LONG:
ae531b94 PB	441	if (mac_cb(skb)->dest.pan_id != span &&
ae531b94 PB	442	mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
32bad7e3	443	skb->pkt_type = PACKET_OTHERHOST;
ae531b94	444	else if (mac_cb(skb)->dest.extended_addr == sdata->extended_addr)
32bad7e3	445	skb->pkt_type = PACKET_HOST;
	446	else
	447	skb->pkt_type = PACKET_OTHERHOST;
	448	break;
	449	case IEEE802154_ADDR_SHORT:
ae531b94 PB	450	if (mac_cb(skb)->dest.pan_id != span &&
ae531b94 PB	451	mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
32bad7e3	452	skb->pkt_type = PACKET_OTHERHOST;
ae531b94	453	else if (mac_cb(skb)->dest.short_addr == sshort)
32bad7e3	454	skb->pkt_type = PACKET_HOST;
ae531b94 PB	455	else if (mac_cb(skb)->dest.short_addr ==
ae531b94 PB	456	cpu_to_le16(IEEE802154_ADDR_BROADCAST))
32bad7e3	457	skb->pkt_type = PACKET_BROADCAST;
	458	else
	459	skb->pkt_type = PACKET_OTHERHOST;
	460	break;
	461	default:
6e361d6f MT	462	spin_unlock_bh(&sdata->mib_lock);
	463	pr_debug("invalid dest mode\n");
	464	kfree_skb(skb);
	465	return NET_RX_DROP;
32bad7e3	466	}
	467
	468	spin_unlock_bh(&sdata->mib_lock);
	469
	470	skb->dev = sdata->dev;
	471
f30be4d5 PB	472	rc = mac802154_llsec_decrypt(&sdata->sec, skb);
	473	if (rc) {
	474	pr_debug("decryption failed: %i\n", rc);
b288a496	475	goto fail;
f30be4d5 PB	476	}
f30be4d5 PB	477
32bad7e3	478	sdata->dev->stats.rx_packets++;
	479	sdata->dev->stats.rx_bytes += skb->len;
	480
32edc40a	481	switch (mac_cb(skb)->type) {
32bad7e3	482	case IEEE802154_FC_TYPE_DATA:
	483	return mac802154_process_data(sdata->dev, skb);
	484	default:
2cc33c7e	485	pr_warn("ieee802154: bad frame received (type = %d)\n",
32edc40a	486	mac_cb(skb)->type);
b288a496	487	goto fail;
32bad7e3	488	}
b288a496 VB	489
	490	fail:
	491	kfree_skb(skb);
	492	return NET_RX_DROP;
32bad7e3	493	}
32bad7e3	494
e6278d92 PB	495	static void mac802154_print_addr(const char *name,
e6278d92 PB	496	const struct ieee802154_addr *addr)
32bad7e3	497	{
e6278d92 PB	498	if (addr->mode == IEEE802154_ADDR_NONE)
e6278d92 PB	499	pr_debug("%s not present\n", name);
32bad7e3	500
e6278d92 PB	501	pr_debug("%s PAN ID: %04x\n", name, le16_to_cpu(addr->pan_id));
	502	if (addr->mode == IEEE802154_ADDR_SHORT) {
	503	pr_debug("%s is short: %04x\n", name,
	504	le16_to_cpu(addr->short_addr));
	505	} else {
	506	u64 hw = swab64((__force u64) addr->extended_addr);
32bad7e3	507
e6278d92 PB	508	pr_debug("%s is hardware: %8phC\n", name, &hw);
	509	}
	510	}
32bad7e3	511
f30be4d5 PB	512	static int mac802154_parse_frame_start(struct sk_buff *skb,
f30be4d5 PB	513	struct ieee802154_hdr *hdr)
e6278d92	514	{
e6278d92	515	int hlen;
32edc40a	516	struct ieee802154_mac_cb *cb = mac_cb_init(skb);
32bad7e3	517
f30be4d5	518	hlen = ieee802154_hdr_pull(skb, hdr);
e6278d92 PB	519	if (hlen < 0)
e6278d92 PB	520	return -EINVAL;
32bad7e3	521
e6278d92	522	skb->mac_len = hlen;
32bad7e3	523
f30be4d5 PB	524	pr_debug("fc: %04x dsn: %02x\n", le16_to_cpup((__le16 *)&hdr->fc),
f30be4d5 PB	525	hdr->seq);
32bad7e3	526
f30be4d5 PB	527	cb->type = hdr->fc.type;
	528	cb->ackreq = hdr->fc.ack_request;
	529	cb->secen = hdr->fc.security_enabled;
32bad7e3	530
f30be4d5 PB	531	mac802154_print_addr("destination", &hdr->dest);
f30be4d5 PB	532	mac802154_print_addr("source", &hdr->source);
32bad7e3	533
f30be4d5 PB	534	cb->source = hdr->source;
f30be4d5 PB	535	cb->dest = hdr->dest;
ae531b94	536
f30be4d5	537	if (hdr->fc.security_enabled) {
e6278d92	538	u64 key;
32bad7e3	539
f30be4d5	540	pr_debug("seclevel %i\n", hdr->sec.level);
32bad7e3	541
f30be4d5	542	switch (hdr->sec.key_id_mode) {
e6278d92 PB	543	case IEEE802154_SCF_KEY_IMPLICIT:
	544	pr_debug("implicit key\n");
	545	break;
32bad7e3	546
e6278d92	547	case IEEE802154_SCF_KEY_INDEX:
f30be4d5	548	pr_debug("key %02x\n", hdr->sec.key_id);
e6278d92	549	break;
32bad7e3	550
e6278d92 PB	551	case IEEE802154_SCF_KEY_SHORT_INDEX:
e6278d92 PB	552	pr_debug("key %04x:%04x %02x\n",
f30be4d5 PB	553	le32_to_cpu(hdr->sec.short_src) >> 16,
	554	le32_to_cpu(hdr->sec.short_src) & 0xffff,
	555	hdr->sec.key_id);
e6278d92	556	break;
32bad7e3	557
e6278d92	558	case IEEE802154_SCF_KEY_HW_INDEX:
f30be4d5	559	key = swab64((__force u64) hdr->sec.extended_src);
e6278d92	560	pr_debug("key source %8phC %02x\n", &key,
f30be4d5	561	hdr->sec.key_id);
e6278d92	562	break;
32bad7e3	563	}
	564	}
	565
	566	return 0;
32bad7e3	567	}
32bad7e3	568
a5e1ec53	569	void mac802154_wpans_rx(struct ieee802154_local local, struct sk_buff skb)
32bad7e3	570	{
32bad7e3	571	int ret;
036562f9	572	struct ieee802154_sub_if_data *sdata;
f30be4d5	573	struct ieee802154_hdr hdr;
32bad7e3	574
f30be4d5	575	ret = mac802154_parse_frame_start(skb, &hdr);
32bad7e3	576	if (ret) {
32bad7e3	577	pr_debug("got invalid frame\n");
7629d1ea	578	kfree_skb(skb);
32bad7e3	579	return;
	580	}
	581
	582	rcu_read_lock();
d98be45b	583	list_for_each_entry_rcu(sdata, &local->interfaces, list) {
2d3b5b0a PB	584	if (sdata->type != IEEE802154_DEV_WPAN \|\|
2d3b5b0a PB	585	!netif_running(sdata->dev))
32bad7e3	586	continue;
32bad7e3	587
2d3b5b0a PB	588	mac802154_subif_frame(sdata, skb, &hdr);
	589	skb = NULL;
	590	break;
32bad7e3	591	}
32bad7e3	592	rcu_read_unlock();
2d3b5b0a PB	593
	594	if (skb)
	595	kfree_skb(skb);
32bad7e3	596	}