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
;
97 * if this package isn't ipv6 one, where should it be routed?
99 if (type
!= ETH_P_IPV6
)
103 saddr
= dev
->dev_addr
;
105 raw_dump_inline(__func__
, "saddr", (unsigned char *)saddr
, 8);
106 raw_dump_inline(__func__
, "daddr", (unsigned char *)daddr
, 8);
108 lowpan_header_compress(skb
, dev
, type
, daddr
, saddr
, len
);
110 /* NOTE1: I'm still unsure about the fact that compression and WPAN
111 * header are created here and not later in the xmit. So wait for
112 * an opinion of net maintainers.
114 /* NOTE2: to be absolutely correct, we must derive PANid information
115 * from MAC subif of the 'dev' and 'real_dev' network devices, but
116 * this isn't implemented in mainline yet, so currently we assign 0xff
118 mac_cb(skb
)->flags
= IEEE802154_FC_TYPE_DATA
;
119 mac_cb(skb
)->seq
= ieee802154_mlme_ops(dev
)->get_dsn(dev
);
121 /* prepare wpan address data */
122 sa
.addr_type
= IEEE802154_ADDR_LONG
;
123 sa
.pan_id
= ieee802154_mlme_ops(dev
)->get_pan_id(dev
);
125 memcpy(&(sa
.hwaddr
), saddr
, 8);
126 /* intra-PAN communications */
127 da
.pan_id
= ieee802154_mlme_ops(dev
)->get_pan_id(dev
);
129 /* if the destination address is the broadcast address, use the
130 * corresponding short address
132 if (lowpan_is_addr_broadcast(daddr
)) {
133 da
.addr_type
= IEEE802154_ADDR_SHORT
;
134 da
.short_addr
= IEEE802154_ADDR_BROADCAST
;
136 da
.addr_type
= IEEE802154_ADDR_LONG
;
137 memcpy(&(da
.hwaddr
), daddr
, IEEE802154_ADDR_LEN
);
139 /* request acknowledgment */
140 mac_cb(skb
)->flags
|= MAC_CB_FLAG_ACKREQ
;
143 return dev_hard_header(skb
, lowpan_dev_info(dev
)->real_dev
,
144 type
, (void *)&da
, (void *)&sa
, skb
->len
);
147 static int lowpan_give_skb_to_devices(struct sk_buff
*skb
,
148 struct net_device
*dev
)
150 struct lowpan_dev_record
*entry
;
151 struct sk_buff
*skb_cp
;
152 int stat
= NET_RX_SUCCESS
;
155 list_for_each_entry_rcu(entry
, &lowpan_devices
, list
)
156 if (lowpan_dev_info(entry
->ldev
)->real_dev
== skb
->dev
) {
157 skb_cp
= skb_copy(skb
, GFP_ATOMIC
);
163 skb_cp
->dev
= entry
->ldev
;
164 stat
= netif_rx(skb_cp
);
171 static int process_data(struct sk_buff
*skb
)
174 const struct ieee802154_addr
*_saddr
, *_daddr
;
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 _saddr
= &mac_cb(skb
)->sa
;
188 _daddr
= &mac_cb(skb
)->da
;
190 return lowpan_process_data(skb
, skb
->dev
, (u8
*)_saddr
->hwaddr
,
191 _saddr
->addr_type
, IEEE802154_ADDR_LEN
,
192 (u8
*)_daddr
->hwaddr
, _daddr
->addr_type
,
193 IEEE802154_ADDR_LEN
, iphc0
, iphc1
,
194 lowpan_give_skb_to_devices
);
201 static int lowpan_set_address(struct net_device
*dev
, void *p
)
203 struct sockaddr
*sa
= p
;
205 if (netif_running(dev
))
208 /* TODO: validate addr */
209 memcpy(dev
->dev_addr
, sa
->sa_data
, dev
->addr_len
);
215 lowpan_fragment_xmit(struct sk_buff
*skb
, u8
*head
,
216 int mlen
, int plen
, int offset
, int type
)
218 struct sk_buff
*frag
;
221 hlen
= (type
== LOWPAN_DISPATCH_FRAG1
) ?
222 LOWPAN_FRAG1_HEAD_SIZE
: LOWPAN_FRAGN_HEAD_SIZE
;
224 raw_dump_inline(__func__
, "6lowpan fragment header", head
, hlen
);
226 frag
= netdev_alloc_skb(skb
->dev
,
227 hlen
+ mlen
+ plen
+ IEEE802154_MFR_SIZE
);
231 frag
->priority
= skb
->priority
;
233 /* copy header, MFR and payload */
235 skb_copy_to_linear_data(frag
, skb_mac_header(skb
), mlen
);
238 skb_copy_to_linear_data_offset(frag
, mlen
, head
, hlen
);
241 skb_copy_to_linear_data_offset(frag
, mlen
+ hlen
,
242 skb_network_header(skb
) + offset
, plen
);
244 raw_dump_table(__func__
, " raw fragment dump", frag
->data
, frag
->len
);
246 return dev_queue_xmit(frag
);
250 lowpan_skb_fragmentation(struct sk_buff
*skb
, struct net_device
*dev
)
253 u16 dgram_offset
, dgram_size
, payload_length
, header_length
,
254 lowpan_size
, frag_plen
, offset
;
258 header_length
= skb
->mac_len
;
259 payload_length
= skb
->len
- header_length
;
260 tag
= lowpan_dev_info(dev
)->fragment_tag
++;
261 lowpan_size
= skb_network_header_len(skb
);
262 dgram_size
= lowpan_uncompress_size(skb
, &dgram_offset
) -
265 /* first fragment header */
266 head
[0] = LOWPAN_DISPATCH_FRAG1
| ((dgram_size
>> 8) & 0x7);
267 head
[1] = dgram_size
& 0xff;
268 memcpy(head
+ 2, &tag
, sizeof(tag
));
270 /* calc the nearest payload length(divided to 8) for first fragment
271 * which fits into a IEEE802154_MTU
273 frag_plen
= round_down(IEEE802154_MTU
- header_length
-
274 LOWPAN_FRAG1_HEAD_SIZE
- lowpan_size
-
275 IEEE802154_MFR_SIZE
, 8);
277 err
= lowpan_fragment_xmit(skb
, head
, header_length
,
278 frag_plen
+ lowpan_size
, 0,
279 LOWPAN_DISPATCH_FRAG1
);
281 pr_debug("%s unable to send FRAG1 packet (tag: %d)",
286 offset
= lowpan_size
+ frag_plen
;
287 dgram_offset
+= frag_plen
;
289 /* next fragment header */
290 head
[0] &= ~LOWPAN_DISPATCH_FRAG1
;
291 head
[0] |= LOWPAN_DISPATCH_FRAGN
;
293 frag_plen
= round_down(IEEE802154_MTU
- header_length
-
294 LOWPAN_FRAGN_HEAD_SIZE
- IEEE802154_MFR_SIZE
, 8);
296 while (payload_length
- offset
> 0) {
299 head
[4] = dgram_offset
>> 3;
301 if (payload_length
- offset
< len
)
302 len
= payload_length
- offset
;
304 err
= lowpan_fragment_xmit(skb
, head
, header_length
, len
,
305 offset
, LOWPAN_DISPATCH_FRAGN
);
307 pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
308 __func__
, tag
, offset
);
320 static netdev_tx_t
lowpan_xmit(struct sk_buff
*skb
, struct net_device
*dev
)
324 pr_debug("package xmit\n");
326 skb
->dev
= lowpan_dev_info(dev
)->real_dev
;
327 if (skb
->dev
== NULL
) {
328 pr_debug("ERROR: no real wpan device found\n");
332 /* Send directly if less than the MTU minus the 2 checksum bytes. */
333 if (skb
->len
<= IEEE802154_MTU
- IEEE802154_MFR_SIZE
) {
334 err
= dev_queue_xmit(skb
);
338 pr_debug("frame is too big, fragmentation is needed\n");
339 err
= lowpan_skb_fragmentation(skb
, dev
);
344 pr_debug("ERROR: xmit failed\n");
346 return (err
< 0) ? NET_XMIT_DROP
: err
;
349 static struct wpan_phy
*lowpan_get_phy(const struct net_device
*dev
)
351 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
352 return ieee802154_mlme_ops(real_dev
)->get_phy(real_dev
);
355 static u16
lowpan_get_pan_id(const struct net_device
*dev
)
357 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
358 return ieee802154_mlme_ops(real_dev
)->get_pan_id(real_dev
);
361 static u16
lowpan_get_short_addr(const struct net_device
*dev
)
363 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
364 return ieee802154_mlme_ops(real_dev
)->get_short_addr(real_dev
);
367 static u8
lowpan_get_dsn(const struct net_device
*dev
)
369 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
370 return ieee802154_mlme_ops(real_dev
)->get_dsn(real_dev
);
373 static struct header_ops lowpan_header_ops
= {
374 .create
= lowpan_header_create
,
377 static struct lock_class_key lowpan_tx_busylock
;
378 static struct lock_class_key lowpan_netdev_xmit_lock_key
;
380 static void lowpan_set_lockdep_class_one(struct net_device
*dev
,
381 struct netdev_queue
*txq
,
384 lockdep_set_class(&txq
->_xmit_lock
,
385 &lowpan_netdev_xmit_lock_key
);
389 static int lowpan_dev_init(struct net_device
*dev
)
391 netdev_for_each_tx_queue(dev
, lowpan_set_lockdep_class_one
, NULL
);
392 dev
->qdisc_tx_busylock
= &lowpan_tx_busylock
;
396 static const struct net_device_ops lowpan_netdev_ops
= {
397 .ndo_init
= lowpan_dev_init
,
398 .ndo_start_xmit
= lowpan_xmit
,
399 .ndo_set_mac_address
= lowpan_set_address
,
402 static struct ieee802154_mlme_ops lowpan_mlme
= {
403 .get_pan_id
= lowpan_get_pan_id
,
404 .get_phy
= lowpan_get_phy
,
405 .get_short_addr
= lowpan_get_short_addr
,
406 .get_dsn
= lowpan_get_dsn
,
409 static void lowpan_setup(struct net_device
*dev
)
411 dev
->addr_len
= IEEE802154_ADDR_LEN
;
412 memset(dev
->broadcast
, 0xff, IEEE802154_ADDR_LEN
);
413 dev
->type
= ARPHRD_IEEE802154
;
414 /* Frame Control + Sequence Number + Address fields + Security Header */
415 dev
->hard_header_len
= 2 + 1 + 20 + 14;
416 dev
->needed_tailroom
= 2; /* FCS */
418 dev
->tx_queue_len
= 0;
419 dev
->flags
= IFF_BROADCAST
| IFF_MULTICAST
;
420 dev
->watchdog_timeo
= 0;
422 dev
->netdev_ops
= &lowpan_netdev_ops
;
423 dev
->header_ops
= &lowpan_header_ops
;
424 dev
->ml_priv
= &lowpan_mlme
;
425 dev
->destructor
= free_netdev
;
428 static int lowpan_validate(struct nlattr
*tb
[], struct nlattr
*data
[])
430 if (tb
[IFLA_ADDRESS
]) {
431 if (nla_len(tb
[IFLA_ADDRESS
]) != IEEE802154_ADDR_LEN
)
437 static int lowpan_rcv(struct sk_buff
*skb
, struct net_device
*dev
,
438 struct packet_type
*pt
, struct net_device
*orig_dev
)
440 struct sk_buff
*local_skb
;
443 if (!netif_running(dev
))
446 if (dev
->type
!= ARPHRD_IEEE802154
)
449 local_skb
= skb_clone(skb
, GFP_ATOMIC
);
455 /* check that it's our buffer */
456 if (skb
->data
[0] == LOWPAN_DISPATCH_IPV6
) {
457 local_skb
->protocol
= htons(ETH_P_IPV6
);
458 local_skb
->pkt_type
= PACKET_HOST
;
460 /* Pull off the 1-byte of 6lowpan header. */
461 skb_pull(local_skb
, 1);
463 ret
= lowpan_give_skb_to_devices(local_skb
, NULL
);
464 if (ret
== NET_RX_DROP
)
467 switch (skb
->data
[0] & 0xe0) {
468 case LOWPAN_DISPATCH_IPHC
: /* ipv6 datagram */
469 ret
= process_data(local_skb
);
470 if (ret
== NET_RX_DROP
)
473 case LOWPAN_DISPATCH_FRAG1
: /* first fragment header */
474 ret
= lowpan_frag_rcv(local_skb
, LOWPAN_DISPATCH_FRAG1
);
476 ret
= process_data(local_skb
);
477 if (ret
== NET_RX_DROP
)
481 case LOWPAN_DISPATCH_FRAGN
: /* next fragments headers */
482 ret
= lowpan_frag_rcv(local_skb
, LOWPAN_DISPATCH_FRAGN
);
484 ret
= process_data(local_skb
);
485 if (ret
== NET_RX_DROP
)
494 return NET_RX_SUCCESS
;
501 static int lowpan_newlink(struct net
*src_net
, struct net_device
*dev
,
502 struct nlattr
*tb
[], struct nlattr
*data
[])
504 struct net_device
*real_dev
;
505 struct lowpan_dev_record
*entry
;
507 pr_debug("adding new link\n");
511 /* find and hold real wpan device */
512 real_dev
= dev_get_by_index(src_net
, nla_get_u32(tb
[IFLA_LINK
]));
515 if (real_dev
->type
!= ARPHRD_IEEE802154
) {
520 lowpan_dev_info(dev
)->real_dev
= real_dev
;
521 mutex_init(&lowpan_dev_info(dev
)->dev_list_mtx
);
523 entry
= kzalloc(sizeof(*entry
), GFP_KERNEL
);
526 lowpan_dev_info(dev
)->real_dev
= NULL
;
532 /* Set the lowpan harware address to the wpan hardware address. */
533 memcpy(dev
->dev_addr
, real_dev
->dev_addr
, IEEE802154_ADDR_LEN
);
535 mutex_lock(&lowpan_dev_info(dev
)->dev_list_mtx
);
536 INIT_LIST_HEAD(&entry
->list
);
537 list_add_tail(&entry
->list
, &lowpan_devices
);
538 mutex_unlock(&lowpan_dev_info(dev
)->dev_list_mtx
);
540 register_netdevice(dev
);
545 static void lowpan_dellink(struct net_device
*dev
, struct list_head
*head
)
547 struct lowpan_dev_info
*lowpan_dev
= lowpan_dev_info(dev
);
548 struct net_device
*real_dev
= lowpan_dev
->real_dev
;
549 struct lowpan_dev_record
*entry
, *tmp
;
553 mutex_lock(&lowpan_dev_info(dev
)->dev_list_mtx
);
554 list_for_each_entry_safe(entry
, tmp
, &lowpan_devices
, list
) {
555 if (entry
->ldev
== dev
) {
556 list_del(&entry
->list
);
560 mutex_unlock(&lowpan_dev_info(dev
)->dev_list_mtx
);
562 mutex_destroy(&lowpan_dev_info(dev
)->dev_list_mtx
);
564 unregister_netdevice_queue(dev
, head
);
569 static struct rtnl_link_ops lowpan_link_ops __read_mostly
= {
571 .priv_size
= sizeof(struct lowpan_dev_info
),
572 .setup
= lowpan_setup
,
573 .newlink
= lowpan_newlink
,
574 .dellink
= lowpan_dellink
,
575 .validate
= lowpan_validate
,
578 static inline int __init
lowpan_netlink_init(void)
580 return rtnl_link_register(&lowpan_link_ops
);
583 static inline void lowpan_netlink_fini(void)
585 rtnl_link_unregister(&lowpan_link_ops
);
588 static int lowpan_device_event(struct notifier_block
*unused
,
589 unsigned long event
, void *ptr
)
591 struct net_device
*dev
= netdev_notifier_info_to_dev(ptr
);
593 struct lowpan_dev_record
*entry
, *tmp
;
595 if (dev
->type
!= ARPHRD_IEEE802154
)
598 if (event
== NETDEV_UNREGISTER
) {
599 list_for_each_entry_safe(entry
, tmp
, &lowpan_devices
, list
) {
600 if (lowpan_dev_info(entry
->ldev
)->real_dev
== dev
)
601 lowpan_dellink(entry
->ldev
, &del_list
);
604 unregister_netdevice_many(&del_list
);
611 static struct notifier_block lowpan_dev_notifier
= {
612 .notifier_call
= lowpan_device_event
,
615 static struct packet_type lowpan_packet_type
= {
616 .type
= htons(ETH_P_IEEE802154
),
620 static int __init
lowpan_init_module(void)
624 err
= lowpan_net_frag_init();
628 err
= lowpan_netlink_init();
632 dev_add_pack(&lowpan_packet_type
);
634 err
= register_netdevice_notifier(&lowpan_dev_notifier
);
641 dev_remove_pack(&lowpan_packet_type
);
642 lowpan_netlink_fini();
644 lowpan_net_frag_exit();
649 static void __exit
lowpan_cleanup_module(void)
651 lowpan_netlink_fini();
653 dev_remove_pack(&lowpan_packet_type
);
655 lowpan_net_frag_exit();
657 unregister_netdevice_notifier(&lowpan_dev_notifier
);
660 module_init(lowpan_init_module
);
661 module_exit(lowpan_cleanup_module
);
662 MODULE_LICENSE("GPL");
663 MODULE_ALIAS_RTNL_LINK("lowpan");