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> |
27 | #include <net/mac802154.h> | |
5ad60d36 | 28 | #include <net/cfg802154.h> |
e1e49b64 | 29 | |
7e57905b | 30 | static int numlbs = 2; |
e1e49b64 | 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); | |
cb97d9a3 MT |
72 | if (newskb) |
73 | ieee802154_rx_irqsafe(priv->hw, newskb, 0xcc); | |
e1e49b64 | 74 | } |
75 | spin_unlock(&priv->lock); | |
76 | } | |
77 | ||
78 | static int | |
5a504397 | 79 | fakelb_hw_xmit(struct ieee802154_hw *hw, struct sk_buff *skb) |
e1e49b64 | 80 | { |
5a504397 | 81 | struct fakelb_dev_priv *priv = hw->priv; |
e1e49b64 | 82 | struct fakelb_priv *fake = priv->fake; |
83 | ||
e1e49b64 | 84 | read_lock_bh(&fake->lock); |
85 | if (priv->list.next == priv->list.prev) { | |
86 | /* we are the only one device */ | |
87 | fakelb_hw_deliver(priv, skb); | |
88 | } else { | |
89 | struct fakelb_dev_priv *dp; | |
90 | list_for_each_entry(dp, &priv->fake->list, list) { | |
91 | if (dp != priv && | |
5a504397 AA |
92 | (dp->hw->phy->current_channel == |
93 | priv->hw->phy->current_channel)) | |
e1e49b64 | 94 | fakelb_hw_deliver(dp, skb); |
95 | } | |
96 | } | |
97 | read_unlock_bh(&fake->lock); | |
98 | ||
99 | return 0; | |
100 | } | |
101 | ||
102 | static int | |
5a504397 AA |
103 | fakelb_hw_start(struct ieee802154_hw *hw) { |
104 | struct fakelb_dev_priv *priv = hw->priv; | |
e1e49b64 | 105 | int ret = 0; |
106 | ||
107 | spin_lock(&priv->lock); | |
108 | if (priv->working) | |
109 | ret = -EBUSY; | |
110 | else | |
111 | priv->working = 1; | |
112 | spin_unlock(&priv->lock); | |
113 | ||
114 | return ret; | |
115 | } | |
116 | ||
117 | static void | |
5a504397 AA |
118 | fakelb_hw_stop(struct ieee802154_hw *hw) { |
119 | struct fakelb_dev_priv *priv = hw->priv; | |
e1e49b64 | 120 | |
121 | spin_lock(&priv->lock); | |
122 | priv->working = 0; | |
123 | spin_unlock(&priv->lock); | |
124 | } | |
125 | ||
16301861 | 126 | static const struct ieee802154_ops fakelb_ops = { |
e1e49b64 | 127 | .owner = THIS_MODULE, |
ed0a5dce | 128 | .xmit_sync = fakelb_hw_xmit, |
e1e49b64 | 129 | .ed = fakelb_hw_ed, |
130 | .set_channel = fakelb_hw_channel, | |
131 | .start = fakelb_hw_start, | |
132 | .stop = fakelb_hw_stop, | |
133 | }; | |
134 | ||
135 | /* Number of dummy devices to be set up by this module. */ | |
136 | module_param(numlbs, int, 0); | |
137 | MODULE_PARM_DESC(numlbs, " number of pseudo devices"); | |
138 | ||
139 | static int fakelb_add_one(struct device *dev, struct fakelb_priv *fake) | |
140 | { | |
141 | struct fakelb_dev_priv *priv; | |
142 | int err; | |
5a504397 | 143 | struct ieee802154_hw *hw; |
e1e49b64 | 144 | |
5a504397 AA |
145 | hw = ieee802154_alloc_hw(sizeof(*priv), &fakelb_ops); |
146 | if (!hw) | |
e1e49b64 | 147 | return -ENOMEM; |
148 | ||
5a504397 AA |
149 | priv = hw->priv; |
150 | priv->hw = hw; | |
e1e49b64 | 151 | |
152 | /* 868 MHz BPSK 802.15.4-2003 */ | |
72f655e4 | 153 | hw->phy->supported.channels[0] |= 1; |
e1e49b64 | 154 | /* 915 MHz BPSK 802.15.4-2003 */ |
72f655e4 | 155 | hw->phy->supported.channels[0] |= 0x7fe; |
e1e49b64 | 156 | /* 2.4 GHz O-QPSK 802.15.4-2003 */ |
72f655e4 | 157 | hw->phy->supported.channels[0] |= 0x7FFF800; |
e1e49b64 | 158 | /* 868 MHz ASK 802.15.4-2006 */ |
72f655e4 | 159 | hw->phy->supported.channels[1] |= 1; |
e1e49b64 | 160 | /* 915 MHz ASK 802.15.4-2006 */ |
72f655e4 | 161 | hw->phy->supported.channels[1] |= 0x7fe; |
e1e49b64 | 162 | /* 868 MHz O-QPSK 802.15.4-2006 */ |
72f655e4 | 163 | hw->phy->supported.channels[2] |= 1; |
e1e49b64 | 164 | /* 915 MHz O-QPSK 802.15.4-2006 */ |
72f655e4 | 165 | hw->phy->supported.channels[2] |= 0x7fe; |
e1e49b64 | 166 | /* 2.4 GHz CSS 802.15.4a-2007 */ |
72f655e4 | 167 | hw->phy->supported.channels[3] |= 0x3fff; |
e1e49b64 | 168 | /* UWB Sub-gigahertz 802.15.4a-2007 */ |
72f655e4 | 169 | hw->phy->supported.channels[4] |= 1; |
e1e49b64 | 170 | /* UWB Low band 802.15.4a-2007 */ |
72f655e4 | 171 | hw->phy->supported.channels[4] |= 0x1e; |
e1e49b64 | 172 | /* UWB High band 802.15.4a-2007 */ |
72f655e4 | 173 | hw->phy->supported.channels[4] |= 0xffe0; |
e1e49b64 | 174 | /* 750 MHz O-QPSK 802.15.4c-2009 */ |
72f655e4 | 175 | hw->phy->supported.channels[5] |= 0xf; |
e1e49b64 | 176 | /* 750 MHz MPSK 802.15.4c-2009 */ |
72f655e4 | 177 | hw->phy->supported.channels[5] |= 0xf0; |
e1e49b64 | 178 | /* 950 MHz BPSK 802.15.4d-2009 */ |
72f655e4 | 179 | hw->phy->supported.channels[6] |= 0x3ff; |
e1e49b64 | 180 | /* 950 MHz GFSK 802.15.4d-2009 */ |
72f655e4 | 181 | hw->phy->supported.channels[6] |= 0x3ffc00; |
e1e49b64 | 182 | |
183 | INIT_LIST_HEAD(&priv->list); | |
184 | priv->fake = fake; | |
185 | ||
186 | spin_lock_init(&priv->lock); | |
187 | ||
5a504397 | 188 | hw->parent = dev; |
e1e49b64 | 189 | |
5a504397 | 190 | err = ieee802154_register_hw(hw); |
e1e49b64 | 191 | if (err) |
192 | goto err_reg; | |
193 | ||
194 | write_lock_bh(&fake->lock); | |
195 | list_add_tail(&priv->list, &fake->list); | |
196 | write_unlock_bh(&fake->lock); | |
197 | ||
198 | return 0; | |
199 | ||
200 | err_reg: | |
5a504397 | 201 | ieee802154_free_hw(priv->hw); |
e1e49b64 | 202 | return err; |
203 | } | |
204 | ||
205 | static void fakelb_del(struct fakelb_dev_priv *priv) | |
206 | { | |
207 | write_lock_bh(&priv->fake->lock); | |
208 | list_del(&priv->list); | |
209 | write_unlock_bh(&priv->fake->lock); | |
210 | ||
5a504397 AA |
211 | ieee802154_unregister_hw(priv->hw); |
212 | ieee802154_free_hw(priv->hw); | |
e1e49b64 | 213 | } |
214 | ||
bb1f4606 | 215 | static int fakelb_probe(struct platform_device *pdev) |
e1e49b64 | 216 | { |
217 | struct fakelb_priv *priv; | |
3186d3d7 | 218 | struct fakelb_dev_priv *dp, *tmp; |
e1e49b64 | 219 | int err = -ENOMEM; |
220 | int i; | |
221 | ||
12b5c38f HS |
222 | priv = devm_kzalloc(&pdev->dev, sizeof(struct fakelb_priv), |
223 | GFP_KERNEL); | |
e1e49b64 | 224 | if (!priv) |
225 | goto err_alloc; | |
226 | ||
227 | INIT_LIST_HEAD(&priv->list); | |
228 | rwlock_init(&priv->lock); | |
229 | ||
230 | for (i = 0; i < numlbs; i++) { | |
231 | err = fakelb_add_one(&pdev->dev, priv); | |
232 | if (err < 0) | |
233 | goto err_slave; | |
234 | } | |
235 | ||
236 | platform_set_drvdata(pdev, priv); | |
237 | dev_info(&pdev->dev, "added ieee802154 hardware\n"); | |
238 | return 0; | |
239 | ||
240 | err_slave: | |
3186d3d7 | 241 | list_for_each_entry_safe(dp, tmp, &priv->list, list) |
e1e49b64 | 242 | fakelb_del(dp); |
e1e49b64 | 243 | err_alloc: |
244 | return err; | |
245 | } | |
246 | ||
bb1f4606 | 247 | static int fakelb_remove(struct platform_device *pdev) |
e1e49b64 | 248 | { |
249 | struct fakelb_priv *priv = platform_get_drvdata(pdev); | |
250 | struct fakelb_dev_priv *dp, *temp; | |
251 | ||
252 | list_for_each_entry_safe(dp, temp, &priv->list, list) | |
253 | fakelb_del(dp); | |
e1e49b64 | 254 | |
255 | return 0; | |
256 | } | |
257 | ||
258 | static struct platform_device *ieee802154fake_dev; | |
259 | ||
260 | static struct platform_driver ieee802154fake_driver = { | |
261 | .probe = fakelb_probe, | |
bb1f4606 | 262 | .remove = fakelb_remove, |
e1e49b64 | 263 | .driver = { |
264 | .name = "ieee802154fakelb", | |
e1e49b64 | 265 | }, |
266 | }; | |
267 | ||
268 | static __init int fakelb_init_module(void) | |
269 | { | |
270 | ieee802154fake_dev = platform_device_register_simple( | |
271 | "ieee802154fakelb", -1, NULL, 0); | |
272 | return platform_driver_register(&ieee802154fake_driver); | |
273 | } | |
274 | ||
275 | static __exit void fake_remove_module(void) | |
276 | { | |
277 | platform_driver_unregister(&ieee802154fake_driver); | |
278 | platform_device_unregister(ieee802154fake_dev); | |
279 | } | |
280 | ||
281 | module_init(fakelb_init_module); | |
282 | module_exit(fake_remove_module); | |
283 | MODULE_LICENSE("GPL"); |