1 /* Copyright 2011, Siemens AG
2 * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
5 /* Based on patches from Jon Smirl <jonsmirl@gmail.com>
6 * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
8 * This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License version 2
10 * as published by the Free Software Foundation.
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
18 /* Jon's code is based on 6lowpan implementation for Contiki which is:
19 * Copyright (c) 2008, Swedish Institute of Computer Science.
20 * All rights reserved.
22 * Redistribution and use in source and binary forms, with or without
23 * modification, are permitted provided that the following conditions
25 * 1. Redistributions of source code must retain the above copyright
26 * notice, this list of conditions and the following disclaimer.
27 * 2. Redistributions in binary form must reproduce the above copyright
28 * notice, this list of conditions and the following disclaimer in the
29 * documentation and/or other materials provided with the distribution.
30 * 3. Neither the name of the Institute nor the names of its contributors
31 * may be used to endorse or promote products derived from this software
32 * without specific prior written permission.
34 * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
35 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
36 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
37 * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
38 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
39 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
40 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
41 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
42 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
43 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
47 #include <linux/bitops.h>
48 #include <linux/if_arp.h>
49 #include <linux/module.h>
50 #include <linux/moduleparam.h>
51 #include <linux/netdevice.h>
52 #include <linux/ieee802154.h>
53 #include <net/af_ieee802154.h>
54 #include <net/ieee802154_netdev.h>
55 #include <net/6lowpan.h>
58 #include "reassembly.h"
60 static LIST_HEAD(lowpan_devices
);
61 static int lowpan_open_count
;
63 /* private device info */
64 struct lowpan_dev_info
{
65 struct net_device
*real_dev
; /* real WPAN device ptr */
66 struct mutex dev_list_mtx
; /* mutex for list ops */
70 struct lowpan_dev_record
{
71 struct net_device
*ldev
;
72 struct list_head list
;
75 /* don't save pan id, it's intra pan */
79 /* IPv6 needs big endian here */
85 struct lowpan_addr_info
{
86 struct lowpan_addr daddr
;
87 struct lowpan_addr saddr
;
91 lowpan_dev_info
*lowpan_dev_info(const struct net_device
*dev
)
93 return netdev_priv(dev
);
97 lowpan_addr_info
*lowpan_skb_priv(const struct sk_buff
*skb
)
99 WARN_ON_ONCE(skb_headroom(skb
) < sizeof(struct lowpan_addr_info
));
100 return (struct lowpan_addr_info
*)(skb
->data
-
101 sizeof(struct lowpan_addr_info
));
104 static int lowpan_header_create(struct sk_buff
*skb
, struct net_device
*dev
,
105 unsigned short type
, const void *_daddr
,
106 const void *_saddr
, unsigned int len
)
108 const u8
*saddr
= _saddr
;
109 const u8
*daddr
= _daddr
;
110 struct lowpan_addr_info
*info
;
113 * if this package isn't ipv6 one, where should it be routed?
115 if (type
!= ETH_P_IPV6
)
119 saddr
= dev
->dev_addr
;
121 raw_dump_inline(__func__
, "saddr", (unsigned char *)saddr
, 8);
122 raw_dump_inline(__func__
, "daddr", (unsigned char *)daddr
, 8);
124 info
= lowpan_skb_priv(skb
);
126 /* TODO: Currently we only support extended_addr */
127 info
->daddr
.mode
= IEEE802154_ADDR_LONG
;
128 memcpy(&info
->daddr
.u
.extended_addr
, daddr
,
129 sizeof(info
->daddr
.u
.extended_addr
));
130 info
->saddr
.mode
= IEEE802154_ADDR_LONG
;
131 memcpy(&info
->saddr
.u
.extended_addr
, saddr
,
132 sizeof(info
->daddr
.u
.extended_addr
));
137 static int lowpan_give_skb_to_devices(struct sk_buff
*skb
,
138 struct net_device
*dev
)
140 struct lowpan_dev_record
*entry
;
141 struct sk_buff
*skb_cp
;
142 int stat
= NET_RX_SUCCESS
;
144 skb
->protocol
= htons(ETH_P_IPV6
);
145 skb
->pkt_type
= PACKET_HOST
;
148 list_for_each_entry_rcu(entry
, &lowpan_devices
, list
)
149 if (lowpan_dev_info(entry
->ldev
)->real_dev
== skb
->dev
) {
150 skb_cp
= skb_copy(skb
, GFP_ATOMIC
);
157 skb_cp
->dev
= entry
->ldev
;
158 stat
= netif_rx(skb_cp
);
159 if (stat
== NET_RX_DROP
)
170 iphc_decompress(struct sk_buff
*skb
, const struct ieee802154_hdr
*hdr
)
173 struct ieee802154_addr_sa sa
, da
;
176 raw_dump_table(__func__
, "raw skb data dump", skb
->data
, skb
->len
);
177 /* at least two bytes will be used for the encoding */
181 if (lowpan_fetch_skb_u8(skb
, &iphc0
))
184 if (lowpan_fetch_skb_u8(skb
, &iphc1
))
187 ieee802154_addr_to_sa(&sa
, &hdr
->source
);
188 ieee802154_addr_to_sa(&da
, &hdr
->dest
);
190 if (sa
.addr_type
== IEEE802154_ADDR_SHORT
)
191 sap
= &sa
.short_addr
;
195 if (da
.addr_type
== IEEE802154_ADDR_SHORT
)
196 dap
= &da
.short_addr
;
200 return lowpan_header_decompress(skb
, skb
->dev
, sap
, sa
.addr_type
,
201 IEEE802154_ADDR_LEN
, dap
, da
.addr_type
,
202 IEEE802154_ADDR_LEN
, iphc0
, iphc1
);
205 static struct sk_buff
*
206 lowpan_alloc_frag(struct sk_buff
*skb
, int size
,
207 const struct ieee802154_hdr
*master_hdr
)
209 struct net_device
*real_dev
= lowpan_dev_info(skb
->dev
)->real_dev
;
210 struct sk_buff
*frag
;
213 frag
= alloc_skb(real_dev
->hard_header_len
+
214 real_dev
->needed_tailroom
+ size
,
218 frag
->dev
= real_dev
;
219 frag
->priority
= skb
->priority
;
220 skb_reserve(frag
, real_dev
->hard_header_len
);
221 skb_reset_network_header(frag
);
222 *mac_cb(frag
) = *mac_cb(skb
);
224 rc
= dev_hard_header(frag
, real_dev
, 0, &master_hdr
->dest
,
225 &master_hdr
->source
, size
);
231 frag
= ERR_PTR(-ENOMEM
);
238 lowpan_xmit_fragment(struct sk_buff
*skb
, const struct ieee802154_hdr
*wpan_hdr
,
239 u8
*frag_hdr
, int frag_hdrlen
,
242 struct sk_buff
*frag
;
244 raw_dump_inline(__func__
, " fragment header", frag_hdr
, frag_hdrlen
);
246 frag
= lowpan_alloc_frag(skb
, frag_hdrlen
+ len
, wpan_hdr
);
248 return -PTR_ERR(frag
);
250 memcpy(skb_put(frag
, frag_hdrlen
), frag_hdr
, frag_hdrlen
);
251 memcpy(skb_put(frag
, len
), skb_network_header(skb
) + offset
, len
);
253 raw_dump_table(__func__
, " fragment dump", frag
->data
, frag
->len
);
255 return dev_queue_xmit(frag
);
259 lowpan_xmit_fragmented(struct sk_buff
*skb
, struct net_device
*dev
,
260 const struct ieee802154_hdr
*wpan_hdr
)
262 u16 dgram_size
, dgram_offset
;
265 int frag_cap
, frag_len
, payload_cap
, rc
;
266 int skb_unprocessed
, skb_offset
;
268 dgram_size
= lowpan_uncompress_size(skb
, &dgram_offset
) -
270 frag_tag
= htons(lowpan_dev_info(dev
)->fragment_tag
);
271 lowpan_dev_info(dev
)->fragment_tag
++;
273 frag_hdr
[0] = LOWPAN_DISPATCH_FRAG1
| ((dgram_size
>> 8) & 0x07);
274 frag_hdr
[1] = dgram_size
& 0xff;
275 memcpy(frag_hdr
+ 2, &frag_tag
, sizeof(frag_tag
));
277 payload_cap
= ieee802154_max_payload(wpan_hdr
);
279 frag_len
= round_down(payload_cap
- LOWPAN_FRAG1_HEAD_SIZE
-
280 skb_network_header_len(skb
), 8);
282 skb_offset
= skb_network_header_len(skb
);
283 skb_unprocessed
= skb
->len
- skb
->mac_len
- skb_offset
;
285 rc
= lowpan_xmit_fragment(skb
, wpan_hdr
, frag_hdr
,
286 LOWPAN_FRAG1_HEAD_SIZE
, 0,
287 frag_len
+ skb_network_header_len(skb
));
289 pr_debug("%s unable to send FRAG1 packet (tag: %d)",
290 __func__
, ntohs(frag_tag
));
294 frag_hdr
[0] &= ~LOWPAN_DISPATCH_FRAG1
;
295 frag_hdr
[0] |= LOWPAN_DISPATCH_FRAGN
;
296 frag_cap
= round_down(payload_cap
- LOWPAN_FRAGN_HEAD_SIZE
, 8);
299 dgram_offset
+= frag_len
;
300 skb_offset
+= frag_len
;
301 skb_unprocessed
-= frag_len
;
302 frag_len
= min(frag_cap
, skb_unprocessed
);
304 frag_hdr
[4] = dgram_offset
>> 3;
306 rc
= lowpan_xmit_fragment(skb
, wpan_hdr
, frag_hdr
,
307 LOWPAN_FRAGN_HEAD_SIZE
, skb_offset
,
310 pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
311 __func__
, ntohs(frag_tag
), skb_offset
);
314 } while (skb_unprocessed
> frag_cap
);
317 return NET_XMIT_SUCCESS
;
324 static int lowpan_header(struct sk_buff
*skb
, struct net_device
*dev
)
326 struct ieee802154_addr sa
, da
;
327 struct ieee802154_mac_cb
*cb
= mac_cb_init(skb
);
328 struct lowpan_addr_info info
;
331 memcpy(&info
, lowpan_skb_priv(skb
), sizeof(info
));
333 /* TODO: Currently we only support extended_addr */
334 daddr
= &info
.daddr
.u
.extended_addr
;
335 saddr
= &info
.saddr
.u
.extended_addr
;
337 lowpan_header_compress(skb
, dev
, ETH_P_IPV6
, daddr
, saddr
, skb
->len
);
339 cb
->type
= IEEE802154_FC_TYPE_DATA
;
341 /* prepare wpan address data */
342 sa
.mode
= IEEE802154_ADDR_LONG
;
343 sa
.pan_id
= ieee802154_mlme_ops(dev
)->get_pan_id(dev
);
344 sa
.extended_addr
= ieee802154_devaddr_from_raw(saddr
);
346 /* intra-PAN communications */
347 da
.pan_id
= sa
.pan_id
;
349 /* if the destination address is the broadcast address, use the
350 * corresponding short address
352 if (lowpan_is_addr_broadcast((const u8
*)daddr
)) {
353 da
.mode
= IEEE802154_ADDR_SHORT
;
354 da
.short_addr
= cpu_to_le16(IEEE802154_ADDR_BROADCAST
);
357 da
.mode
= IEEE802154_ADDR_LONG
;
358 da
.extended_addr
= ieee802154_devaddr_from_raw(daddr
);
362 return dev_hard_header(skb
, lowpan_dev_info(dev
)->real_dev
,
363 ETH_P_IPV6
, (void *)&da
, (void *)&sa
, 0);
366 static netdev_tx_t
lowpan_xmit(struct sk_buff
*skb
, struct net_device
*dev
)
368 struct ieee802154_hdr wpan_hdr
;
371 pr_debug("package xmit\n");
373 /* We must take a copy of the skb before we modify/replace the ipv6
374 * header as the header could be used elsewhere
376 skb
= skb_unshare(skb
, GFP_ATOMIC
);
378 return NET_XMIT_DROP
;
380 ret
= lowpan_header(skb
, dev
);
383 return NET_XMIT_DROP
;
386 if (ieee802154_hdr_peek(skb
, &wpan_hdr
) < 0) {
388 return NET_XMIT_DROP
;
391 max_single
= ieee802154_max_payload(&wpan_hdr
);
393 if (skb_tail_pointer(skb
) - skb_network_header(skb
) <= max_single
) {
394 skb
->dev
= lowpan_dev_info(dev
)->real_dev
;
395 return dev_queue_xmit(skb
);
399 pr_debug("frame is too big, fragmentation is needed\n");
400 rc
= lowpan_xmit_fragmented(skb
, dev
, &wpan_hdr
);
402 return rc
< 0 ? NET_XMIT_DROP
: rc
;
406 static __le16
lowpan_get_pan_id(const struct net_device
*dev
)
408 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
410 return ieee802154_mlme_ops(real_dev
)->get_pan_id(real_dev
);
413 static __le16
lowpan_get_short_addr(const struct net_device
*dev
)
415 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
417 return ieee802154_mlme_ops(real_dev
)->get_short_addr(real_dev
);
420 static u8
lowpan_get_dsn(const struct net_device
*dev
)
422 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
424 return ieee802154_mlme_ops(real_dev
)->get_dsn(real_dev
);
427 static struct header_ops lowpan_header_ops
= {
428 .create
= lowpan_header_create
,
431 static struct lock_class_key lowpan_tx_busylock
;
432 static struct lock_class_key lowpan_netdev_xmit_lock_key
;
434 static void lowpan_set_lockdep_class_one(struct net_device
*dev
,
435 struct netdev_queue
*txq
,
438 lockdep_set_class(&txq
->_xmit_lock
,
439 &lowpan_netdev_xmit_lock_key
);
442 static int lowpan_dev_init(struct net_device
*dev
)
444 netdev_for_each_tx_queue(dev
, lowpan_set_lockdep_class_one
, NULL
);
445 dev
->qdisc_tx_busylock
= &lowpan_tx_busylock
;
449 static const struct net_device_ops lowpan_netdev_ops
= {
450 .ndo_init
= lowpan_dev_init
,
451 .ndo_start_xmit
= lowpan_xmit
,
454 static struct ieee802154_mlme_ops lowpan_mlme
= {
455 .get_pan_id
= lowpan_get_pan_id
,
456 .get_short_addr
= lowpan_get_short_addr
,
457 .get_dsn
= lowpan_get_dsn
,
460 static void lowpan_setup(struct net_device
*dev
)
462 dev
->addr_len
= IEEE802154_ADDR_LEN
;
463 memset(dev
->broadcast
, 0xff, IEEE802154_ADDR_LEN
);
464 dev
->type
= ARPHRD_IEEE802154
;
465 /* Frame Control + Sequence Number + Address fields + Security Header */
466 dev
->hard_header_len
= 2 + 1 + 20 + 14;
467 dev
->needed_tailroom
= 2; /* FCS */
468 dev
->mtu
= IPV6_MIN_MTU
;
469 dev
->tx_queue_len
= 0;
470 dev
->flags
= IFF_BROADCAST
| IFF_MULTICAST
;
471 dev
->watchdog_timeo
= 0;
473 dev
->netdev_ops
= &lowpan_netdev_ops
;
474 dev
->header_ops
= &lowpan_header_ops
;
475 dev
->ml_priv
= &lowpan_mlme
;
476 dev
->destructor
= free_netdev
;
479 static int lowpan_validate(struct nlattr
*tb
[], struct nlattr
*data
[])
481 if (tb
[IFLA_ADDRESS
]) {
482 if (nla_len(tb
[IFLA_ADDRESS
]) != IEEE802154_ADDR_LEN
)
488 static int lowpan_rcv(struct sk_buff
*skb
, struct net_device
*dev
,
489 struct packet_type
*pt
, struct net_device
*orig_dev
)
491 struct ieee802154_hdr hdr
;
494 skb
= skb_share_check(skb
, GFP_ATOMIC
);
498 if (!netif_running(dev
))
501 if (skb
->pkt_type
== PACKET_OTHERHOST
)
504 if (dev
->type
!= ARPHRD_IEEE802154
)
507 if (ieee802154_hdr_peek_addrs(skb
, &hdr
) < 0)
510 /* check that it's our buffer */
511 if (skb
->data
[0] == LOWPAN_DISPATCH_IPV6
) {
512 /* Pull off the 1-byte of 6lowpan header. */
514 return lowpan_give_skb_to_devices(skb
, NULL
);
516 switch (skb
->data
[0] & 0xe0) {
517 case LOWPAN_DISPATCH_IPHC
: /* ipv6 datagram */
518 ret
= iphc_decompress(skb
, &hdr
);
522 return lowpan_give_skb_to_devices(skb
, NULL
);
523 case LOWPAN_DISPATCH_FRAG1
: /* first fragment header */
524 ret
= lowpan_frag_rcv(skb
, LOWPAN_DISPATCH_FRAG1
);
526 ret
= iphc_decompress(skb
, &hdr
);
530 return lowpan_give_skb_to_devices(skb
, NULL
);
531 } else if (ret
== -1) {
534 return NET_RX_SUCCESS
;
536 case LOWPAN_DISPATCH_FRAGN
: /* next fragments headers */
537 ret
= lowpan_frag_rcv(skb
, LOWPAN_DISPATCH_FRAGN
);
539 ret
= iphc_decompress(skb
, &hdr
);
543 return lowpan_give_skb_to_devices(skb
, NULL
);
544 } else if (ret
== -1) {
547 return NET_RX_SUCCESS
;
560 static struct packet_type lowpan_packet_type
= {
561 .type
= htons(ETH_P_IEEE802154
),
565 static int lowpan_newlink(struct net
*src_net
, struct net_device
*dev
,
566 struct nlattr
*tb
[], struct nlattr
*data
[])
568 struct net_device
*real_dev
;
569 struct lowpan_dev_record
*entry
;
574 pr_debug("adding new link\n");
578 /* find and hold real wpan device */
579 real_dev
= dev_get_by_index(src_net
, nla_get_u32(tb
[IFLA_LINK
]));
582 if (real_dev
->type
!= ARPHRD_IEEE802154
) {
587 lowpan_dev_info(dev
)->real_dev
= real_dev
;
588 mutex_init(&lowpan_dev_info(dev
)->dev_list_mtx
);
590 entry
= kzalloc(sizeof(*entry
), GFP_KERNEL
);
593 lowpan_dev_info(dev
)->real_dev
= NULL
;
599 /* Set the lowpan hardware address to the wpan hardware address. */
600 memcpy(dev
->dev_addr
, real_dev
->dev_addr
, IEEE802154_ADDR_LEN
);
602 mutex_lock(&lowpan_dev_info(dev
)->dev_list_mtx
);
603 INIT_LIST_HEAD(&entry
->list
);
604 list_add_tail(&entry
->list
, &lowpan_devices
);
605 mutex_unlock(&lowpan_dev_info(dev
)->dev_list_mtx
);
607 ret
= register_netdevice(dev
);
609 if (!lowpan_open_count
)
610 dev_add_pack(&lowpan_packet_type
);
617 static void lowpan_dellink(struct net_device
*dev
, struct list_head
*head
)
619 struct lowpan_dev_info
*lowpan_dev
= lowpan_dev_info(dev
);
620 struct net_device
*real_dev
= lowpan_dev
->real_dev
;
621 struct lowpan_dev_record
*entry
, *tmp
;
626 if (!lowpan_open_count
)
627 dev_remove_pack(&lowpan_packet_type
);
629 mutex_lock(&lowpan_dev_info(dev
)->dev_list_mtx
);
630 list_for_each_entry_safe(entry
, tmp
, &lowpan_devices
, list
) {
631 if (entry
->ldev
== dev
) {
632 list_del(&entry
->list
);
636 mutex_unlock(&lowpan_dev_info(dev
)->dev_list_mtx
);
638 mutex_destroy(&lowpan_dev_info(dev
)->dev_list_mtx
);
640 unregister_netdevice_queue(dev
, head
);
645 static struct rtnl_link_ops lowpan_link_ops __read_mostly
= {
647 .priv_size
= sizeof(struct lowpan_dev_info
),
648 .setup
= lowpan_setup
,
649 .newlink
= lowpan_newlink
,
650 .dellink
= lowpan_dellink
,
651 .validate
= lowpan_validate
,
654 static inline int __init
lowpan_netlink_init(void)
656 return rtnl_link_register(&lowpan_link_ops
);
659 static inline void lowpan_netlink_fini(void)
661 rtnl_link_unregister(&lowpan_link_ops
);
664 static int lowpan_device_event(struct notifier_block
*unused
,
665 unsigned long event
, void *ptr
)
667 struct net_device
*dev
= netdev_notifier_info_to_dev(ptr
);
669 struct lowpan_dev_record
*entry
, *tmp
;
671 if (dev
->type
!= ARPHRD_IEEE802154
)
674 if (event
== NETDEV_UNREGISTER
) {
675 list_for_each_entry_safe(entry
, tmp
, &lowpan_devices
, list
) {
676 if (lowpan_dev_info(entry
->ldev
)->real_dev
== dev
)
677 lowpan_dellink(entry
->ldev
, &del_list
);
680 unregister_netdevice_many(&del_list
);
687 static struct notifier_block lowpan_dev_notifier
= {
688 .notifier_call
= lowpan_device_event
,
691 static int __init
lowpan_init_module(void)
695 err
= lowpan_net_frag_init();
699 err
= lowpan_netlink_init();
703 err
= register_netdevice_notifier(&lowpan_dev_notifier
);
710 lowpan_netlink_fini();
712 lowpan_net_frag_exit();
717 static void __exit
lowpan_cleanup_module(void)
719 lowpan_netlink_fini();
721 lowpan_net_frag_exit();
723 unregister_netdevice_notifier(&lowpan_dev_notifier
);
726 module_init(lowpan_init_module
);
727 module_exit(lowpan_cleanup_module
);
728 MODULE_LICENSE("GPL");
729 MODULE_ALIAS_RTNL_LINK("lowpan");