[linux-2.6-block.git] / drivers / net / ieee802154 / fakelb.c

/*
 * Loopback IEEE 802.15.4 interface
 *
 * 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:
 * Sergey Lapin <slapin@ossfans.org>
 * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
 * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
 */

#include <linux/module.h>
#include <linux/timer.h>
#include <linux/platform_device.h>
#include <linux/netdevice.h>
#include <linux/device.h>
#include <linux/spinlock.h>
#include <net/mac802154.h>
#include <net/cfg802154.h>

static int numlbs = 1;

struct fakelb_dev_priv {
	struct ieee802154_hw *hw;

	struct list_head list;
	struct fakelb_priv *fake;

	spinlock_t lock;
	bool working;
};

struct fakelb_priv {
	struct list_head list;
	rwlock_t lock;
};

static int
fakelb_hw_ed(struct ieee802154_hw *hw, u8 *level)
{
	BUG_ON(!level);
	*level = 0xbe;

	return 0;
}

static int
fakelb_hw_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
{
	pr_debug("set channel to %d\n", channel);

	return 0;
}

static void
fakelb_hw_deliver(struct fakelb_dev_priv *priv, struct sk_buff *skb)
{
	struct sk_buff *newskb;

	spin_lock(&priv->lock);
	if (priv->working) {
		newskb = pskb_copy(skb, GFP_ATOMIC);
		ieee802154_rx_irqsafe(priv->hw, newskb, 0xcc);
	}
	spin_unlock(&priv->lock);
}

static int
fakelb_hw_xmit(struct ieee802154_hw *hw, struct sk_buff *skb)
{
	struct fakelb_dev_priv *priv = hw->priv;
	struct fakelb_priv *fake = priv->fake;

	read_lock_bh(&fake->lock);
	if (priv->list.next == priv->list.prev) {
		/* we are the only one device */
		fakelb_hw_deliver(priv, skb);
	} else {
		struct fakelb_dev_priv *dp;
		list_for_each_entry(dp, &priv->fake->list, list) {
			if (dp != priv &&
			    (dp->hw->phy->current_channel ==
			     priv->hw->phy->current_channel))
				fakelb_hw_deliver(dp, skb);
		}
	}
	read_unlock_bh(&fake->lock);

	return 0;
}

static int
fakelb_hw_start(struct ieee802154_hw *hw) {
	struct fakelb_dev_priv *priv = hw->priv;
	int ret = 0;

	spin_lock(&priv->lock);
	if (priv->working)
		ret = -EBUSY;
	else
		priv->working = 1;
	spin_unlock(&priv->lock);

	return ret;
}

static void
fakelb_hw_stop(struct ieee802154_hw *hw) {
	struct fakelb_dev_priv *priv = hw->priv;

	spin_lock(&priv->lock);
	priv->working = 0;
	spin_unlock(&priv->lock);
}

static const struct ieee802154_ops fakelb_ops = {
	.owner = THIS_MODULE,
	.xmit_sync = fakelb_hw_xmit,
	.ed = fakelb_hw_ed,
	.set_channel = fakelb_hw_channel,
	.start = fakelb_hw_start,
	.stop = fakelb_hw_stop,
};

/* Number of dummy devices to be set up by this module. */
module_param(numlbs, int, 0);
MODULE_PARM_DESC(numlbs, " number of pseudo devices");

static int fakelb_add_one(struct device *dev, struct fakelb_priv *fake)
{
	struct fakelb_dev_priv *priv;
	int err;
	struct ieee802154_hw *hw;

	hw = ieee802154_alloc_hw(sizeof(*priv), &fakelb_ops);
	if (!hw)
		return -ENOMEM;

	priv = hw->priv;
	priv->hw = hw;

	/* 868 MHz BPSK	802.15.4-2003 */
	hw->phy->channels_supported[0] |= 1;
	/* 915 MHz BPSK	802.15.4-2003 */
	hw->phy->channels_supported[0] |= 0x7fe;
	/* 2.4 GHz O-QPSK 802.15.4-2003 */
	hw->phy->channels_supported[0] |= 0x7FFF800;
	/* 868 MHz ASK 802.15.4-2006 */
	hw->phy->channels_supported[1] |= 1;
	/* 915 MHz ASK 802.15.4-2006 */
	hw->phy->channels_supported[1] |= 0x7fe;
	/* 868 MHz O-QPSK 802.15.4-2006 */
	hw->phy->channels_supported[2] |= 1;
	/* 915 MHz O-QPSK 802.15.4-2006 */
	hw->phy->channels_supported[2] |= 0x7fe;
	/* 2.4 GHz CSS 802.15.4a-2007 */
	hw->phy->channels_supported[3] |= 0x3fff;
	/* UWB Sub-gigahertz 802.15.4a-2007 */
	hw->phy->channels_supported[4] |= 1;
	/* UWB Low band 802.15.4a-2007 */
	hw->phy->channels_supported[4] |= 0x1e;
	/* UWB High band 802.15.4a-2007 */
	hw->phy->channels_supported[4] |= 0xffe0;
	/* 750 MHz O-QPSK 802.15.4c-2009 */
	hw->phy->channels_supported[5] |= 0xf;
	/* 750 MHz MPSK 802.15.4c-2009 */
	hw->phy->channels_supported[5] |= 0xf0;
	/* 950 MHz BPSK 802.15.4d-2009 */
	hw->phy->channels_supported[6] |= 0x3ff;
	/* 950 MHz GFSK 802.15.4d-2009 */
	hw->phy->channels_supported[6] |= 0x3ffc00;

	INIT_LIST_HEAD(&priv->list);
	priv->fake = fake;

	spin_lock_init(&priv->lock);

	hw->parent = dev;

	err = ieee802154_register_hw(hw);
	if (err)
		goto err_reg;

	write_lock_bh(&fake->lock);
	list_add_tail(&priv->list, &fake->list);
	write_unlock_bh(&fake->lock);

	return 0;

err_reg:
	ieee802154_free_hw(priv->hw);
	return err;
}

static void fakelb_del(struct fakelb_dev_priv *priv)
{
	write_lock_bh(&priv->fake->lock);
	list_del(&priv->list);
	write_unlock_bh(&priv->fake->lock);

	ieee802154_unregister_hw(priv->hw);
	ieee802154_free_hw(priv->hw);
}

static int fakelb_probe(struct platform_device *pdev)
{
	struct fakelb_priv *priv;
	struct fakelb_dev_priv *dp;
	int err = -ENOMEM;
	int i;

	priv = devm_kzalloc(&pdev->dev, sizeof(struct fakelb_priv),
			    GFP_KERNEL);
	if (!priv)
		goto err_alloc;

	INIT_LIST_HEAD(&priv->list);
	rwlock_init(&priv->lock);

	for (i = 0; i < numlbs; i++) {
		err = fakelb_add_one(&pdev->dev, priv);
		if (err < 0)
			goto err_slave;
	}

	platform_set_drvdata(pdev, priv);
	dev_info(&pdev->dev, "added ieee802154 hardware\n");
	return 0;

err_slave:
	list_for_each_entry(dp, &priv->list, list)
		fakelb_del(dp);
err_alloc:
	return err;
}

static int fakelb_remove(struct platform_device *pdev)
{
	struct fakelb_priv *priv = platform_get_drvdata(pdev);
	struct fakelb_dev_priv *dp, *temp;

	list_for_each_entry_safe(dp, temp, &priv->list, list)
		fakelb_del(dp);

	return 0;
}

static struct platform_device *ieee802154fake_dev;

static struct platform_driver ieee802154fake_driver = {
	.probe = fakelb_probe,
	.remove = fakelb_remove,
	.driver = {
			.name = "ieee802154fakelb",
	},
};

static __init int fakelb_init_module(void)
{
	ieee802154fake_dev = platform_device_register_simple(
			     "ieee802154fakelb", -1, NULL, 0);
	return platform_driver_register(&ieee802154fake_driver);
}

static __exit void fake_remove_module(void)
{
	platform_driver_unregister(&ieee802154fake_driver);
	platform_device_unregister(ieee802154fake_dev);
}

module_init(fakelb_init_module);
module_exit(fake_remove_module);
MODULE_LICENSE("GPL");
Commit	Line	Data
e1e49b64	1	/*
	2	* Loopback IEEE 802.15.4 interface
	3	*
	4	* Copyright 2007-2012 Siemens AG
	5	*
	6	* This program is free software; you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License version 2
	8	* as published by the Free Software Foundation.
	9	*
	10	* This program is distributed in the hope that it will be useful,
	11	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	12	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	13	* GNU General Public License for more details.
	14	*
e1e49b64	15	* Written by:
	16	* Sergey Lapin <slapin@ossfans.org>
	17	* Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
	18	* Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
	19	*/
	20
	21	#include <linux/module.h>
	22	#include <linux/timer.h>
	23	#include <linux/platform_device.h>
	24	#include <linux/netdevice.h>
12b5c38f	25	#include <linux/device.h>
e1e49b64	26	#include <linux/spinlock.h>
e1e49b64	27	#include <net/mac802154.h>
5ad60d36	28	#include <net/cfg802154.h>
e1e49b64	29
	30	static int numlbs = 1;
	31
	32	struct fakelb_dev_priv {
5a504397	33	struct ieee802154_hw *hw;
e1e49b64	34
	35	struct list_head list;
	36	struct fakelb_priv *fake;
	37
	38	spinlock_t lock;
	39	bool working;
	40	};
	41
	42	struct fakelb_priv {
	43	struct list_head list;
	44	rwlock_t lock;
	45	};
	46
	47	static int
5a504397	48	fakelb_hw_ed(struct ieee802154_hw hw, u8 level)
e1e49b64	49	{
e1e49b64	50	BUG_ON(!level);
	51	*level = 0xbe;
	52
	53	return 0;
	54	}
	55
	56	static int
e37d2ec8	57	fakelb_hw_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
e1e49b64	58	{
	59	pr_debug("set channel to %d\n", channel);
	60
e1e49b64	61	return 0;
	62	}
	63
	64	static void
	65	fakelb_hw_deliver(struct fakelb_dev_priv priv, struct sk_buff skb)
	66	{
	67	struct sk_buff *newskb;
	68
	69	spin_lock(&priv->lock);
	70	if (priv->working) {
	71	newskb = pskb_copy(skb, GFP_ATOMIC);
5a504397	72	ieee802154_rx_irqsafe(priv->hw, newskb, 0xcc);
e1e49b64	73	}
	74	spin_unlock(&priv->lock);
	75	}
	76
	77	static int
5a504397	78	fakelb_hw_xmit(struct ieee802154_hw hw, struct sk_buff skb)
e1e49b64	79	{
5a504397	80	struct fakelb_dev_priv *priv = hw->priv;
e1e49b64	81	struct fakelb_priv *fake = priv->fake;
e1e49b64	82
e1e49b64	83	read_lock_bh(&fake->lock);
	84	if (priv->list.next == priv->list.prev) {
	85	/* we are the only one device */
	86	fakelb_hw_deliver(priv, skb);
	87	} else {
	88	struct fakelb_dev_priv *dp;
	89	list_for_each_entry(dp, &priv->fake->list, list) {
	90	if (dp != priv &&
5a504397 AA	91	(dp->hw->phy->current_channel ==
5a504397 AA	92	priv->hw->phy->current_channel))
e1e49b64	93	fakelb_hw_deliver(dp, skb);
	94	}
	95	}
	96	read_unlock_bh(&fake->lock);
	97
	98	return 0;
	99	}
	100
	101	static int
5a504397 AA	102	fakelb_hw_start(struct ieee802154_hw *hw) {
5a504397 AA	103	struct fakelb_dev_priv *priv = hw->priv;
e1e49b64	104	int ret = 0;
	105
	106	spin_lock(&priv->lock);
	107	if (priv->working)
	108	ret = -EBUSY;
	109	else
	110	priv->working = 1;
	111	spin_unlock(&priv->lock);
	112
	113	return ret;
	114	}
	115
	116	static void
5a504397 AA	117	fakelb_hw_stop(struct ieee802154_hw *hw) {
5a504397 AA	118	struct fakelb_dev_priv *priv = hw->priv;
e1e49b64	119
	120	spin_lock(&priv->lock);
	121	priv->working = 0;
	122	spin_unlock(&priv->lock);
	123	}
	124
16301861	125	static const struct ieee802154_ops fakelb_ops = {
e1e49b64	126	.owner = THIS_MODULE,
ed0a5dce	127	.xmit_sync = fakelb_hw_xmit,
e1e49b64	128	.ed = fakelb_hw_ed,
	129	.set_channel = fakelb_hw_channel,
	130	.start = fakelb_hw_start,
	131	.stop = fakelb_hw_stop,
	132	};
	133
	134	/* Number of dummy devices to be set up by this module. */
	135	module_param(numlbs, int, 0);
	136	MODULE_PARM_DESC(numlbs, " number of pseudo devices");
	137
	138	static int fakelb_add_one(struct device dev, struct fakelb_priv fake)
	139	{
	140	struct fakelb_dev_priv *priv;
	141	int err;
5a504397	142	struct ieee802154_hw *hw;
e1e49b64	143
5a504397 AA	144	hw = ieee802154_alloc_hw(sizeof(*priv), &fakelb_ops);
5a504397 AA	145	if (!hw)
e1e49b64	146	return -ENOMEM;
e1e49b64	147
5a504397 AA	148	priv = hw->priv;
5a504397 AA	149	priv->hw = hw;
e1e49b64	150
e1e49b64	151	/* 868 MHz BPSK 802.15.4-2003 */
5a504397	152	hw->phy->channels_supported[0] \|= 1;
e1e49b64	153	/* 915 MHz BPSK 802.15.4-2003 */
5a504397	154	hw->phy->channels_supported[0] \|= 0x7fe;
e1e49b64	155	/* 2.4 GHz O-QPSK 802.15.4-2003 */
5a504397	156	hw->phy->channels_supported[0] \|= 0x7FFF800;
e1e49b64	157	/* 868 MHz ASK 802.15.4-2006 */
5a504397	158	hw->phy->channels_supported[1] \|= 1;
e1e49b64	159	/* 915 MHz ASK 802.15.4-2006 */
5a504397	160	hw->phy->channels_supported[1] \|= 0x7fe;
e1e49b64	161	/* 868 MHz O-QPSK 802.15.4-2006 */
5a504397	162	hw->phy->channels_supported[2] \|= 1;
e1e49b64	163	/* 915 MHz O-QPSK 802.15.4-2006 */
5a504397	164	hw->phy->channels_supported[2] \|= 0x7fe;
e1e49b64	165	/* 2.4 GHz CSS 802.15.4a-2007 */
5a504397	166	hw->phy->channels_supported[3] \|= 0x3fff;
e1e49b64	167	/* UWB Sub-gigahertz 802.15.4a-2007 */
5a504397	168	hw->phy->channels_supported[4] \|= 1;
e1e49b64	169	/* UWB Low band 802.15.4a-2007 */
5a504397	170	hw->phy->channels_supported[4] \|= 0x1e;
e1e49b64	171	/* UWB High band 802.15.4a-2007 */
5a504397	172	hw->phy->channels_supported[4] \|= 0xffe0;
e1e49b64	173	/* 750 MHz O-QPSK 802.15.4c-2009 */
5a504397	174	hw->phy->channels_supported[5] \|= 0xf;
e1e49b64	175	/* 750 MHz MPSK 802.15.4c-2009 */
5a504397	176	hw->phy->channels_supported[5] \|= 0xf0;
e1e49b64	177	/* 950 MHz BPSK 802.15.4d-2009 */
5a504397	178	hw->phy->channels_supported[6] \|= 0x3ff;
e1e49b64	179	/* 950 MHz GFSK 802.15.4d-2009 */
5a504397	180	hw->phy->channels_supported[6] \|= 0x3ffc00;
e1e49b64	181
	182	INIT_LIST_HEAD(&priv->list);
	183	priv->fake = fake;
	184
	185	spin_lock_init(&priv->lock);
	186
5a504397	187	hw->parent = dev;
e1e49b64	188
5a504397	189	err = ieee802154_register_hw(hw);
e1e49b64	190	if (err)
	191	goto err_reg;
	192
	193	write_lock_bh(&fake->lock);
	194	list_add_tail(&priv->list, &fake->list);
	195	write_unlock_bh(&fake->lock);
	196
	197	return 0;
	198
	199	err_reg:
5a504397	200	ieee802154_free_hw(priv->hw);
e1e49b64	201	return err;
	202	}
	203
	204	static void fakelb_del(struct fakelb_dev_priv *priv)
	205	{
	206	write_lock_bh(&priv->fake->lock);
	207	list_del(&priv->list);
	208	write_unlock_bh(&priv->fake->lock);
	209
5a504397 AA	210	ieee802154_unregister_hw(priv->hw);
5a504397 AA	211	ieee802154_free_hw(priv->hw);
e1e49b64	212	}
e1e49b64	213
bb1f4606	214	static int fakelb_probe(struct platform_device *pdev)
e1e49b64	215	{
	216	struct fakelb_priv *priv;
	217	struct fakelb_dev_priv *dp;
	218	int err = -ENOMEM;
	219	int i;
	220
12b5c38f HS	221	priv = devm_kzalloc(&pdev->dev, sizeof(struct fakelb_priv),
12b5c38f HS	222	GFP_KERNEL);
e1e49b64	223	if (!priv)
	224	goto err_alloc;
	225
	226	INIT_LIST_HEAD(&priv->list);
	227	rwlock_init(&priv->lock);
	228
	229	for (i = 0; i < numlbs; i++) {
	230	err = fakelb_add_one(&pdev->dev, priv);
	231	if (err < 0)
	232	goto err_slave;
	233	}
	234
	235	platform_set_drvdata(pdev, priv);
	236	dev_info(&pdev->dev, "added ieee802154 hardware\n");
	237	return 0;
	238
	239	err_slave:
	240	list_for_each_entry(dp, &priv->list, list)
	241	fakelb_del(dp);
e1e49b64	242	err_alloc:
	243	return err;
	244	}
	245
bb1f4606	246	static int fakelb_remove(struct platform_device *pdev)
e1e49b64	247	{
	248	struct fakelb_priv *priv = platform_get_drvdata(pdev);
	249	struct fakelb_dev_priv dp, temp;
	250
	251	list_for_each_entry_safe(dp, temp, &priv->list, list)
	252	fakelb_del(dp);
e1e49b64	253
	254	return 0;
	255	}
	256
	257	static struct platform_device *ieee802154fake_dev;
	258
	259	static struct platform_driver ieee802154fake_driver = {
	260	.probe = fakelb_probe,
bb1f4606	261	.remove = fakelb_remove,
e1e49b64	262	.driver = {
e1e49b64	263	.name = "ieee802154fakelb",
e1e49b64	264	},
	265	};
	266
	267	static __init int fakelb_init_module(void)
	268	{
	269	ieee802154fake_dev = platform_device_register_simple(
	270	"ieee802154fakelb", -1, NULL, 0);
	271	return platform_driver_register(&ieee802154fake_driver);
	272	}
	273
	274	static __exit void fake_remove_module(void)
	275	{
	276	platform_driver_unregister(&ieee802154fake_driver);
	277	platform_device_unregister(ieee802154fake_dev);
	278	}
	279
	280	module_init(fakelb_init_module);
	281	module_exit(fake_remove_module);
	282	MODULE_LICENSE("GPL");