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