wpan.c revision 2cc33c7e319c10b4e169050b3e21908531b09bf9 
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
38static inline int mac802154_fetch_skb_u8(struct sk_buff *skb, u8 *val)
39{
40	if (unlikely(!pskb_may_pull(skb, 1)))
41		return -EINVAL;
42
43	*val = skb->data[0];
44	skb_pull(skb, 1);
45
46	return 0;
47}
48
49static inline int mac802154_fetch_skb_u16(struct sk_buff *skb, u16 *val)
50{
51	if (unlikely(!pskb_may_pull(skb, 2)))
52		return -EINVAL;
53
54	*val = skb->data[0] | (skb->data[1] << 8);
55	skb_pull(skb, 2);
56
57	return 0;
58}
59
60static inline void mac802154_haddr_copy_swap(u8 *dest, const u8 *src)
61{
62	int i;
63	for (i = 0; i < IEEE802154_ADDR_LEN; i++)
64		dest[IEEE802154_ADDR_LEN - i - 1] = src[i];
65}
66
67static int
68mac802154_wpan_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
69{
70	struct mac802154_sub_if_data *priv = netdev_priv(dev);
71	struct sockaddr_ieee802154 *sa =
72		(struct sockaddr_ieee802154 *)&ifr->ifr_addr;
73	int err = -ENOIOCTLCMD;
74
75	spin_lock_bh(&priv->mib_lock);
76
77	switch (cmd) {
78	case SIOCGIFADDR:
79		if (priv->pan_id == IEEE802154_PANID_BROADCAST ||
80		    priv->short_addr == IEEE802154_ADDR_BROADCAST) {
81			err = -EADDRNOTAVAIL;
82			break;
83		}
84
85		sa->family = AF_IEEE802154;
86		sa->addr.addr_type = IEEE802154_ADDR_SHORT;
87		sa->addr.pan_id = priv->pan_id;
88		sa->addr.short_addr = priv->short_addr;
89
90		err = 0;
91		break;
92	case SIOCSIFADDR:
93		dev_warn(&dev->dev,
94			 "Using DEBUGing ioctl SIOCSIFADDR isn't recommened!\n");
95		if (sa->family != AF_IEEE802154 ||
96		    sa->addr.addr_type != IEEE802154_ADDR_SHORT ||
97		    sa->addr.pan_id == IEEE802154_PANID_BROADCAST ||
98		    sa->addr.short_addr == IEEE802154_ADDR_BROADCAST ||
99		    sa->addr.short_addr == IEEE802154_ADDR_UNDEF) {
100			err = -EINVAL;
101			break;
102		}
103
104		priv->pan_id = sa->addr.pan_id;
105		priv->short_addr = sa->addr.short_addr;
106
107		err = 0;
108		break;
109	}
110
111	spin_unlock_bh(&priv->mib_lock);
112	return err;
113}
114
115static int mac802154_wpan_mac_addr(struct net_device *dev, void *p)
116{
117	struct sockaddr *addr = p;
118
119	if (netif_running(dev))
120		return -EBUSY;
121
122	/* FIXME: validate addr */
123	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
124	mac802154_dev_set_ieee_addr(dev);
125	return 0;
126}
127
128static int mac802154_header_create(struct sk_buff *skb,
129				   struct net_device *dev,
130				   unsigned short type,
131				   const void *_daddr,
132				   const void *_saddr,
133				   unsigned len)
134{
135	const struct ieee802154_addr *saddr = _saddr;
136	const struct ieee802154_addr *daddr = _daddr;
137	struct ieee802154_addr dev_addr;
138	struct mac802154_sub_if_data *priv = netdev_priv(dev);
139	int pos = 2;
140	u8 head[MAC802154_FRAME_HARD_HEADER_LEN];
141	u16 fc;
142
143	if (!daddr)
144		return -EINVAL;
145
146	head[pos++] = mac_cb(skb)->seq; /* DSN/BSN */
147	fc = mac_cb_type(skb);
148	if (mac_cb_is_ackreq(skb))
149		fc |= IEEE802154_FC_ACK_REQ;
150
151	if (!saddr) {
152		spin_lock_bh(&priv->mib_lock);
153
154		if (priv->short_addr == IEEE802154_ADDR_BROADCAST ||
155		    priv->short_addr == IEEE802154_ADDR_UNDEF ||
156		    priv->pan_id == IEEE802154_PANID_BROADCAST) {
157			dev_addr.addr_type = IEEE802154_ADDR_LONG;
158			memcpy(dev_addr.hwaddr, dev->dev_addr,
159			       IEEE802154_ADDR_LEN);
160		} else {
161			dev_addr.addr_type = IEEE802154_ADDR_SHORT;
162			dev_addr.short_addr = priv->short_addr;
163		}
164
165		dev_addr.pan_id = priv->pan_id;
166		saddr = &dev_addr;
167
168		spin_unlock_bh(&priv->mib_lock);
169	}
170
171	if (daddr->addr_type != IEEE802154_ADDR_NONE) {
172		fc |= (daddr->addr_type << IEEE802154_FC_DAMODE_SHIFT);
173
174		head[pos++] = daddr->pan_id & 0xff;
175		head[pos++] = daddr->pan_id >> 8;
176
177		if (daddr->addr_type == IEEE802154_ADDR_SHORT) {
178			head[pos++] = daddr->short_addr & 0xff;
179			head[pos++] = daddr->short_addr >> 8;
180		} else {
181			mac802154_haddr_copy_swap(head + pos, daddr->hwaddr);
182			pos += IEEE802154_ADDR_LEN;
183		}
184	}
185
186	if (saddr->addr_type != IEEE802154_ADDR_NONE) {
187		fc |= (saddr->addr_type << IEEE802154_FC_SAMODE_SHIFT);
188
189		if ((saddr->pan_id == daddr->pan_id) &&
190		    (saddr->pan_id != IEEE802154_PANID_BROADCAST)) {
191			/* PANID compression/intra PAN */
192			fc |= IEEE802154_FC_INTRA_PAN;
193		} else {
194			head[pos++] = saddr->pan_id & 0xff;
195			head[pos++] = saddr->pan_id >> 8;
196		}
197
198		if (saddr->addr_type == IEEE802154_ADDR_SHORT) {
199			head[pos++] = saddr->short_addr & 0xff;
200			head[pos++] = saddr->short_addr >> 8;
201		} else {
202			mac802154_haddr_copy_swap(head + pos, saddr->hwaddr);
203			pos += IEEE802154_ADDR_LEN;
204		}
205	}
206
207	head[0] = fc;
208	head[1] = fc >> 8;
209
210	memcpy(skb_push(skb, pos), head, pos);
211	skb_reset_mac_header(skb);
212	skb->mac_len = pos;
213
214	return pos;
215}
216
217static int
218mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
219{
220	const u8 *hdr = skb_mac_header(skb);
221	const u8 *tail = skb_tail_pointer(skb);
222	struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr;
223	u16 fc;
224	int da_type;
225
226	if (hdr + 3 > tail)
227		goto malformed;
228
229	fc = hdr[0] | (hdr[1] << 8);
230
231	hdr += 3;
232
233	da_type = IEEE802154_FC_DAMODE(fc);
234	addr->addr_type = IEEE802154_FC_SAMODE(fc);
235
236	switch (da_type) {
237	case IEEE802154_ADDR_NONE:
238		if (fc & IEEE802154_FC_INTRA_PAN)
239			goto malformed;
240		break;
241	case IEEE802154_ADDR_LONG:
242		if (fc & IEEE802154_FC_INTRA_PAN) {
243			if (hdr + 2 > tail)
244				goto malformed;
245			addr->pan_id = hdr[0] | (hdr[1] << 8);
246			hdr += 2;
247		}
248
249		if (hdr + IEEE802154_ADDR_LEN > tail)
250			goto malformed;
251
252		hdr += IEEE802154_ADDR_LEN;
253		break;
254	case IEEE802154_ADDR_SHORT:
255		if (fc & IEEE802154_FC_INTRA_PAN) {
256			if (hdr + 2 > tail)
257				goto malformed;
258			addr->pan_id = hdr[0] | (hdr[1] << 8);
259			hdr += 2;
260		}
261
262		if (hdr + 2 > tail)
263			goto malformed;
264
265		hdr += 2;
266		break;
267	default:
268		goto malformed;
269
270	}
271
272	switch (addr->addr_type) {
273	case IEEE802154_ADDR_NONE:
274		break;
275	case IEEE802154_ADDR_LONG:
276		if (!(fc & IEEE802154_FC_INTRA_PAN)) {
277			if (hdr + 2 > tail)
278				goto malformed;
279			addr->pan_id = hdr[0] | (hdr[1] << 8);
280			hdr += 2;
281		}
282
283		if (hdr + IEEE802154_ADDR_LEN > tail)
284			goto malformed;
285
286		mac802154_haddr_copy_swap(addr->hwaddr, hdr);
287		hdr += IEEE802154_ADDR_LEN;
288		break;
289	case IEEE802154_ADDR_SHORT:
290		if (!(fc & IEEE802154_FC_INTRA_PAN)) {
291			if (hdr + 2 > tail)
292				goto malformed;
293			addr->pan_id = hdr[0] | (hdr[1] << 8);
294			hdr += 2;
295		}
296
297		if (hdr + 2 > tail)
298			goto malformed;
299
300		addr->short_addr = hdr[0] | (hdr[1] << 8);
301		hdr += 2;
302		break;
303	default:
304		goto malformed;
305	}
306
307	return sizeof(struct ieee802154_addr);
308
309malformed:
310	pr_debug("malformed packet\n");
311	return 0;
312}
313
314static netdev_tx_t
315mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev)
316{
317	struct mac802154_sub_if_data *priv;
318	u8 chan, page;
319
320	priv = netdev_priv(dev);
321
322	spin_lock_bh(&priv->mib_lock);
323	chan = priv->chan;
324	page = priv->page;
325	spin_unlock_bh(&priv->mib_lock);
326
327	if (chan == MAC802154_CHAN_NONE ||
328	    page >= WPAN_NUM_PAGES ||
329	    chan >= WPAN_NUM_CHANNELS) {
330		kfree_skb(skb);
331		return NETDEV_TX_OK;
332	}
333
334	skb->skb_iif = dev->ifindex;
335	dev->stats.tx_packets++;
336	dev->stats.tx_bytes += skb->len;
337
338	return mac802154_tx(priv->hw, skb, page, chan);
339}
340
341static struct header_ops mac802154_header_ops = {
342	.create		= mac802154_header_create,
343	.parse		= mac802154_header_parse,
344};
345
346static const struct net_device_ops mac802154_wpan_ops = {
347	.ndo_open		= mac802154_slave_open,
348	.ndo_stop		= mac802154_slave_close,
349	.ndo_start_xmit		= mac802154_wpan_xmit,
350	.ndo_do_ioctl		= mac802154_wpan_ioctl,
351	.ndo_set_mac_address	= mac802154_wpan_mac_addr,
352};
353
354void mac802154_wpan_setup(struct net_device *dev)
355{
356	struct mac802154_sub_if_data *priv;
357
358	dev->addr_len		= IEEE802154_ADDR_LEN;
359	memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
360
361	dev->hard_header_len	= MAC802154_FRAME_HARD_HEADER_LEN;
362	dev->header_ops		= &mac802154_header_ops;
363	dev->needed_tailroom	= 2; /* FCS */
364	dev->mtu		= IEEE802154_MTU;
365	dev->tx_queue_len	= 300;
366	dev->type		= ARPHRD_IEEE802154;
367	dev->flags		= IFF_NOARP | IFF_BROADCAST;
368	dev->watchdog_timeo	= 0;
369
370	dev->destructor		= free_netdev;
371	dev->netdev_ops		= &mac802154_wpan_ops;
372	dev->ml_priv		= &mac802154_mlme_wpan;
373
374	priv = netdev_priv(dev);
375	priv->type = IEEE802154_DEV_WPAN;
376
377	priv->chan = MAC802154_CHAN_NONE;
378	priv->page = 0;
379
380	spin_lock_init(&priv->mib_lock);
381
382	get_random_bytes(&priv->bsn, 1);
383	get_random_bytes(&priv->dsn, 1);
384
385	priv->pan_id = IEEE802154_PANID_BROADCAST;
386	priv->short_addr = IEEE802154_ADDR_BROADCAST;
387}
388
389static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb)
390{
391	return netif_rx_ni(skb);
392}
393
394static int
395mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
396{
397	pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
398
399	spin_lock_bh(&sdata->mib_lock);
400
401	switch (mac_cb(skb)->da.addr_type) {
402	case IEEE802154_ADDR_NONE:
403		if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE)
404			/* FIXME: check if we are PAN coordinator */
405			skb->pkt_type = PACKET_OTHERHOST;
406		else
407			/* ACK comes with both addresses empty */
408			skb->pkt_type = PACKET_HOST;
409		break;
410	case IEEE802154_ADDR_LONG:
411		if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
412		    mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
413			skb->pkt_type = PACKET_OTHERHOST;
414		else if (!memcmp(mac_cb(skb)->da.hwaddr, sdata->dev->dev_addr,
415				 IEEE802154_ADDR_LEN))
416			skb->pkt_type = PACKET_HOST;
417		else
418			skb->pkt_type = PACKET_OTHERHOST;
419		break;
420	case IEEE802154_ADDR_SHORT:
421		if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
422		    mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
423			skb->pkt_type = PACKET_OTHERHOST;
424		else if (mac_cb(skb)->da.short_addr == sdata->short_addr)
425			skb->pkt_type = PACKET_HOST;
426		else if (mac_cb(skb)->da.short_addr ==
427					IEEE802154_ADDR_BROADCAST)
428			skb->pkt_type = PACKET_BROADCAST;
429		else
430			skb->pkt_type = PACKET_OTHERHOST;
431		break;
432	default:
433		break;
434	}
435
436	spin_unlock_bh(&sdata->mib_lock);
437
438	skb->dev = sdata->dev;
439
440	sdata->dev->stats.rx_packets++;
441	sdata->dev->stats.rx_bytes += skb->len;
442
443	switch (mac_cb_type(skb)) {
444	case IEEE802154_FC_TYPE_DATA:
445		return mac802154_process_data(sdata->dev, skb);
446	default:
447		pr_warn("ieee802154: bad frame received (type = %d)\n",
448			mac_cb_type(skb));
449		kfree_skb(skb);
450		return NET_RX_DROP;
451	}
452}
453
454static int mac802154_parse_frame_start(struct sk_buff *skb)
455{
456	u8 *head = skb->data;
457	u16 fc;
458
459	if (mac802154_fetch_skb_u16(skb, &fc) ||
460	    mac802154_fetch_skb_u8(skb, &(mac_cb(skb)->seq)))
461		goto err;
462
463	pr_debug("fc: %04x dsn: %02x\n", fc, head[2]);
464
465	mac_cb(skb)->flags = IEEE802154_FC_TYPE(fc);
466	mac_cb(skb)->sa.addr_type = IEEE802154_FC_SAMODE(fc);
467	mac_cb(skb)->da.addr_type = IEEE802154_FC_DAMODE(fc);
468
469	if (fc & IEEE802154_FC_INTRA_PAN)
470		mac_cb(skb)->flags |= MAC_CB_FLAG_INTRAPAN;
471
472	if (mac_cb(skb)->da.addr_type != IEEE802154_ADDR_NONE) {
473		if (mac802154_fetch_skb_u16(skb, &(mac_cb(skb)->da.pan_id)))
474			goto err;
475
476		/* source PAN id compression */
477		if (mac_cb_is_intrapan(skb))
478			mac_cb(skb)->sa.pan_id = mac_cb(skb)->da.pan_id;
479
480		pr_debug("dest PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
481
482		if (mac_cb(skb)->da.addr_type == IEEE802154_ADDR_SHORT) {
483			u16 *da = &(mac_cb(skb)->da.short_addr);
484
485			if (mac802154_fetch_skb_u16(skb, da))
486				goto err;
487
488			pr_debug("destination address is short: %04x\n",
489				 mac_cb(skb)->da.short_addr);
490		} else {
491			if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
492				goto err;
493
494			mac802154_haddr_copy_swap(mac_cb(skb)->da.hwaddr,
495						  skb->data);
496			skb_pull(skb, IEEE802154_ADDR_LEN);
497
498			pr_debug("destination address is hardware\n");
499		}
500	}
501
502	if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE) {
503		/* non PAN-compression, fetch source address id */
504		if (!(mac_cb_is_intrapan(skb))) {
505			u16 *sa_pan = &(mac_cb(skb)->sa.pan_id);
506
507			if (mac802154_fetch_skb_u16(skb, sa_pan))
508				goto err;
509		}
510
511		pr_debug("source PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
512
513		if (mac_cb(skb)->sa.addr_type == IEEE802154_ADDR_SHORT) {
514			u16 *sa = &(mac_cb(skb)->sa.short_addr);
515
516			if (mac802154_fetch_skb_u16(skb, sa))
517				goto err;
518
519			pr_debug("source address is short: %04x\n",
520				 mac_cb(skb)->sa.short_addr);
521		} else {
522			if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
523				goto err;
524
525			mac802154_haddr_copy_swap(mac_cb(skb)->sa.hwaddr,
526						  skb->data);
527			skb_pull(skb, IEEE802154_ADDR_LEN);
528
529			pr_debug("source address is hardware\n");
530		}
531	}
532
533	return 0;
534err:
535	return -EINVAL;
536}
537
538void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb)
539{
540	int ret;
541	struct sk_buff *sskb;
542	struct mac802154_sub_if_data *sdata;
543
544	ret = mac802154_parse_frame_start(skb);
545	if (ret) {
546		pr_debug("got invalid frame\n");
547		return;
548	}
549
550	rcu_read_lock();
551	list_for_each_entry_rcu(sdata, &priv->slaves, list) {
552		if (sdata->type != IEEE802154_DEV_WPAN)
553			continue;
554
555		sskb = skb_clone(skb, GFP_ATOMIC);
556		if (sskb)
557			mac802154_subif_frame(sdata, sskb);
558	}
559	rcu_read_unlock();
560}
561