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 <net/af_ieee802154.h>
53 #include <net/ieee802154.h>
54 #include <net/ieee802154_netdev.h>
55 #include <net/6lowpan.h>
58 #include "reassembly.h"
60 static LIST_HEAD(lowpan_devices
);
62 /* private device info */
63 struct lowpan_dev_info
{
64 struct net_device
*real_dev
; /* real WPAN device ptr */
65 struct mutex dev_list_mtx
; /* mutex for list ops */
69 struct lowpan_dev_record
{
70 struct net_device
*ldev
;
71 struct list_head list
;
75 lowpan_dev_info
*lowpan_dev_info(const struct net_device
*dev
)
77 return netdev_priv(dev
);
80 static inline void lowpan_address_flip(u8
*src
, u8
*dest
)
83 for (i
= 0; i
< IEEE802154_ADDR_LEN
; i
++)
84 (dest
)[IEEE802154_ADDR_LEN
- i
- 1] = (src
)[i
];
87 static int lowpan_header_create(struct sk_buff
*skb
,
88 struct net_device
*dev
,
89 unsigned short type
, const void *_daddr
,
90 const void *_saddr
, unsigned int len
)
92 const u8
*saddr
= _saddr
;
93 const u8
*daddr
= _daddr
;
94 struct ieee802154_addr sa
, da
;
95 struct ieee802154_mac_cb
*cb
= mac_cb_init(skb
);
98 * if this package isn't ipv6 one, where should it be routed?
100 if (type
!= ETH_P_IPV6
)
104 saddr
= dev
->dev_addr
;
106 raw_dump_inline(__func__
, "saddr", (unsigned char *)saddr
, 8);
107 raw_dump_inline(__func__
, "daddr", (unsigned char *)daddr
, 8);
109 lowpan_header_compress(skb
, dev
, type
, daddr
, saddr
, len
);
111 /* NOTE1: I'm still unsure about the fact that compression and WPAN
112 * header are created here and not later in the xmit. So wait for
113 * an opinion of net maintainers.
115 /* NOTE2: to be absolutely correct, we must derive PANid information
116 * from MAC subif of the 'dev' and 'real_dev' network devices, but
117 * this isn't implemented in mainline yet, so currently we assign 0xff
119 cb
->type
= IEEE802154_FC_TYPE_DATA
;
121 /* prepare wpan address data */
122 sa
.mode
= IEEE802154_ADDR_LONG
;
123 sa
.pan_id
= ieee802154_mlme_ops(dev
)->get_pan_id(dev
);
124 sa
.extended_addr
= ieee802154_devaddr_from_raw(saddr
);
126 /* intra-PAN communications */
127 da
.pan_id
= sa
.pan_id
;
129 /* if the destination address is the broadcast address, use the
130 * corresponding short address
132 if (lowpan_is_addr_broadcast(daddr
)) {
133 da
.mode
= IEEE802154_ADDR_SHORT
;
134 da
.short_addr
= cpu_to_le16(IEEE802154_ADDR_BROADCAST
);
136 da
.mode
= IEEE802154_ADDR_LONG
;
137 da
.extended_addr
= ieee802154_devaddr_from_raw(daddr
);
140 cb
->ackreq
= !lowpan_is_addr_broadcast(daddr
);
142 return dev_hard_header(skb
, lowpan_dev_info(dev
)->real_dev
,
143 type
, (void *)&da
, (void *)&sa
, 0);
146 static int lowpan_give_skb_to_devices(struct sk_buff
*skb
,
147 struct net_device
*dev
)
149 struct lowpan_dev_record
*entry
;
150 struct sk_buff
*skb_cp
;
151 int stat
= NET_RX_SUCCESS
;
154 list_for_each_entry_rcu(entry
, &lowpan_devices
, list
)
155 if (lowpan_dev_info(entry
->ldev
)->real_dev
== skb
->dev
) {
156 skb_cp
= skb_copy(skb
, GFP_ATOMIC
);
162 skb_cp
->dev
= entry
->ldev
;
163 stat
= netif_rx(skb_cp
);
170 static int process_data(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_process_data(skb
, skb
->dev
, sap
, sa
.addr_type
,
201 IEEE802154_ADDR_LEN
, dap
, da
.addr_type
,
202 IEEE802154_ADDR_LEN
, iphc0
, iphc1
,
203 lowpan_give_skb_to_devices
);
210 static int lowpan_set_address(struct net_device
*dev
, void *p
)
212 struct sockaddr
*sa
= p
;
214 if (netif_running(dev
))
217 /* TODO: validate addr */
218 memcpy(dev
->dev_addr
, sa
->sa_data
, dev
->addr_len
);
223 static struct sk_buff
*
224 lowpan_alloc_frag(struct sk_buff
*skb
, int size
,
225 const struct ieee802154_hdr
*master_hdr
)
227 struct net_device
*real_dev
= lowpan_dev_info(skb
->dev
)->real_dev
;
228 struct sk_buff
*frag
;
231 frag
= alloc_skb(real_dev
->hard_header_len
+
232 real_dev
->needed_tailroom
+ size
,
236 frag
->dev
= real_dev
;
237 frag
->priority
= skb
->priority
;
238 skb_reserve(frag
, real_dev
->hard_header_len
);
239 skb_reset_network_header(frag
);
240 *mac_cb(frag
) = *mac_cb(skb
);
242 rc
= dev_hard_header(frag
, real_dev
, 0, &master_hdr
->dest
,
243 &master_hdr
->source
, size
);
249 frag
= ERR_PTR(ENOMEM
);
256 lowpan_xmit_fragment(struct sk_buff
*skb
, const struct ieee802154_hdr
*wpan_hdr
,
257 u8
*frag_hdr
, int frag_hdrlen
,
260 struct sk_buff
*frag
;
262 raw_dump_inline(__func__
, " fragment header", frag_hdr
, frag_hdrlen
);
264 frag
= lowpan_alloc_frag(skb
, frag_hdrlen
+ len
, wpan_hdr
);
266 return -PTR_ERR(frag
);
268 memcpy(skb_put(frag
, frag_hdrlen
), frag_hdr
, frag_hdrlen
);
269 memcpy(skb_put(frag
, len
), skb_network_header(skb
) + offset
, len
);
271 raw_dump_table(__func__
, " fragment dump", frag
->data
, frag
->len
);
273 return dev_queue_xmit(frag
);
277 lowpan_xmit_fragmented(struct sk_buff
*skb
, struct net_device
*dev
,
278 const struct ieee802154_hdr
*wpan_hdr
)
280 u16 dgram_size
, dgram_offset
;
283 int frag_cap
, frag_len
, payload_cap
, rc
;
284 int skb_unprocessed
, skb_offset
;
286 dgram_size
= lowpan_uncompress_size(skb
, &dgram_offset
) -
288 frag_tag
= lowpan_dev_info(dev
)->fragment_tag
++;
290 frag_hdr
[0] = LOWPAN_DISPATCH_FRAG1
| ((dgram_size
>> 8) & 0x07);
291 frag_hdr
[1] = dgram_size
& 0xff;
292 memcpy(frag_hdr
+ 2, &frag_tag
, sizeof(frag_tag
));
294 payload_cap
= ieee802154_max_payload(wpan_hdr
);
296 frag_len
= round_down(payload_cap
- LOWPAN_FRAG1_HEAD_SIZE
-
297 skb_network_header_len(skb
), 8);
299 skb_offset
= skb_network_header_len(skb
);
300 skb_unprocessed
= skb
->len
- skb
->mac_len
- skb_offset
;
302 rc
= lowpan_xmit_fragment(skb
, wpan_hdr
, frag_hdr
,
303 LOWPAN_FRAG1_HEAD_SIZE
, 0,
304 frag_len
+ skb_network_header_len(skb
));
306 pr_debug("%s unable to send FRAG1 packet (tag: %d)",
311 frag_hdr
[0] &= ~LOWPAN_DISPATCH_FRAG1
;
312 frag_hdr
[0] |= LOWPAN_DISPATCH_FRAGN
;
313 frag_cap
= round_down(payload_cap
- LOWPAN_FRAGN_HEAD_SIZE
, 8);
316 dgram_offset
+= frag_len
;
317 skb_offset
+= frag_len
;
318 skb_unprocessed
-= frag_len
;
319 frag_len
= min(frag_cap
, skb_unprocessed
);
321 frag_hdr
[4] = dgram_offset
>> 3;
323 rc
= lowpan_xmit_fragment(skb
, wpan_hdr
, frag_hdr
,
324 LOWPAN_FRAGN_HEAD_SIZE
, skb_offset
,
327 pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
328 __func__
, frag_tag
, skb_offset
);
331 } while (skb_unprocessed
> frag_cap
);
334 return NET_XMIT_SUCCESS
;
341 static netdev_tx_t
lowpan_xmit(struct sk_buff
*skb
, struct net_device
*dev
)
343 struct ieee802154_hdr wpan_hdr
;
346 pr_debug("package xmit\n");
348 if (ieee802154_hdr_peek(skb
, &wpan_hdr
) < 0) {
350 return NET_XMIT_DROP
;
353 max_single
= ieee802154_max_payload(&wpan_hdr
);
355 if (skb_tail_pointer(skb
) - skb_network_header(skb
) <= max_single
) {
356 skb
->dev
= lowpan_dev_info(dev
)->real_dev
;
357 return dev_queue_xmit(skb
);
361 pr_debug("frame is too big, fragmentation is needed\n");
362 rc
= lowpan_xmit_fragmented(skb
, dev
, &wpan_hdr
);
364 return rc
< 0 ? NET_XMIT_DROP
: rc
;
368 static struct wpan_phy
*lowpan_get_phy(const struct net_device
*dev
)
370 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
371 return ieee802154_mlme_ops(real_dev
)->get_phy(real_dev
);
374 static __le16
lowpan_get_pan_id(const struct net_device
*dev
)
376 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
377 return ieee802154_mlme_ops(real_dev
)->get_pan_id(real_dev
);
380 static __le16
lowpan_get_short_addr(const struct net_device
*dev
)
382 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
383 return ieee802154_mlme_ops(real_dev
)->get_short_addr(real_dev
);
386 static u8
lowpan_get_dsn(const struct net_device
*dev
)
388 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
389 return ieee802154_mlme_ops(real_dev
)->get_dsn(real_dev
);
392 static struct header_ops lowpan_header_ops
= {
393 .create
= lowpan_header_create
,
396 static struct lock_class_key lowpan_tx_busylock
;
397 static struct lock_class_key lowpan_netdev_xmit_lock_key
;
399 static void lowpan_set_lockdep_class_one(struct net_device
*dev
,
400 struct netdev_queue
*txq
,
403 lockdep_set_class(&txq
->_xmit_lock
,
404 &lowpan_netdev_xmit_lock_key
);
408 static int lowpan_dev_init(struct net_device
*dev
)
410 netdev_for_each_tx_queue(dev
, lowpan_set_lockdep_class_one
, NULL
);
411 dev
->qdisc_tx_busylock
= &lowpan_tx_busylock
;
415 static const struct net_device_ops lowpan_netdev_ops
= {
416 .ndo_init
= lowpan_dev_init
,
417 .ndo_start_xmit
= lowpan_xmit
,
418 .ndo_set_mac_address
= lowpan_set_address
,
421 static struct ieee802154_mlme_ops lowpan_mlme
= {
422 .get_pan_id
= lowpan_get_pan_id
,
423 .get_phy
= lowpan_get_phy
,
424 .get_short_addr
= lowpan_get_short_addr
,
425 .get_dsn
= lowpan_get_dsn
,
428 static void lowpan_setup(struct net_device
*dev
)
430 dev
->addr_len
= IEEE802154_ADDR_LEN
;
431 memset(dev
->broadcast
, 0xff, IEEE802154_ADDR_LEN
);
432 dev
->type
= ARPHRD_IEEE802154
;
433 /* Frame Control + Sequence Number + Address fields + Security Header */
434 dev
->hard_header_len
= 2 + 1 + 20 + 14;
435 dev
->needed_tailroom
= 2; /* FCS */
437 dev
->tx_queue_len
= 0;
438 dev
->flags
= IFF_BROADCAST
| IFF_MULTICAST
;
439 dev
->watchdog_timeo
= 0;
441 dev
->netdev_ops
= &lowpan_netdev_ops
;
442 dev
->header_ops
= &lowpan_header_ops
;
443 dev
->ml_priv
= &lowpan_mlme
;
444 dev
->destructor
= free_netdev
;
447 static int lowpan_validate(struct nlattr
*tb
[], struct nlattr
*data
[])
449 if (tb
[IFLA_ADDRESS
]) {
450 if (nla_len(tb
[IFLA_ADDRESS
]) != IEEE802154_ADDR_LEN
)
456 static int lowpan_rcv(struct sk_buff
*skb
, struct net_device
*dev
,
457 struct packet_type
*pt
, struct net_device
*orig_dev
)
459 struct ieee802154_hdr hdr
;
462 skb
= skb_share_check(skb
, GFP_ATOMIC
);
466 if (!netif_running(dev
))
469 if (dev
->type
!= ARPHRD_IEEE802154
)
472 if (ieee802154_hdr_peek_addrs(skb
, &hdr
) < 0)
475 /* check that it's our buffer */
476 if (skb
->data
[0] == LOWPAN_DISPATCH_IPV6
) {
477 skb
->protocol
= htons(ETH_P_IPV6
);
478 skb
->pkt_type
= PACKET_HOST
;
480 /* Pull off the 1-byte of 6lowpan header. */
483 ret
= lowpan_give_skb_to_devices(skb
, NULL
);
484 if (ret
== NET_RX_DROP
)
487 switch (skb
->data
[0] & 0xe0) {
488 case LOWPAN_DISPATCH_IPHC
: /* ipv6 datagram */
489 ret
= process_data(skb
, &hdr
);
490 if (ret
== NET_RX_DROP
)
493 case LOWPAN_DISPATCH_FRAG1
: /* first fragment header */
494 ret
= lowpan_frag_rcv(skb
, LOWPAN_DISPATCH_FRAG1
);
496 ret
= process_data(skb
, &hdr
);
497 if (ret
== NET_RX_DROP
)
501 case LOWPAN_DISPATCH_FRAGN
: /* next fragments headers */
502 ret
= lowpan_frag_rcv(skb
, LOWPAN_DISPATCH_FRAGN
);
504 ret
= process_data(skb
, &hdr
);
505 if (ret
== NET_RX_DROP
)
514 return NET_RX_SUCCESS
;
521 static int lowpan_newlink(struct net
*src_net
, struct net_device
*dev
,
522 struct nlattr
*tb
[], struct nlattr
*data
[])
524 struct net_device
*real_dev
;
525 struct lowpan_dev_record
*entry
;
527 pr_debug("adding new link\n");
531 /* find and hold real wpan device */
532 real_dev
= dev_get_by_index(src_net
, nla_get_u32(tb
[IFLA_LINK
]));
535 if (real_dev
->type
!= ARPHRD_IEEE802154
) {
540 lowpan_dev_info(dev
)->real_dev
= real_dev
;
541 mutex_init(&lowpan_dev_info(dev
)->dev_list_mtx
);
543 entry
= kzalloc(sizeof(*entry
), GFP_KERNEL
);
546 lowpan_dev_info(dev
)->real_dev
= NULL
;
552 /* Set the lowpan harware address to the wpan hardware address. */
553 memcpy(dev
->dev_addr
, real_dev
->dev_addr
, IEEE802154_ADDR_LEN
);
555 mutex_lock(&lowpan_dev_info(dev
)->dev_list_mtx
);
556 INIT_LIST_HEAD(&entry
->list
);
557 list_add_tail(&entry
->list
, &lowpan_devices
);
558 mutex_unlock(&lowpan_dev_info(dev
)->dev_list_mtx
);
560 register_netdevice(dev
);
565 static void lowpan_dellink(struct net_device
*dev
, struct list_head
*head
)
567 struct lowpan_dev_info
*lowpan_dev
= lowpan_dev_info(dev
);
568 struct net_device
*real_dev
= lowpan_dev
->real_dev
;
569 struct lowpan_dev_record
*entry
, *tmp
;
573 mutex_lock(&lowpan_dev_info(dev
)->dev_list_mtx
);
574 list_for_each_entry_safe(entry
, tmp
, &lowpan_devices
, list
) {
575 if (entry
->ldev
== dev
) {
576 list_del(&entry
->list
);
580 mutex_unlock(&lowpan_dev_info(dev
)->dev_list_mtx
);
582 mutex_destroy(&lowpan_dev_info(dev
)->dev_list_mtx
);
584 unregister_netdevice_queue(dev
, head
);
589 static struct rtnl_link_ops lowpan_link_ops __read_mostly
= {
591 .priv_size
= sizeof(struct lowpan_dev_info
),
592 .setup
= lowpan_setup
,
593 .newlink
= lowpan_newlink
,
594 .dellink
= lowpan_dellink
,
595 .validate
= lowpan_validate
,
598 static inline int __init
lowpan_netlink_init(void)
600 return rtnl_link_register(&lowpan_link_ops
);
603 static inline void lowpan_netlink_fini(void)
605 rtnl_link_unregister(&lowpan_link_ops
);
608 static int lowpan_device_event(struct notifier_block
*unused
,
609 unsigned long event
, void *ptr
)
611 struct net_device
*dev
= netdev_notifier_info_to_dev(ptr
);
613 struct lowpan_dev_record
*entry
, *tmp
;
615 if (dev
->type
!= ARPHRD_IEEE802154
)
618 if (event
== NETDEV_UNREGISTER
) {
619 list_for_each_entry_safe(entry
, tmp
, &lowpan_devices
, list
) {
620 if (lowpan_dev_info(entry
->ldev
)->real_dev
== dev
)
621 lowpan_dellink(entry
->ldev
, &del_list
);
624 unregister_netdevice_many(&del_list
);
631 static struct notifier_block lowpan_dev_notifier
= {
632 .notifier_call
= lowpan_device_event
,
635 static struct packet_type lowpan_packet_type
= {
636 .type
= htons(ETH_P_IEEE802154
),
640 static int __init
lowpan_init_module(void)
644 err
= lowpan_net_frag_init();
648 err
= lowpan_netlink_init();
652 dev_add_pack(&lowpan_packet_type
);
654 err
= register_netdevice_notifier(&lowpan_dev_notifier
);
661 dev_remove_pack(&lowpan_packet_type
);
662 lowpan_netlink_fini();
664 lowpan_net_frag_exit();
669 static void __exit
lowpan_cleanup_module(void)
671 lowpan_netlink_fini();
673 dev_remove_pack(&lowpan_packet_type
);
675 lowpan_net_frag_exit();
677 unregister_netdevice_notifier(&lowpan_dev_notifier
);
680 module_init(lowpan_init_module
);
681 module_exit(lowpan_cleanup_module
);
682 MODULE_LICENSE("GPL");
683 MODULE_ALIAS_RTNL_LINK("lowpan");