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
.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
);
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
, const struct ieee802154_hdr
*hdr
)
174 struct ieee802154_addr_sa sa
, da
;
177 raw_dump_table(__func__
, "raw skb data dump", skb
->data
, skb
->len
);
178 /* at least two bytes will be used for the encoding */
182 if (lowpan_fetch_skb_u8(skb
, &iphc0
))
185 if (lowpan_fetch_skb_u8(skb
, &iphc1
))
188 ieee802154_addr_to_sa(&sa
, &hdr
->source
);
189 ieee802154_addr_to_sa(&da
, &hdr
->dest
);
191 if (sa
.addr_type
== IEEE802154_ADDR_SHORT
)
192 sap
= &sa
.short_addr
;
196 if (da
.addr_type
== IEEE802154_ADDR_SHORT
)
197 dap
= &da
.short_addr
;
201 return lowpan_process_data(skb
, skb
->dev
, sap
, sa
.addr_type
,
202 IEEE802154_ADDR_LEN
, dap
, da
.addr_type
,
203 IEEE802154_ADDR_LEN
, iphc0
, iphc1
,
204 lowpan_give_skb_to_devices
);
211 static int lowpan_set_address(struct net_device
*dev
, void *p
)
213 struct sockaddr
*sa
= p
;
215 if (netif_running(dev
))
218 /* TODO: validate addr */
219 memcpy(dev
->dev_addr
, sa
->sa_data
, dev
->addr_len
);
225 lowpan_fragment_xmit(struct sk_buff
*skb
, u8
*head
,
226 int mlen
, int plen
, int offset
, int type
)
228 struct sk_buff
*frag
;
231 hlen
= (type
== LOWPAN_DISPATCH_FRAG1
) ?
232 LOWPAN_FRAG1_HEAD_SIZE
: LOWPAN_FRAGN_HEAD_SIZE
;
234 raw_dump_inline(__func__
, "6lowpan fragment header", head
, hlen
);
236 frag
= netdev_alloc_skb(skb
->dev
,
237 hlen
+ mlen
+ plen
+ IEEE802154_MFR_SIZE
);
241 frag
->priority
= skb
->priority
;
243 /* copy header, MFR and payload */
245 skb_copy_to_linear_data(frag
, skb_mac_header(skb
), mlen
);
248 skb_copy_to_linear_data_offset(frag
, mlen
, head
, hlen
);
251 skb_copy_to_linear_data_offset(frag
, mlen
+ hlen
,
252 skb_network_header(skb
) + offset
, plen
);
254 raw_dump_table(__func__
, " raw fragment dump", frag
->data
, frag
->len
);
256 return dev_queue_xmit(frag
);
260 lowpan_skb_fragmentation(struct sk_buff
*skb
, struct net_device
*dev
)
263 u16 dgram_offset
, dgram_size
, payload_length
, header_length
,
264 lowpan_size
, frag_plen
, offset
;
268 header_length
= skb
->mac_len
;
269 payload_length
= skb
->len
- header_length
;
270 tag
= lowpan_dev_info(dev
)->fragment_tag
++;
271 lowpan_size
= skb_network_header_len(skb
);
272 dgram_size
= lowpan_uncompress_size(skb
, &dgram_offset
) -
275 /* first fragment header */
276 head
[0] = LOWPAN_DISPATCH_FRAG1
| ((dgram_size
>> 8) & 0x7);
277 head
[1] = dgram_size
& 0xff;
278 memcpy(head
+ 2, &tag
, sizeof(tag
));
280 /* calc the nearest payload length(divided to 8) for first fragment
281 * which fits into a IEEE802154_MTU
283 frag_plen
= round_down(IEEE802154_MTU
- header_length
-
284 LOWPAN_FRAG1_HEAD_SIZE
- lowpan_size
-
285 IEEE802154_MFR_SIZE
, 8);
287 err
= lowpan_fragment_xmit(skb
, head
, header_length
,
288 frag_plen
+ lowpan_size
, 0,
289 LOWPAN_DISPATCH_FRAG1
);
291 pr_debug("%s unable to send FRAG1 packet (tag: %d)",
296 offset
= lowpan_size
+ frag_plen
;
297 dgram_offset
+= frag_plen
;
299 /* next fragment header */
300 head
[0] &= ~LOWPAN_DISPATCH_FRAG1
;
301 head
[0] |= LOWPAN_DISPATCH_FRAGN
;
303 frag_plen
= round_down(IEEE802154_MTU
- header_length
-
304 LOWPAN_FRAGN_HEAD_SIZE
- IEEE802154_MFR_SIZE
, 8);
306 while (payload_length
- offset
> 0) {
309 head
[4] = dgram_offset
>> 3;
311 if (payload_length
- offset
< len
)
312 len
= payload_length
- offset
;
314 err
= lowpan_fragment_xmit(skb
, head
, header_length
, len
,
315 offset
, LOWPAN_DISPATCH_FRAGN
);
317 pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
318 __func__
, tag
, offset
);
330 static netdev_tx_t
lowpan_xmit(struct sk_buff
*skb
, struct net_device
*dev
)
334 pr_debug("package xmit\n");
336 skb
->dev
= lowpan_dev_info(dev
)->real_dev
;
337 if (skb
->dev
== NULL
) {
338 pr_debug("ERROR: no real wpan device found\n");
342 /* Send directly if less than the MTU minus the 2 checksum bytes. */
343 if (skb
->len
<= IEEE802154_MTU
- IEEE802154_MFR_SIZE
) {
344 err
= dev_queue_xmit(skb
);
348 pr_debug("frame is too big, fragmentation is needed\n");
349 err
= lowpan_skb_fragmentation(skb
, dev
);
354 pr_debug("ERROR: xmit failed\n");
356 return (err
< 0) ? NET_XMIT_DROP
: err
;
359 static struct wpan_phy
*lowpan_get_phy(const struct net_device
*dev
)
361 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
362 return ieee802154_mlme_ops(real_dev
)->get_phy(real_dev
);
365 static __le16
lowpan_get_pan_id(const struct net_device
*dev
)
367 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
368 return ieee802154_mlme_ops(real_dev
)->get_pan_id(real_dev
);
371 static __le16
lowpan_get_short_addr(const struct net_device
*dev
)
373 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
374 return ieee802154_mlme_ops(real_dev
)->get_short_addr(real_dev
);
377 static u8
lowpan_get_dsn(const struct net_device
*dev
)
379 struct net_device
*real_dev
= lowpan_dev_info(dev
)->real_dev
;
380 return ieee802154_mlme_ops(real_dev
)->get_dsn(real_dev
);
383 static struct header_ops lowpan_header_ops
= {
384 .create
= lowpan_header_create
,
387 static struct lock_class_key lowpan_tx_busylock
;
388 static struct lock_class_key lowpan_netdev_xmit_lock_key
;
390 static void lowpan_set_lockdep_class_one(struct net_device
*dev
,
391 struct netdev_queue
*txq
,
394 lockdep_set_class(&txq
->_xmit_lock
,
395 &lowpan_netdev_xmit_lock_key
);
399 static int lowpan_dev_init(struct net_device
*dev
)
401 netdev_for_each_tx_queue(dev
, lowpan_set_lockdep_class_one
, NULL
);
402 dev
->qdisc_tx_busylock
= &lowpan_tx_busylock
;
406 static const struct net_device_ops lowpan_netdev_ops
= {
407 .ndo_init
= lowpan_dev_init
,
408 .ndo_start_xmit
= lowpan_xmit
,
409 .ndo_set_mac_address
= lowpan_set_address
,
412 static struct ieee802154_mlme_ops lowpan_mlme
= {
413 .get_pan_id
= lowpan_get_pan_id
,
414 .get_phy
= lowpan_get_phy
,
415 .get_short_addr
= lowpan_get_short_addr
,
416 .get_dsn
= lowpan_get_dsn
,
419 static void lowpan_setup(struct net_device
*dev
)
421 dev
->addr_len
= IEEE802154_ADDR_LEN
;
422 memset(dev
->broadcast
, 0xff, IEEE802154_ADDR_LEN
);
423 dev
->type
= ARPHRD_IEEE802154
;
424 /* Frame Control + Sequence Number + Address fields + Security Header */
425 dev
->hard_header_len
= 2 + 1 + 20 + 14;
426 dev
->needed_tailroom
= 2; /* FCS */
428 dev
->tx_queue_len
= 0;
429 dev
->flags
= IFF_BROADCAST
| IFF_MULTICAST
;
430 dev
->watchdog_timeo
= 0;
432 dev
->netdev_ops
= &lowpan_netdev_ops
;
433 dev
->header_ops
= &lowpan_header_ops
;
434 dev
->ml_priv
= &lowpan_mlme
;
435 dev
->destructor
= free_netdev
;
438 static int lowpan_validate(struct nlattr
*tb
[], struct nlattr
*data
[])
440 if (tb
[IFLA_ADDRESS
]) {
441 if (nla_len(tb
[IFLA_ADDRESS
]) != IEEE802154_ADDR_LEN
)
447 static int lowpan_rcv(struct sk_buff
*skb
, struct net_device
*dev
,
448 struct packet_type
*pt
, struct net_device
*orig_dev
)
450 struct sk_buff
*local_skb
;
451 struct ieee802154_hdr hdr
;
454 if (!netif_running(dev
))
457 if (dev
->type
!= ARPHRD_IEEE802154
)
460 if (ieee802154_hdr_peek_addrs(skb
, &hdr
) < 0)
463 local_skb
= skb_clone(skb
, GFP_ATOMIC
);
469 /* check that it's our buffer */
470 if (skb
->data
[0] == LOWPAN_DISPATCH_IPV6
) {
471 local_skb
->protocol
= htons(ETH_P_IPV6
);
472 local_skb
->pkt_type
= PACKET_HOST
;
474 /* Pull off the 1-byte of 6lowpan header. */
475 skb_pull(local_skb
, 1);
477 ret
= lowpan_give_skb_to_devices(local_skb
, NULL
);
478 if (ret
== NET_RX_DROP
)
481 switch (skb
->data
[0] & 0xe0) {
482 case LOWPAN_DISPATCH_IPHC
: /* ipv6 datagram */
483 ret
= process_data(local_skb
, &hdr
);
484 if (ret
== NET_RX_DROP
)
487 case LOWPAN_DISPATCH_FRAG1
: /* first fragment header */
488 ret
= lowpan_frag_rcv(local_skb
, LOWPAN_DISPATCH_FRAG1
);
490 ret
= process_data(local_skb
, &hdr
);
491 if (ret
== NET_RX_DROP
)
495 case LOWPAN_DISPATCH_FRAGN
: /* next fragments headers */
496 ret
= lowpan_frag_rcv(local_skb
, LOWPAN_DISPATCH_FRAGN
);
498 ret
= process_data(local_skb
, &hdr
);
499 if (ret
== NET_RX_DROP
)
508 return NET_RX_SUCCESS
;
515 static int lowpan_newlink(struct net
*src_net
, struct net_device
*dev
,
516 struct nlattr
*tb
[], struct nlattr
*data
[])
518 struct net_device
*real_dev
;
519 struct lowpan_dev_record
*entry
;
521 pr_debug("adding new link\n");
525 /* find and hold real wpan device */
526 real_dev
= dev_get_by_index(src_net
, nla_get_u32(tb
[IFLA_LINK
]));
529 if (real_dev
->type
!= ARPHRD_IEEE802154
) {
534 lowpan_dev_info(dev
)->real_dev
= real_dev
;
535 mutex_init(&lowpan_dev_info(dev
)->dev_list_mtx
);
537 entry
= kzalloc(sizeof(*entry
), GFP_KERNEL
);
540 lowpan_dev_info(dev
)->real_dev
= NULL
;
546 /* Set the lowpan harware address to the wpan hardware address. */
547 memcpy(dev
->dev_addr
, real_dev
->dev_addr
, IEEE802154_ADDR_LEN
);
549 mutex_lock(&lowpan_dev_info(dev
)->dev_list_mtx
);
550 INIT_LIST_HEAD(&entry
->list
);
551 list_add_tail(&entry
->list
, &lowpan_devices
);
552 mutex_unlock(&lowpan_dev_info(dev
)->dev_list_mtx
);
554 register_netdevice(dev
);
559 static void lowpan_dellink(struct net_device
*dev
, struct list_head
*head
)
561 struct lowpan_dev_info
*lowpan_dev
= lowpan_dev_info(dev
);
562 struct net_device
*real_dev
= lowpan_dev
->real_dev
;
563 struct lowpan_dev_record
*entry
, *tmp
;
567 mutex_lock(&lowpan_dev_info(dev
)->dev_list_mtx
);
568 list_for_each_entry_safe(entry
, tmp
, &lowpan_devices
, list
) {
569 if (entry
->ldev
== dev
) {
570 list_del(&entry
->list
);
574 mutex_unlock(&lowpan_dev_info(dev
)->dev_list_mtx
);
576 mutex_destroy(&lowpan_dev_info(dev
)->dev_list_mtx
);
578 unregister_netdevice_queue(dev
, head
);
583 static struct rtnl_link_ops lowpan_link_ops __read_mostly
= {
585 .priv_size
= sizeof(struct lowpan_dev_info
),
586 .setup
= lowpan_setup
,
587 .newlink
= lowpan_newlink
,
588 .dellink
= lowpan_dellink
,
589 .validate
= lowpan_validate
,
592 static inline int __init
lowpan_netlink_init(void)
594 return rtnl_link_register(&lowpan_link_ops
);
597 static inline void lowpan_netlink_fini(void)
599 rtnl_link_unregister(&lowpan_link_ops
);
602 static int lowpan_device_event(struct notifier_block
*unused
,
603 unsigned long event
, void *ptr
)
605 struct net_device
*dev
= netdev_notifier_info_to_dev(ptr
);
607 struct lowpan_dev_record
*entry
, *tmp
;
609 if (dev
->type
!= ARPHRD_IEEE802154
)
612 if (event
== NETDEV_UNREGISTER
) {
613 list_for_each_entry_safe(entry
, tmp
, &lowpan_devices
, list
) {
614 if (lowpan_dev_info(entry
->ldev
)->real_dev
== dev
)
615 lowpan_dellink(entry
->ldev
, &del_list
);
618 unregister_netdevice_many(&del_list
);
625 static struct notifier_block lowpan_dev_notifier
= {
626 .notifier_call
= lowpan_device_event
,
629 static struct packet_type lowpan_packet_type
= {
630 .type
= htons(ETH_P_IEEE802154
),
634 static int __init
lowpan_init_module(void)
638 err
= lowpan_net_frag_init();
642 err
= lowpan_netlink_init();
646 dev_add_pack(&lowpan_packet_type
);
648 err
= register_netdevice_notifier(&lowpan_dev_notifier
);
655 dev_remove_pack(&lowpan_packet_type
);
656 lowpan_netlink_fini();
658 lowpan_net_frag_exit();
663 static void __exit
lowpan_cleanup_module(void)
665 lowpan_netlink_fini();
667 dev_remove_pack(&lowpan_packet_type
);
669 lowpan_net_frag_exit();
671 unregister_netdevice_notifier(&lowpan_dev_notifier
);
674 module_init(lowpan_init_module
);
675 module_exit(lowpan_cleanup_module
);
676 MODULE_LICENSE("GPL");
677 MODULE_ALIAS_RTNL_LINK("lowpan");