tjh.dev
Linux kernel mirror (for testing)
git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
kernel
os
linux
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
149 if (!saddr) {
150 spin_lock_bh(&priv->mib_lock);
151
152 if (priv->short_addr == IEEE802154_ADDR_BROADCAST ||
153 priv->short_addr == IEEE802154_ADDR_UNDEF ||
154 priv->pan_id == IEEE802154_PANID_BROADCAST) {
155 dev_addr.addr_type = IEEE802154_ADDR_LONG;
156 memcpy(dev_addr.hwaddr, dev->dev_addr,
157 IEEE802154_ADDR_LEN);
158 } else {
159 dev_addr.addr_type = IEEE802154_ADDR_SHORT;
160 dev_addr.short_addr = priv->short_addr;
161 }
162
163 dev_addr.pan_id = priv->pan_id;
164 saddr = &dev_addr;
165
166 spin_unlock_bh(&priv->mib_lock);
167 }
168
169 if (daddr->addr_type != IEEE802154_ADDR_NONE) {
170 fc |= (daddr->addr_type << IEEE802154_FC_DAMODE_SHIFT);
171
172 head[pos++] = daddr->pan_id & 0xff;
173 head[pos++] = daddr->pan_id >> 8;
174
175 if (daddr->addr_type == IEEE802154_ADDR_SHORT) {
176 head[pos++] = daddr->short_addr & 0xff;
177 head[pos++] = daddr->short_addr >> 8;
178 } else {
179 mac802154_haddr_copy_swap(head + pos, daddr->hwaddr);
180 pos += IEEE802154_ADDR_LEN;
181 }
182 }
183
184 if (saddr->addr_type != IEEE802154_ADDR_NONE) {
185 fc |= (saddr->addr_type << IEEE802154_FC_SAMODE_SHIFT);
186
187 if ((saddr->pan_id == daddr->pan_id) &&
188 (saddr->pan_id != IEEE802154_PANID_BROADCAST)) {
189 /* PANID compression/intra PAN */
190 fc |= IEEE802154_FC_INTRA_PAN;
191 } else {
192 head[pos++] = saddr->pan_id & 0xff;
193 head[pos++] = saddr->pan_id >> 8;
194 }
195
196 if (saddr->addr_type == IEEE802154_ADDR_SHORT) {
197 head[pos++] = saddr->short_addr & 0xff;
198 head[pos++] = saddr->short_addr >> 8;
199 } else {
200 mac802154_haddr_copy_swap(head + pos, saddr->hwaddr);
201 pos += IEEE802154_ADDR_LEN;
202 }
203 }
204
205 head[0] = fc;
206 head[1] = fc >> 8;
207
208 memcpy(skb_push(skb, pos), head, pos);
209
210 return pos;
211}
212
213static int
214mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
215{
216 const u8 *hdr = skb_mac_header(skb);
217 const u8 *tail = skb_tail_pointer(skb);
218 struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr;
219 u16 fc;
220 int da_type;
221
222 if (hdr + 3 > tail)
223 goto malformed;
224
225 fc = hdr[0] | (hdr[1] << 8);
226
227 hdr += 3;
228
229 da_type = IEEE802154_FC_DAMODE(fc);
230 addr->addr_type = IEEE802154_FC_SAMODE(fc);
231
232 switch (da_type) {
233 case IEEE802154_ADDR_NONE:
234 if (fc & IEEE802154_FC_INTRA_PAN)
235 goto malformed;
236 break;
237 case IEEE802154_ADDR_LONG:
238 if (fc & IEEE802154_FC_INTRA_PAN) {
239 if (hdr + 2 > tail)
240 goto malformed;
241 addr->pan_id = hdr[0] | (hdr[1] << 8);
242 hdr += 2;
243 }
244
245 if (hdr + IEEE802154_ADDR_LEN > tail)
246 goto malformed;
247
248 hdr += IEEE802154_ADDR_LEN;
249 break;
250 case IEEE802154_ADDR_SHORT:
251 if (fc & IEEE802154_FC_INTRA_PAN) {
252 if (hdr + 2 > tail)
253 goto malformed;
254 addr->pan_id = hdr[0] | (hdr[1] << 8);
255 hdr += 2;
256 }
257
258 if (hdr + 2 > tail)
259 goto malformed;
260
261 hdr += 2;
262 break;
263 default:
264 goto malformed;
265
266 }
267
268 switch (addr->addr_type) {
269 case IEEE802154_ADDR_NONE:
270 break;
271 case IEEE802154_ADDR_LONG:
272 if (!(fc & IEEE802154_FC_INTRA_PAN)) {
273 if (hdr + 2 > tail)
274 goto malformed;
275 addr->pan_id = hdr[0] | (hdr[1] << 8);
276 hdr += 2;
277 }
278
279 if (hdr + IEEE802154_ADDR_LEN > tail)
280 goto malformed;
281
282 mac802154_haddr_copy_swap(addr->hwaddr, hdr);
283 hdr += IEEE802154_ADDR_LEN;
284 break;
285 case IEEE802154_ADDR_SHORT:
286 if (!(fc & IEEE802154_FC_INTRA_PAN)) {
287 if (hdr + 2 > tail)
288 goto malformed;
289 addr->pan_id = hdr[0] | (hdr[1] << 8);
290 hdr += 2;
291 }
292
293 if (hdr + 2 > tail)
294 goto malformed;
295
296 addr->short_addr = hdr[0] | (hdr[1] << 8);
297 hdr += 2;
298 break;
299 default:
300 goto malformed;
301 }
302
303 return sizeof(struct ieee802154_addr);
304
305malformed:
306 pr_debug("malformed packet\n");
307 return 0;
308}
309
310static netdev_tx_t
311mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev)
312{
313 struct mac802154_sub_if_data *priv;
314 u8 chan, page;
315
316 priv = netdev_priv(dev);
317
318 spin_lock_bh(&priv->mib_lock);
319 chan = priv->chan;
320 page = priv->page;
321 spin_unlock_bh(&priv->mib_lock);
322
323 if (chan == MAC802154_CHAN_NONE ||
324 page >= WPAN_NUM_PAGES ||
325 chan >= WPAN_NUM_CHANNELS) {
326 kfree_skb(skb);
327 return NETDEV_TX_OK;
328 }
329
330 skb->skb_iif = dev->ifindex;
331 dev->stats.tx_packets++;
332 dev->stats.tx_bytes += skb->len;
333
334 return mac802154_tx(priv->hw, skb, page, chan);
335}
336
337static struct header_ops mac802154_header_ops = {
338 .create = mac802154_header_create,
339 .parse = mac802154_header_parse,
340};
341
342static const struct net_device_ops mac802154_wpan_ops = {
343 .ndo_open = mac802154_slave_open,
344 .ndo_stop = mac802154_slave_close,
345 .ndo_start_xmit = mac802154_wpan_xmit,
346 .ndo_do_ioctl = mac802154_wpan_ioctl,
347 .ndo_set_mac_address = mac802154_wpan_mac_addr,
348};
349
350void mac802154_wpan_setup(struct net_device *dev)
351{
352 struct mac802154_sub_if_data *priv;
353
354 dev->addr_len = IEEE802154_ADDR_LEN;
355 memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
356
357 dev->hard_header_len = MAC802154_FRAME_HARD_HEADER_LEN;
358 dev->header_ops = &mac802154_header_ops;
359 dev->needed_tailroom = 2; /* FCS */
360 dev->mtu = IEEE802154_MTU;
361 dev->tx_queue_len = 10;
362 dev->type = ARPHRD_IEEE802154;
363 dev->flags = IFF_NOARP | IFF_BROADCAST;
364 dev->watchdog_timeo = 0;
365
366 dev->destructor = free_netdev;
367 dev->netdev_ops = &mac802154_wpan_ops;
368 dev->ml_priv = &mac802154_mlme_wpan;
369
370 priv = netdev_priv(dev);
371 priv->type = IEEE802154_DEV_WPAN;
372
373 priv->chan = MAC802154_CHAN_NONE;
374 priv->page = 0;
375
376 spin_lock_init(&priv->mib_lock);
377
378 get_random_bytes(&priv->bsn, 1);
379 get_random_bytes(&priv->dsn, 1);
380
381 priv->pan_id = IEEE802154_PANID_BROADCAST;
382 priv->short_addr = IEEE802154_ADDR_BROADCAST;
383}
384
385static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb)
386{
387 return netif_rx_ni(skb);
388}
389
390static int
391mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
392{
393 pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
394
395 spin_lock_bh(&sdata->mib_lock);
396
397 switch (mac_cb(skb)->da.addr_type) {
398 case IEEE802154_ADDR_NONE:
399 if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE)
400 /* FIXME: check if we are PAN coordinator */
401 skb->pkt_type = PACKET_OTHERHOST;
402 else
403 /* ACK comes with both addresses empty */
404 skb->pkt_type = PACKET_HOST;
405 break;
406 case IEEE802154_ADDR_LONG:
407 if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
408 mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
409 skb->pkt_type = PACKET_OTHERHOST;
410 else if (!memcmp(mac_cb(skb)->da.hwaddr, sdata->dev->dev_addr,
411 IEEE802154_ADDR_LEN))
412 skb->pkt_type = PACKET_HOST;
413 else
414 skb->pkt_type = PACKET_OTHERHOST;
415 break;
416 case IEEE802154_ADDR_SHORT:
417 if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
418 mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
419 skb->pkt_type = PACKET_OTHERHOST;
420 else if (mac_cb(skb)->da.short_addr == sdata->short_addr)
421 skb->pkt_type = PACKET_HOST;
422 else if (mac_cb(skb)->da.short_addr ==
423 IEEE802154_ADDR_BROADCAST)
424 skb->pkt_type = PACKET_BROADCAST;
425 else
426 skb->pkt_type = PACKET_OTHERHOST;
427 break;
428 default:
429 break;
430 }
431
432 spin_unlock_bh(&sdata->mib_lock);
433
434 skb->dev = sdata->dev;
435
436 sdata->dev->stats.rx_packets++;
437 sdata->dev->stats.rx_bytes += skb->len;
438
439 switch (mac_cb_type(skb)) {
440 case IEEE802154_FC_TYPE_DATA:
441 return mac802154_process_data(sdata->dev, skb);
442 default:
443 pr_warning("ieee802154: bad frame received (type = %d)\n",
444 mac_cb_type(skb));
445 kfree_skb(skb);
446 return NET_RX_DROP;
447 }
448}
449
450static int mac802154_parse_frame_start(struct sk_buff *skb)
451{
452 u8 *head = skb->data;
453 u16 fc;
454
455 if (mac802154_fetch_skb_u16(skb, &fc) ||
456 mac802154_fetch_skb_u8(skb, &(mac_cb(skb)->seq)))
457 goto err;
458
459 pr_debug("fc: %04x dsn: %02x\n", fc, head[2]);
460
461 mac_cb(skb)->flags = IEEE802154_FC_TYPE(fc);
462 mac_cb(skb)->sa.addr_type = IEEE802154_FC_SAMODE(fc);
463 mac_cb(skb)->da.addr_type = IEEE802154_FC_DAMODE(fc);
464
465 if (fc & IEEE802154_FC_INTRA_PAN)
466 mac_cb(skb)->flags |= MAC_CB_FLAG_INTRAPAN;
467
468 if (mac_cb(skb)->da.addr_type != IEEE802154_ADDR_NONE) {
469 if (mac802154_fetch_skb_u16(skb, &(mac_cb(skb)->da.pan_id)))
470 goto err;
471
472 /* source PAN id compression */
473 if (mac_cb_is_intrapan(skb))
474 mac_cb(skb)->sa.pan_id = mac_cb(skb)->da.pan_id;
475
476 pr_debug("dest PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
477
478 if (mac_cb(skb)->da.addr_type == IEEE802154_ADDR_SHORT) {
479 u16 *da = &(mac_cb(skb)->da.short_addr);
480
481 if (mac802154_fetch_skb_u16(skb, da))
482 goto err;
483
484 pr_debug("destination address is short: %04x\n",
485 mac_cb(skb)->da.short_addr);
486 } else {
487 if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
488 goto err;
489
490 mac802154_haddr_copy_swap(mac_cb(skb)->da.hwaddr,
491 skb->data);
492 skb_pull(skb, IEEE802154_ADDR_LEN);
493
494 pr_debug("destination address is hardware\n");
495 }
496 }
497
498 if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE) {
499 /* non PAN-compression, fetch source address id */
500 if (!(mac_cb_is_intrapan(skb))) {
501 u16 *sa_pan = &(mac_cb(skb)->sa.pan_id);
502
503 if (mac802154_fetch_skb_u16(skb, sa_pan))
504 goto err;
505 }
506
507 pr_debug("source PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
508
509 if (mac_cb(skb)->sa.addr_type == IEEE802154_ADDR_SHORT) {
510 u16 *sa = &(mac_cb(skb)->sa.short_addr);
511
512 if (mac802154_fetch_skb_u16(skb, sa))
513 goto err;
514
515 pr_debug("source address is short: %04x\n",
516 mac_cb(skb)->sa.short_addr);
517 } else {
518 if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
519 goto err;
520
521 mac802154_haddr_copy_swap(mac_cb(skb)->sa.hwaddr,
522 skb->data);
523 skb_pull(skb, IEEE802154_ADDR_LEN);
524
525 pr_debug("source address is hardware\n");
526 }
527 }
528
529 return 0;
530err:
531 return -EINVAL;
532}
533
534void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb)
535{
536 int ret;
537 struct sk_buff *sskb;
538 struct mac802154_sub_if_data *sdata;
539
540 ret = mac802154_parse_frame_start(skb);
541 if (ret) {
542 pr_debug("got invalid frame\n");
543 return;
544 }
545
546 rcu_read_lock();
547 list_for_each_entry_rcu(sdata, &priv->slaves, list) {
548 if (sdata->type != IEEE802154_DEV_WPAN)
549 continue;
550
551 sskb = skb_clone(skb, GFP_ATOMIC);
552 if (sskb)
553 mac802154_subif_frame(sdata, sskb);
554 }
555 rcu_read_unlock();
556}