2 * Copyright 2011, Siemens AG
3 * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
6 /* Based on patches from Jon Smirl <jonsmirl@gmail.com>
7 * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License version 2
11 * as published by the Free Software Foundation.
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
20 /* Jon's code is based on 6lowpan implementation for Contiki which is:
21 * Copyright (c) 2008, Swedish Institute of Computer Science.
22 * All rights reserved.
24 * Redistribution and use in source and binary forms, with or without
25 * modification, are permitted provided that the following conditions
27 * 1. Redistributions of source code must retain the above copyright
28 * notice, this list of conditions and the following disclaimer.
29 * 2. Redistributions in binary form must reproduce the above copyright
30 * notice, this list of conditions and the following disclaimer in the
31 * documentation and/or other materials provided with the distribution.
32 * 3. Neither the name of the Institute nor the names of its contributors
33 * may be used to endorse or promote products derived from this software
34 * without specific prior written permission.
36 * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
37 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
38 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
39 * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
40 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
41 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
42 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
43 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
44 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
45 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
49 #include <linux/bitops.h>
50 #include <linux/if_arp.h>
51 #include <linux/module.h>
52 #include <linux/netdevice.h>
53 #include <net/6lowpan.h>
55 #include <net/af_ieee802154.h>
59 /* Uncompress address function for source and
60 * destination address(non-multicast).
62 * address_mode is sam value or dam value.
64 static int uncompress_addr(struct sk_buff
*skb
,
65 struct in6_addr
*ipaddr
, const u8 address_mode
,
66 const u8
*lladdr
, const u8 addr_type
,
71 switch (address_mode
) {
72 case LOWPAN_IPHC_ADDR_00
:
73 /* for global link addresses */
74 fail
= lowpan_fetch_skb(skb
, ipaddr
->s6_addr
, 16);
76 case LOWPAN_IPHC_ADDR_01
:
77 /* fe:80::XXXX:XXXX:XXXX:XXXX */
78 ipaddr
->s6_addr
[0] = 0xFE;
79 ipaddr
->s6_addr
[1] = 0x80;
80 fail
= lowpan_fetch_skb(skb
, &ipaddr
->s6_addr
[8], 8);
82 case LOWPAN_IPHC_ADDR_02
:
83 /* fe:80::ff:fe00:XXXX */
84 ipaddr
->s6_addr
[0] = 0xFE;
85 ipaddr
->s6_addr
[1] = 0x80;
86 ipaddr
->s6_addr
[11] = 0xFF;
87 ipaddr
->s6_addr
[12] = 0xFE;
88 fail
= lowpan_fetch_skb(skb
, &ipaddr
->s6_addr
[14], 2);
90 case LOWPAN_IPHC_ADDR_03
:
93 case IEEE802154_ADDR_LONG
:
94 /* fe:80::XXXX:XXXX:XXXX:XXXX
98 ipaddr
->s6_addr
[0] = 0xFE;
99 ipaddr
->s6_addr
[1] = 0x80;
100 memcpy(&ipaddr
->s6_addr
[8], lladdr
, addr_len
);
101 /* second bit-flip (Universe/Local)
102 * is done according RFC2464
104 ipaddr
->s6_addr
[8] ^= 0x02;
106 case IEEE802154_ADDR_SHORT
:
107 /* fe:80::ff:fe00:XXXX
111 * Universe/Local bit is zero.
113 ipaddr
->s6_addr
[0] = 0xFE;
114 ipaddr
->s6_addr
[1] = 0x80;
115 ipaddr
->s6_addr
[11] = 0xFF;
116 ipaddr
->s6_addr
[12] = 0xFE;
117 ipaddr
->s6_addr16
[7] = htons(*((u16
*)lladdr
));
120 pr_debug("Invalid addr_type set\n");
125 pr_debug("Invalid address mode value: 0x%x\n", address_mode
);
130 pr_debug("Failed to fetch skb data\n");
134 raw_dump_inline(NULL
, "Reconstructed ipv6 addr is",
135 ipaddr
->s6_addr
, 16);
140 /* Uncompress address function for source context
141 * based address(non-multicast).
143 static int uncompress_context_based_src_addr(struct sk_buff
*skb
,
144 struct in6_addr
*ipaddr
,
148 case LOWPAN_IPHC_ADDR_00
:
150 * Do nothing, address is already ::
153 case LOWPAN_IPHC_ADDR_01
:
155 case LOWPAN_IPHC_ADDR_02
:
157 case LOWPAN_IPHC_ADDR_03
:
159 netdev_warn(skb
->dev
, "SAM value 0x%x not supported\n", sam
);
162 pr_debug("Invalid sam value: 0x%x\n", sam
);
166 raw_dump_inline(NULL
,
167 "Reconstructed context based ipv6 src addr is",
168 ipaddr
->s6_addr
, 16);
173 /* Uncompress function for multicast destination address,
176 static int lowpan_uncompress_multicast_daddr(struct sk_buff
*skb
,
177 struct in6_addr
*ipaddr
,
183 case LOWPAN_IPHC_DAM_00
:
184 /* 00: 128 bits. The full address
185 * is carried in-line.
187 fail
= lowpan_fetch_skb(skb
, ipaddr
->s6_addr
, 16);
189 case LOWPAN_IPHC_DAM_01
:
190 /* 01: 48 bits. The address takes
191 * the form ffXX::00XX:XXXX:XXXX.
193 ipaddr
->s6_addr
[0] = 0xFF;
194 fail
= lowpan_fetch_skb(skb
, &ipaddr
->s6_addr
[1], 1);
195 fail
|= lowpan_fetch_skb(skb
, &ipaddr
->s6_addr
[11], 5);
197 case LOWPAN_IPHC_DAM_10
:
198 /* 10: 32 bits. The address takes
199 * the form ffXX::00XX:XXXX.
201 ipaddr
->s6_addr
[0] = 0xFF;
202 fail
= lowpan_fetch_skb(skb
, &ipaddr
->s6_addr
[1], 1);
203 fail
|= lowpan_fetch_skb(skb
, &ipaddr
->s6_addr
[13], 3);
205 case LOWPAN_IPHC_DAM_11
:
206 /* 11: 8 bits. The address takes
207 * the form ff02::00XX.
209 ipaddr
->s6_addr
[0] = 0xFF;
210 ipaddr
->s6_addr
[1] = 0x02;
211 fail
= lowpan_fetch_skb(skb
, &ipaddr
->s6_addr
[15], 1);
214 pr_debug("DAM value has a wrong value: 0x%x\n", dam
);
219 pr_debug("Failed to fetch skb data\n");
223 raw_dump_inline(NULL
, "Reconstructed ipv6 multicast addr is",
224 ipaddr
->s6_addr
, 16);
229 /* TTL uncompression values */
230 static const u8 lowpan_ttl_values
[] = { 0, 1, 64, 255 };
233 lowpan_header_decompress(struct sk_buff
*skb
, struct net_device
*dev
,
234 const u8
*saddr
, const u8 saddr_type
,
235 const u8 saddr_len
, const u8
*daddr
,
236 const u8 daddr_type
, const u8 daddr_len
,
239 struct ipv6hdr hdr
= {};
240 u8 tmp
, num_context
= 0;
243 raw_dump_table(__func__
, "raw skb data dump uncompressed",
244 skb
->data
, skb
->len
);
246 /* another if the CID flag is set */
247 if (iphc1
& LOWPAN_IPHC_CID
) {
248 pr_debug("CID flag is set, increase header with one\n");
249 if (lowpan_fetch_skb(skb
, &num_context
, sizeof(num_context
)))
255 /* Traffic Class and Flow Label */
256 switch ((iphc0
& LOWPAN_IPHC_TF
) >> 3) {
257 /* Traffic Class and FLow Label carried in-line
258 * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
261 if (lowpan_fetch_skb(skb
, &tmp
, sizeof(tmp
)))
264 memcpy(&hdr
.flow_lbl
, &skb
->data
[0], 3);
266 hdr
.priority
= ((tmp
>> 2) & 0x0f);
267 hdr
.flow_lbl
[0] = ((tmp
>> 2) & 0x30) | (tmp
<< 6) |
268 (hdr
.flow_lbl
[0] & 0x0f);
270 /* Traffic class carried in-line
271 * ECN + DSCP (1 byte), Flow Label is elided
274 if (lowpan_fetch_skb(skb
, &tmp
, sizeof(tmp
)))
277 hdr
.priority
= ((tmp
>> 2) & 0x0f);
278 hdr
.flow_lbl
[0] = ((tmp
<< 6) & 0xC0) | ((tmp
>> 2) & 0x30);
280 /* Flow Label carried in-line
281 * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
284 if (lowpan_fetch_skb(skb
, &tmp
, sizeof(tmp
)))
287 hdr
.flow_lbl
[0] = (skb
->data
[0] & 0x0F) | ((tmp
>> 2) & 0x30);
288 memcpy(&hdr
.flow_lbl
[1], &skb
->data
[0], 2);
291 /* Traffic Class and Flow Label are elided */
299 if ((iphc0
& LOWPAN_IPHC_NH_C
) == 0) {
300 /* Next header is carried inline */
301 if (lowpan_fetch_skb(skb
, &hdr
.nexthdr
, sizeof(hdr
.nexthdr
)))
304 pr_debug("NH flag is set, next header carried inline: %02x\n",
309 if ((iphc0
& 0x03) != LOWPAN_IPHC_TTL_I
) {
310 hdr
.hop_limit
= lowpan_ttl_values
[iphc0
& 0x03];
312 if (lowpan_fetch_skb(skb
, &hdr
.hop_limit
,
313 sizeof(hdr
.hop_limit
)))
317 /* Extract SAM to the tmp variable */
318 tmp
= ((iphc1
& LOWPAN_IPHC_SAM
) >> LOWPAN_IPHC_SAM_BIT
) & 0x03;
320 if (iphc1
& LOWPAN_IPHC_SAC
) {
321 /* Source address context based uncompression */
322 pr_debug("SAC bit is set. Handle context based source address.\n");
323 err
= uncompress_context_based_src_addr(skb
, &hdr
.saddr
, tmp
);
325 /* Source address uncompression */
326 pr_debug("source address stateless compression\n");
327 err
= uncompress_addr(skb
, &hdr
.saddr
, tmp
, saddr
,
328 saddr_type
, saddr_len
);
331 /* Check on error of previous branch */
335 /* Extract DAM to the tmp variable */
336 tmp
= ((iphc1
& LOWPAN_IPHC_DAM_11
) >> LOWPAN_IPHC_DAM_BIT
) & 0x03;
338 /* check for Multicast Compression */
339 if (iphc1
& LOWPAN_IPHC_M
) {
340 if (iphc1
& LOWPAN_IPHC_DAC
) {
341 pr_debug("dest: context-based mcast compression\n");
342 /* TODO: implement this */
344 err
= lowpan_uncompress_multicast_daddr(skb
, &hdr
.daddr
,
351 err
= uncompress_addr(skb
, &hdr
.daddr
, tmp
, daddr
,
352 daddr_type
, daddr_len
);
353 pr_debug("dest: stateless compression mode %d dest %pI6c\n",
359 /* Next header data uncompression */
360 if (iphc0
& LOWPAN_IPHC_NH_C
) {
361 err
= lowpan_nhc_do_uncompression(skb
, dev
, &hdr
);
365 err
= skb_cow(skb
, sizeof(hdr
));
370 hdr
.payload_len
= htons(skb
->len
);
372 pr_debug("skb headroom size = %d, data length = %d\n",
373 skb_headroom(skb
), skb
->len
);
375 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t"
376 "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n",
377 hdr
.version
, ntohs(hdr
.payload_len
), hdr
.nexthdr
,
378 hdr
.hop_limit
, &hdr
.daddr
);
380 skb_push(skb
, sizeof(hdr
));
381 skb_reset_network_header(skb
);
382 skb_copy_to_linear_data(skb
, &hdr
, sizeof(hdr
));
384 raw_dump_table(__func__
, "raw header dump", (u8
*)&hdr
, sizeof(hdr
));
388 EXPORT_SYMBOL_GPL(lowpan_header_decompress
);
390 static u8
lowpan_compress_addr_64(u8
**hc_ptr
, u8 shift
,
391 const struct in6_addr
*ipaddr
,
392 const unsigned char *lladdr
)
396 if (is_addr_mac_addr_based(ipaddr
, lladdr
)) {
397 val
= 3; /* 0-bits */
398 pr_debug("address compression 0 bits\n");
399 } else if (lowpan_is_iid_16_bit_compressable(ipaddr
)) {
400 /* compress IID to 16 bits xxxx::XXXX */
401 lowpan_push_hc_data(hc_ptr
, &ipaddr
->s6_addr16
[7], 2);
402 val
= 2; /* 16-bits */
403 raw_dump_inline(NULL
, "Compressed ipv6 addr is (16 bits)",
406 /* do not compress IID => xxxx::IID */
407 lowpan_push_hc_data(hc_ptr
, &ipaddr
->s6_addr16
[4], 8);
408 val
= 1; /* 64-bits */
409 raw_dump_inline(NULL
, "Compressed ipv6 addr is (64 bits)",
413 return rol8(val
, shift
);
416 int lowpan_header_compress(struct sk_buff
*skb
, struct net_device
*dev
,
417 unsigned short type
, const void *_daddr
,
418 const void *_saddr
, unsigned int len
)
420 u8 tmp
, iphc0
, iphc1
, *hc_ptr
;
425 if (type
!= ETH_P_IPV6
)
431 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n"
432 "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n",
433 hdr
->version
, ntohs(hdr
->payload_len
), hdr
->nexthdr
,
434 hdr
->hop_limit
, &hdr
->daddr
);
436 raw_dump_table(__func__
, "raw skb network header dump",
437 skb_network_header(skb
), sizeof(struct ipv6hdr
));
439 /* As we copy some bit-length fields, in the IPHC encoding bytes,
440 * we sometimes use |=
441 * If the field is 0, and the current bit value in memory is 1,
442 * this does not work. We therefore reset the IPHC encoding here
444 iphc0
= LOWPAN_DISPATCH_IPHC
;
447 /* TODO: context lookup */
449 raw_dump_inline(__func__
, "saddr",
450 (unsigned char *)_saddr
, IEEE802154_ADDR_LEN
);
451 raw_dump_inline(__func__
, "daddr",
452 (unsigned char *)_daddr
, IEEE802154_ADDR_LEN
);
454 raw_dump_table(__func__
, "sending raw skb network uncompressed packet",
455 skb
->data
, skb
->len
);
457 /* Traffic class, flow label
458 * If flow label is 0, compress it. If traffic class is 0, compress it
459 * We have to process both in the same time as the offset of traffic
460 * class depends on the presence of version and flow label
463 /* hc format of TC is ECN | DSCP , original one is DSCP | ECN */
464 tmp
= (hdr
->priority
<< 4) | (hdr
->flow_lbl
[0] >> 4);
465 tmp
= ((tmp
& 0x03) << 6) | (tmp
>> 2);
467 if (((hdr
->flow_lbl
[0] & 0x0F) == 0) &&
468 (hdr
->flow_lbl
[1] == 0) && (hdr
->flow_lbl
[2] == 0)) {
469 /* flow label can be compressed */
470 iphc0
|= LOWPAN_IPHC_FL_C
;
471 if ((hdr
->priority
== 0) &&
472 ((hdr
->flow_lbl
[0] & 0xF0) == 0)) {
473 /* compress (elide) all */
474 iphc0
|= LOWPAN_IPHC_TC_C
;
476 /* compress only the flow label */
481 /* Flow label cannot be compressed */
482 if ((hdr
->priority
== 0) &&
483 ((hdr
->flow_lbl
[0] & 0xF0) == 0)) {
484 /* compress only traffic class */
485 iphc0
|= LOWPAN_IPHC_TC_C
;
486 *hc_ptr
= (tmp
& 0xc0) | (hdr
->flow_lbl
[0] & 0x0F);
487 memcpy(hc_ptr
+ 1, &hdr
->flow_lbl
[1], 2);
490 /* compress nothing */
491 memcpy(hc_ptr
, hdr
, 4);
492 /* replace the top byte with new ECN | DSCP format */
498 /* NOTE: payload length is always compressed */
500 /* Check if we provide the nhc format for nexthdr and compression
501 * functionality. If not nexthdr is handled inline and not compressed.
503 ret
= lowpan_nhc_check_compression(skb
, hdr
, &hc_ptr
, &iphc0
);
508 * if 1: compress, encoding is 01
509 * if 64: compress, encoding is 10
510 * if 255: compress, encoding is 11
511 * else do not compress
513 switch (hdr
->hop_limit
) {
515 iphc0
|= LOWPAN_IPHC_TTL_1
;
518 iphc0
|= LOWPAN_IPHC_TTL_64
;
521 iphc0
|= LOWPAN_IPHC_TTL_255
;
524 lowpan_push_hc_data(&hc_ptr
, &hdr
->hop_limit
,
525 sizeof(hdr
->hop_limit
));
528 addr_type
= ipv6_addr_type(&hdr
->saddr
);
529 /* source address compression */
530 if (addr_type
== IPV6_ADDR_ANY
) {
531 pr_debug("source address is unspecified, setting SAC\n");
532 iphc1
|= LOWPAN_IPHC_SAC
;
534 if (addr_type
& IPV6_ADDR_LINKLOCAL
) {
535 iphc1
|= lowpan_compress_addr_64(&hc_ptr
,
537 &hdr
->saddr
, _saddr
);
538 pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n",
541 pr_debug("send the full source address\n");
542 lowpan_push_hc_data(&hc_ptr
, hdr
->saddr
.s6_addr
, 16);
546 addr_type
= ipv6_addr_type(&hdr
->daddr
);
547 /* destination address compression */
548 if (addr_type
& IPV6_ADDR_MULTICAST
) {
549 pr_debug("destination address is multicast: ");
550 iphc1
|= LOWPAN_IPHC_M
;
551 if (lowpan_is_mcast_addr_compressable8(&hdr
->daddr
)) {
552 pr_debug("compressed to 1 octet\n");
553 iphc1
|= LOWPAN_IPHC_DAM_11
;
555 lowpan_push_hc_data(&hc_ptr
,
556 &hdr
->daddr
.s6_addr
[15], 1);
557 } else if (lowpan_is_mcast_addr_compressable32(&hdr
->daddr
)) {
558 pr_debug("compressed to 4 octets\n");
559 iphc1
|= LOWPAN_IPHC_DAM_10
;
560 /* second byte + the last three */
561 lowpan_push_hc_data(&hc_ptr
,
562 &hdr
->daddr
.s6_addr
[1], 1);
563 lowpan_push_hc_data(&hc_ptr
,
564 &hdr
->daddr
.s6_addr
[13], 3);
565 } else if (lowpan_is_mcast_addr_compressable48(&hdr
->daddr
)) {
566 pr_debug("compressed to 6 octets\n");
567 iphc1
|= LOWPAN_IPHC_DAM_01
;
568 /* second byte + the last five */
569 lowpan_push_hc_data(&hc_ptr
,
570 &hdr
->daddr
.s6_addr
[1], 1);
571 lowpan_push_hc_data(&hc_ptr
,
572 &hdr
->daddr
.s6_addr
[11], 5);
574 pr_debug("using full address\n");
575 iphc1
|= LOWPAN_IPHC_DAM_00
;
576 lowpan_push_hc_data(&hc_ptr
, hdr
->daddr
.s6_addr
, 16);
579 if (addr_type
& IPV6_ADDR_LINKLOCAL
) {
580 /* TODO: context lookup */
581 iphc1
|= lowpan_compress_addr_64(&hc_ptr
,
582 LOWPAN_IPHC_DAM_BIT
, &hdr
->daddr
, _daddr
);
583 pr_debug("dest address unicast link-local %pI6c "
584 "iphc1 0x%02x\n", &hdr
->daddr
, iphc1
);
586 pr_debug("dest address unicast %pI6c\n", &hdr
->daddr
);
587 lowpan_push_hc_data(&hc_ptr
, hdr
->daddr
.s6_addr
, 16);
591 /* next header compression */
592 if (iphc0
& LOWPAN_IPHC_NH_C
) {
593 ret
= lowpan_nhc_do_compression(skb
, hdr
, &hc_ptr
);
601 skb_pull(skb
, sizeof(struct ipv6hdr
));
602 skb_reset_transport_header(skb
);
603 memcpy(skb_push(skb
, hc_ptr
- head
), head
, hc_ptr
- head
);
604 skb_reset_network_header(skb
);
606 pr_debug("header len %d skb %u\n", (int)(hc_ptr
- head
), skb
->len
);
608 raw_dump_table(__func__
, "raw skb data dump compressed",
609 skb
->data
, skb
->len
);
612 EXPORT_SYMBOL_GPL(lowpan_header_compress
);
614 static int __init
lowpan_module_init(void)
616 request_module_nowait("nhc_dest");
617 request_module_nowait("nhc_fragment");
618 request_module_nowait("nhc_hop");
619 request_module_nowait("nhc_ipv6");
620 request_module_nowait("nhc_mobility");
621 request_module_nowait("nhc_routing");
622 request_module_nowait("nhc_udp");
626 module_init(lowpan_module_init
);
628 MODULE_LICENSE("GPL");