12c6bed7cSAlexander Aring /* 22c6bed7cSAlexander Aring * Copyright 2011, Siemens AG 32c6bed7cSAlexander Aring * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com> 42c6bed7cSAlexander Aring */ 52c6bed7cSAlexander Aring 63fd0202aSDavid S. Miller /* Based on patches from Jon Smirl <jonsmirl@gmail.com> 72c6bed7cSAlexander Aring * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com> 82c6bed7cSAlexander Aring * 92c6bed7cSAlexander Aring * This program is free software; you can redistribute it and/or modify 102c6bed7cSAlexander Aring * it under the terms of the GNU General Public License version 2 112c6bed7cSAlexander Aring * as published by the Free Software Foundation. 122c6bed7cSAlexander Aring * 132c6bed7cSAlexander Aring * This program is distributed in the hope that it will be useful, 142c6bed7cSAlexander Aring * but WITHOUT ANY WARRANTY; without even the implied warranty of 152c6bed7cSAlexander Aring * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 162c6bed7cSAlexander Aring * GNU General Public License for more details. 172c6bed7cSAlexander Aring * 182c6bed7cSAlexander Aring */ 192c6bed7cSAlexander Aring 202c6bed7cSAlexander Aring /* Jon's code is based on 6lowpan implementation for Contiki which is: 212c6bed7cSAlexander Aring * Copyright (c) 2008, Swedish Institute of Computer Science. 222c6bed7cSAlexander Aring * All rights reserved. 232c6bed7cSAlexander Aring * 242c6bed7cSAlexander Aring * Redistribution and use in source and binary forms, with or without 252c6bed7cSAlexander Aring * modification, are permitted provided that the following conditions 262c6bed7cSAlexander Aring * are met: 272c6bed7cSAlexander Aring * 1. Redistributions of source code must retain the above copyright 282c6bed7cSAlexander Aring * notice, this list of conditions and the following disclaimer. 292c6bed7cSAlexander Aring * 2. Redistributions in binary form must reproduce the above copyright 302c6bed7cSAlexander Aring * notice, this list of conditions and the following disclaimer in the 312c6bed7cSAlexander Aring * documentation and/or other materials provided with the distribution. 322c6bed7cSAlexander Aring * 3. Neither the name of the Institute nor the names of its contributors 332c6bed7cSAlexander Aring * may be used to endorse or promote products derived from this software 342c6bed7cSAlexander Aring * without specific prior written permission. 352c6bed7cSAlexander Aring * 362c6bed7cSAlexander Aring * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND 372c6bed7cSAlexander Aring * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 382c6bed7cSAlexander Aring * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 392c6bed7cSAlexander Aring * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE 402c6bed7cSAlexander Aring * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 412c6bed7cSAlexander Aring * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 422c6bed7cSAlexander Aring * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 432c6bed7cSAlexander Aring * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 442c6bed7cSAlexander Aring * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 452c6bed7cSAlexander Aring * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 462c6bed7cSAlexander Aring * SUCH DAMAGE. 472c6bed7cSAlexander Aring */ 482c6bed7cSAlexander Aring 492c6bed7cSAlexander Aring #include <linux/bitops.h> 502c6bed7cSAlexander Aring #include <linux/if_arp.h> 512c6bed7cSAlexander Aring #include <linux/module.h> 522c6bed7cSAlexander Aring #include <linux/netdevice.h> 532c6bed7cSAlexander Aring #include <net/6lowpan.h> 542c6bed7cSAlexander Aring #include <net/ipv6.h> 552c6bed7cSAlexander Aring #include <net/af_ieee802154.h> 562c6bed7cSAlexander Aring 57cc6ed268SAlexander Aring #include "nhc.h" 58cc6ed268SAlexander Aring 593fd0202aSDavid S. Miller /* Uncompress address function for source and 602c6bed7cSAlexander Aring * destination address(non-multicast). 612c6bed7cSAlexander Aring * 622c6bed7cSAlexander Aring * address_mode is sam value or dam value. 632c6bed7cSAlexander Aring */ 642c6bed7cSAlexander Aring static int uncompress_addr(struct sk_buff *skb, 652c6bed7cSAlexander Aring struct in6_addr *ipaddr, const u8 address_mode, 662c6bed7cSAlexander Aring const u8 *lladdr, const u8 addr_type, 672c6bed7cSAlexander Aring const u8 addr_len) 682c6bed7cSAlexander Aring { 692c6bed7cSAlexander Aring bool fail; 702c6bed7cSAlexander Aring 712c6bed7cSAlexander Aring switch (address_mode) { 722c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_00: 732c6bed7cSAlexander Aring /* for global link addresses */ 742c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 752c6bed7cSAlexander Aring break; 762c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_01: 772c6bed7cSAlexander Aring /* fe:80::XXXX:XXXX:XXXX:XXXX */ 782c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 792c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 802c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); 812c6bed7cSAlexander Aring break; 822c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_02: 832c6bed7cSAlexander Aring /* fe:80::ff:fe00:XXXX */ 842c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 852c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 862c6bed7cSAlexander Aring ipaddr->s6_addr[11] = 0xFF; 872c6bed7cSAlexander Aring ipaddr->s6_addr[12] = 0xFE; 882c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); 892c6bed7cSAlexander Aring break; 902c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_03: 912c6bed7cSAlexander Aring fail = false; 922c6bed7cSAlexander Aring switch (addr_type) { 932c6bed7cSAlexander Aring case IEEE802154_ADDR_LONG: 942c6bed7cSAlexander Aring /* fe:80::XXXX:XXXX:XXXX:XXXX 952c6bed7cSAlexander Aring * \_________________/ 962c6bed7cSAlexander Aring * hwaddr 972c6bed7cSAlexander Aring */ 982c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 992c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 1002c6bed7cSAlexander Aring memcpy(&ipaddr->s6_addr[8], lladdr, addr_len); 1012c6bed7cSAlexander Aring /* second bit-flip (Universe/Local) 1022c6bed7cSAlexander Aring * is done according RFC2464 1032c6bed7cSAlexander Aring */ 1042c6bed7cSAlexander Aring ipaddr->s6_addr[8] ^= 0x02; 1052c6bed7cSAlexander Aring break; 1062c6bed7cSAlexander Aring case IEEE802154_ADDR_SHORT: 1072c6bed7cSAlexander Aring /* fe:80::ff:fe00:XXXX 1082c6bed7cSAlexander Aring * \__/ 1092c6bed7cSAlexander Aring * short_addr 1102c6bed7cSAlexander Aring * 1112c6bed7cSAlexander Aring * Universe/Local bit is zero. 1122c6bed7cSAlexander Aring */ 1132c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 1142c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 1152c6bed7cSAlexander Aring ipaddr->s6_addr[11] = 0xFF; 1162c6bed7cSAlexander Aring ipaddr->s6_addr[12] = 0xFE; 1172c6bed7cSAlexander Aring ipaddr->s6_addr16[7] = htons(*((u16 *)lladdr)); 1182c6bed7cSAlexander Aring break; 1192c6bed7cSAlexander Aring default: 1202c6bed7cSAlexander Aring pr_debug("Invalid addr_type set\n"); 1212c6bed7cSAlexander Aring return -EINVAL; 1222c6bed7cSAlexander Aring } 1232c6bed7cSAlexander Aring break; 1242c6bed7cSAlexander Aring default: 1252c6bed7cSAlexander Aring pr_debug("Invalid address mode value: 0x%x\n", address_mode); 1262c6bed7cSAlexander Aring return -EINVAL; 1272c6bed7cSAlexander Aring } 1282c6bed7cSAlexander Aring 1292c6bed7cSAlexander Aring if (fail) { 1302c6bed7cSAlexander Aring pr_debug("Failed to fetch skb data\n"); 1312c6bed7cSAlexander Aring return -EIO; 1322c6bed7cSAlexander Aring } 1332c6bed7cSAlexander Aring 1342c6bed7cSAlexander Aring raw_dump_inline(NULL, "Reconstructed ipv6 addr is", 1352c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 1362c6bed7cSAlexander Aring 1372c6bed7cSAlexander Aring return 0; 1382c6bed7cSAlexander Aring } 1392c6bed7cSAlexander Aring 1403fd0202aSDavid S. Miller /* Uncompress address function for source context 1412c6bed7cSAlexander Aring * based address(non-multicast). 1422c6bed7cSAlexander Aring */ 1432c6bed7cSAlexander Aring static int uncompress_context_based_src_addr(struct sk_buff *skb, 1442c6bed7cSAlexander Aring struct in6_addr *ipaddr, 1452c6bed7cSAlexander Aring const u8 sam) 1462c6bed7cSAlexander Aring { 1472c6bed7cSAlexander Aring switch (sam) { 1482c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_00: 1492c6bed7cSAlexander Aring /* unspec address :: 1502c6bed7cSAlexander Aring * Do nothing, address is already :: 1512c6bed7cSAlexander Aring */ 1522c6bed7cSAlexander Aring break; 1532c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_01: 1542c6bed7cSAlexander Aring /* TODO */ 1552c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_02: 1562c6bed7cSAlexander Aring /* TODO */ 1572c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_03: 1582c6bed7cSAlexander Aring /* TODO */ 1592c6bed7cSAlexander Aring netdev_warn(skb->dev, "SAM value 0x%x not supported\n", sam); 1602c6bed7cSAlexander Aring return -EINVAL; 1612c6bed7cSAlexander Aring default: 1622c6bed7cSAlexander Aring pr_debug("Invalid sam value: 0x%x\n", sam); 1632c6bed7cSAlexander Aring return -EINVAL; 1642c6bed7cSAlexander Aring } 1652c6bed7cSAlexander Aring 1662c6bed7cSAlexander Aring raw_dump_inline(NULL, 1672c6bed7cSAlexander Aring "Reconstructed context based ipv6 src addr is", 1682c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 1692c6bed7cSAlexander Aring 1702c6bed7cSAlexander Aring return 0; 1712c6bed7cSAlexander Aring } 1722c6bed7cSAlexander Aring 1732c6bed7cSAlexander Aring /* Uncompress function for multicast destination address, 1742c6bed7cSAlexander Aring * when M bit is set. 1752c6bed7cSAlexander Aring */ 1767fc4cfdaSMarcel Holtmann static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb, 1772c6bed7cSAlexander Aring struct in6_addr *ipaddr, 1782c6bed7cSAlexander Aring const u8 dam) 1792c6bed7cSAlexander Aring { 1802c6bed7cSAlexander Aring bool fail; 1812c6bed7cSAlexander Aring 1822c6bed7cSAlexander Aring switch (dam) { 1832c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_00: 1842c6bed7cSAlexander Aring /* 00: 128 bits. The full address 1852c6bed7cSAlexander Aring * is carried in-line. 1862c6bed7cSAlexander Aring */ 1872c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 1882c6bed7cSAlexander Aring break; 1892c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_01: 1902c6bed7cSAlexander Aring /* 01: 48 bits. The address takes 1912c6bed7cSAlexander Aring * the form ffXX::00XX:XXXX:XXXX. 1922c6bed7cSAlexander Aring */ 1932c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 1942c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 1952c6bed7cSAlexander Aring fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); 1962c6bed7cSAlexander Aring break; 1972c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_10: 1982c6bed7cSAlexander Aring /* 10: 32 bits. The address takes 1992c6bed7cSAlexander Aring * the form ffXX::00XX:XXXX. 2002c6bed7cSAlexander Aring */ 2012c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 2022c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 2032c6bed7cSAlexander Aring fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); 2042c6bed7cSAlexander Aring break; 2052c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_11: 2062c6bed7cSAlexander Aring /* 11: 8 bits. The address takes 2072c6bed7cSAlexander Aring * the form ff02::00XX. 2082c6bed7cSAlexander Aring */ 2092c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 2102c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x02; 2112c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); 2122c6bed7cSAlexander Aring break; 2132c6bed7cSAlexander Aring default: 2142c6bed7cSAlexander Aring pr_debug("DAM value has a wrong value: 0x%x\n", dam); 2152c6bed7cSAlexander Aring return -EINVAL; 2162c6bed7cSAlexander Aring } 2172c6bed7cSAlexander Aring 2182c6bed7cSAlexander Aring if (fail) { 2192c6bed7cSAlexander Aring pr_debug("Failed to fetch skb data\n"); 2202c6bed7cSAlexander Aring return -EIO; 2212c6bed7cSAlexander Aring } 2222c6bed7cSAlexander Aring 2232c6bed7cSAlexander Aring raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is", 2242c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 2252c6bed7cSAlexander Aring 2262c6bed7cSAlexander Aring return 0; 2272c6bed7cSAlexander Aring } 2282c6bed7cSAlexander Aring 2292c6bed7cSAlexander Aring /* TTL uncompression values */ 2302c6bed7cSAlexander Aring static const u8 lowpan_ttl_values[] = { 0, 1, 64, 255 }; 2312c6bed7cSAlexander Aring 23201141234SMartin Townsend int 23301141234SMartin Townsend lowpan_header_decompress(struct sk_buff *skb, struct net_device *dev, 23401141234SMartin Townsend const u8 *saddr, const u8 saddr_type, 23501141234SMartin Townsend const u8 saddr_len, const u8 *daddr, 23601141234SMartin Townsend const u8 daddr_type, const u8 daddr_len, 237f8b36176SMartin Townsend u8 iphc0, u8 iphc1) 2382c6bed7cSAlexander Aring { 2392c6bed7cSAlexander Aring struct ipv6hdr hdr = {}; 2402c6bed7cSAlexander Aring u8 tmp, num_context = 0; 2412c6bed7cSAlexander Aring int err; 2422c6bed7cSAlexander Aring 2432c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb data dump uncompressed", 2442c6bed7cSAlexander Aring skb->data, skb->len); 2452c6bed7cSAlexander Aring 2462c6bed7cSAlexander Aring /* another if the CID flag is set */ 2472c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_CID) { 2482c6bed7cSAlexander Aring pr_debug("CID flag is set, increase header with one\n"); 2494ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &num_context, sizeof(num_context))) 25056b2c3eeSMartin Townsend return -EINVAL; 2512c6bed7cSAlexander Aring } 2522c6bed7cSAlexander Aring 2532c6bed7cSAlexander Aring hdr.version = 6; 2542c6bed7cSAlexander Aring 2552c6bed7cSAlexander Aring /* Traffic Class and Flow Label */ 2562c6bed7cSAlexander Aring switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) { 2573fd0202aSDavid S. Miller /* Traffic Class and FLow Label carried in-line 2582c6bed7cSAlexander Aring * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) 2592c6bed7cSAlexander Aring */ 2602c6bed7cSAlexander Aring case 0: /* 00b */ 2614ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &tmp, sizeof(tmp))) 26256b2c3eeSMartin Townsend return -EINVAL; 2632c6bed7cSAlexander Aring 2642c6bed7cSAlexander Aring memcpy(&hdr.flow_lbl, &skb->data[0], 3); 2652c6bed7cSAlexander Aring skb_pull(skb, 3); 2662c6bed7cSAlexander Aring hdr.priority = ((tmp >> 2) & 0x0f); 2672c6bed7cSAlexander Aring hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) | 2682c6bed7cSAlexander Aring (hdr.flow_lbl[0] & 0x0f); 2692c6bed7cSAlexander Aring break; 2703fd0202aSDavid S. Miller /* Traffic class carried in-line 2712c6bed7cSAlexander Aring * ECN + DSCP (1 byte), Flow Label is elided 2722c6bed7cSAlexander Aring */ 2732c6bed7cSAlexander Aring case 2: /* 10b */ 2744ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &tmp, sizeof(tmp))) 27556b2c3eeSMartin Townsend return -EINVAL; 2762c6bed7cSAlexander Aring 2772c6bed7cSAlexander Aring hdr.priority = ((tmp >> 2) & 0x0f); 2782c6bed7cSAlexander Aring hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30); 2792c6bed7cSAlexander Aring break; 2803fd0202aSDavid S. Miller /* Flow Label carried in-line 2812c6bed7cSAlexander Aring * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided 2822c6bed7cSAlexander Aring */ 2832c6bed7cSAlexander Aring case 1: /* 01b */ 2844ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &tmp, sizeof(tmp))) 28556b2c3eeSMartin Townsend return -EINVAL; 2862c6bed7cSAlexander Aring 2872c6bed7cSAlexander Aring hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30); 2882c6bed7cSAlexander Aring memcpy(&hdr.flow_lbl[1], &skb->data[0], 2); 2892c6bed7cSAlexander Aring skb_pull(skb, 2); 2902c6bed7cSAlexander Aring break; 2912c6bed7cSAlexander Aring /* Traffic Class and Flow Label are elided */ 2922c6bed7cSAlexander Aring case 3: /* 11b */ 2932c6bed7cSAlexander Aring break; 2942c6bed7cSAlexander Aring default: 2952c6bed7cSAlexander Aring break; 2962c6bed7cSAlexander Aring } 2972c6bed7cSAlexander Aring 2982c6bed7cSAlexander Aring /* Next Header */ 2992c6bed7cSAlexander Aring if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { 3002c6bed7cSAlexander Aring /* Next header is carried inline */ 3014ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr))) 30256b2c3eeSMartin Townsend return -EINVAL; 3032c6bed7cSAlexander Aring 3042c6bed7cSAlexander Aring pr_debug("NH flag is set, next header carried inline: %02x\n", 3052c6bed7cSAlexander Aring hdr.nexthdr); 3062c6bed7cSAlexander Aring } 3072c6bed7cSAlexander Aring 3082c6bed7cSAlexander Aring /* Hop Limit */ 3093fd0202aSDavid S. Miller if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I) { 3102c6bed7cSAlexander Aring hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03]; 3113fd0202aSDavid S. Miller } else { 3124ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &hdr.hop_limit, 3134ebc960fSAlexander Aring sizeof(hdr.hop_limit))) 31456b2c3eeSMartin Townsend return -EINVAL; 3152c6bed7cSAlexander Aring } 3162c6bed7cSAlexander Aring 3172c6bed7cSAlexander Aring /* Extract SAM to the tmp variable */ 3182c6bed7cSAlexander Aring tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03; 3192c6bed7cSAlexander Aring 3202c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_SAC) { 3212c6bed7cSAlexander Aring /* Source address context based uncompression */ 3222c6bed7cSAlexander Aring pr_debug("SAC bit is set. Handle context based source address.\n"); 3237fc4cfdaSMarcel Holtmann err = uncompress_context_based_src_addr(skb, &hdr.saddr, tmp); 3242c6bed7cSAlexander Aring } else { 3252c6bed7cSAlexander Aring /* Source address uncompression */ 3262c6bed7cSAlexander Aring pr_debug("source address stateless compression\n"); 3272c6bed7cSAlexander Aring err = uncompress_addr(skb, &hdr.saddr, tmp, saddr, 3282c6bed7cSAlexander Aring saddr_type, saddr_len); 3292c6bed7cSAlexander Aring } 3302c6bed7cSAlexander Aring 3312c6bed7cSAlexander Aring /* Check on error of previous branch */ 3322c6bed7cSAlexander Aring if (err) 33356b2c3eeSMartin Townsend return -EINVAL; 3342c6bed7cSAlexander Aring 3352c6bed7cSAlexander Aring /* Extract DAM to the tmp variable */ 3362c6bed7cSAlexander Aring tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03; 3372c6bed7cSAlexander Aring 3382c6bed7cSAlexander Aring /* check for Multicast Compression */ 3392c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_M) { 3402c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_DAC) { 3412c6bed7cSAlexander Aring pr_debug("dest: context-based mcast compression\n"); 3422c6bed7cSAlexander Aring /* TODO: implement this */ 3432c6bed7cSAlexander Aring } else { 3447fc4cfdaSMarcel Holtmann err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr, 3457fc4cfdaSMarcel Holtmann tmp); 3467fc4cfdaSMarcel Holtmann 3472c6bed7cSAlexander Aring if (err) 34856b2c3eeSMartin Townsend return -EINVAL; 3492c6bed7cSAlexander Aring } 3502c6bed7cSAlexander Aring } else { 3512c6bed7cSAlexander Aring err = uncompress_addr(skb, &hdr.daddr, tmp, daddr, 3522c6bed7cSAlexander Aring daddr_type, daddr_len); 3532c6bed7cSAlexander Aring pr_debug("dest: stateless compression mode %d dest %pI6c\n", 3542c6bed7cSAlexander Aring tmp, &hdr.daddr); 3552c6bed7cSAlexander Aring if (err) 35656b2c3eeSMartin Townsend return -EINVAL; 3572c6bed7cSAlexander Aring } 3582c6bed7cSAlexander Aring 359cc6ed268SAlexander Aring /* Next header data uncompression */ 3602c6bed7cSAlexander Aring if (iphc0 & LOWPAN_IPHC_NH_C) { 361cc6ed268SAlexander Aring err = lowpan_nhc_do_uncompression(skb, dev, &hdr); 362cc6ed268SAlexander Aring if (err < 0) 36311e3ff70SMartin Townsend return err; 36411e3ff70SMartin Townsend } else { 36511e3ff70SMartin Townsend err = skb_cow(skb, sizeof(hdr)); 36656b2c3eeSMartin Townsend if (unlikely(err)) 36711e3ff70SMartin Townsend return err; 36811e3ff70SMartin Townsend } 3692c6bed7cSAlexander Aring 3702c6bed7cSAlexander Aring hdr.payload_len = htons(skb->len); 3712c6bed7cSAlexander Aring 3722c6bed7cSAlexander Aring pr_debug("skb headroom size = %d, data length = %d\n", 3732c6bed7cSAlexander Aring skb_headroom(skb), skb->len); 3742c6bed7cSAlexander Aring 3752c6bed7cSAlexander Aring pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t" 3762c6bed7cSAlexander Aring "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 3772c6bed7cSAlexander Aring hdr.version, ntohs(hdr.payload_len), hdr.nexthdr, 3782c6bed7cSAlexander Aring hdr.hop_limit, &hdr.daddr); 3792c6bed7cSAlexander Aring 380f8b36176SMartin Townsend skb_push(skb, sizeof(hdr)); 381f8b36176SMartin Townsend skb_reset_network_header(skb); 382f8b36176SMartin Townsend skb_copy_to_linear_data(skb, &hdr, sizeof(hdr)); 383f8b36176SMartin Townsend 3847fc4cfdaSMarcel Holtmann raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, sizeof(hdr)); 3852c6bed7cSAlexander Aring 386f8b36176SMartin Townsend return 0; 3872c6bed7cSAlexander Aring } 38801141234SMartin Townsend EXPORT_SYMBOL_GPL(lowpan_header_decompress); 3892c6bed7cSAlexander Aring 39084ca5e03SAlexander Aring static u8 lowpan_compress_addr_64(u8 **hc_ptr, u8 shift, 3912c6bed7cSAlexander Aring const struct in6_addr *ipaddr, 3922c6bed7cSAlexander Aring const unsigned char *lladdr) 3932c6bed7cSAlexander Aring { 3942c6bed7cSAlexander Aring u8 val = 0; 3952c6bed7cSAlexander Aring 3962c6bed7cSAlexander Aring if (is_addr_mac_addr_based(ipaddr, lladdr)) { 3972c6bed7cSAlexander Aring val = 3; /* 0-bits */ 3982c6bed7cSAlexander Aring pr_debug("address compression 0 bits\n"); 3992c6bed7cSAlexander Aring } else if (lowpan_is_iid_16_bit_compressable(ipaddr)) { 4002c6bed7cSAlexander Aring /* compress IID to 16 bits xxxx::XXXX */ 40185c71240SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2); 4022c6bed7cSAlexander Aring val = 2; /* 16-bits */ 4032c6bed7cSAlexander Aring raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)", 40484ca5e03SAlexander Aring *hc_ptr - 2, 2); 4052c6bed7cSAlexander Aring } else { 4062c6bed7cSAlexander Aring /* do not compress IID => xxxx::IID */ 40785c71240SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8); 4082c6bed7cSAlexander Aring val = 1; /* 64-bits */ 4092c6bed7cSAlexander Aring raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)", 41084ca5e03SAlexander Aring *hc_ptr - 8, 8); 4112c6bed7cSAlexander Aring } 4122c6bed7cSAlexander Aring 4132c6bed7cSAlexander Aring return rol8(val, shift); 4142c6bed7cSAlexander Aring } 4152c6bed7cSAlexander Aring 4162c6bed7cSAlexander Aring int lowpan_header_compress(struct sk_buff *skb, struct net_device *dev, 4172c6bed7cSAlexander Aring unsigned short type, const void *_daddr, 4182c6bed7cSAlexander Aring const void *_saddr, unsigned int len) 4192c6bed7cSAlexander Aring { 42084ca5e03SAlexander Aring u8 tmp, iphc0, iphc1, *hc_ptr; 4212c6bed7cSAlexander Aring struct ipv6hdr *hdr; 4222c6bed7cSAlexander Aring u8 head[100] = {}; 423cc6ed268SAlexander Aring int ret, addr_type; 4242c6bed7cSAlexander Aring 4252c6bed7cSAlexander Aring if (type != ETH_P_IPV6) 4262c6bed7cSAlexander Aring return -EINVAL; 4272c6bed7cSAlexander Aring 4282c6bed7cSAlexander Aring hdr = ipv6_hdr(skb); 42984ca5e03SAlexander Aring hc_ptr = head + 2; 4302c6bed7cSAlexander Aring 4312c6bed7cSAlexander Aring pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" 4322c6bed7cSAlexander Aring "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 4332c6bed7cSAlexander Aring hdr->version, ntohs(hdr->payload_len), hdr->nexthdr, 4342c6bed7cSAlexander Aring hdr->hop_limit, &hdr->daddr); 4352c6bed7cSAlexander Aring 4362c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb network header dump", 4372c6bed7cSAlexander Aring skb_network_header(skb), sizeof(struct ipv6hdr)); 4382c6bed7cSAlexander Aring 4393fd0202aSDavid S. Miller /* As we copy some bit-length fields, in the IPHC encoding bytes, 4402c6bed7cSAlexander Aring * we sometimes use |= 4412c6bed7cSAlexander Aring * If the field is 0, and the current bit value in memory is 1, 4422c6bed7cSAlexander Aring * this does not work. We therefore reset the IPHC encoding here 4432c6bed7cSAlexander Aring */ 4442c6bed7cSAlexander Aring iphc0 = LOWPAN_DISPATCH_IPHC; 4452c6bed7cSAlexander Aring iphc1 = 0; 4462c6bed7cSAlexander Aring 4472c6bed7cSAlexander Aring /* TODO: context lookup */ 4482c6bed7cSAlexander Aring 4492c6bed7cSAlexander Aring raw_dump_inline(__func__, "saddr", 4502c6bed7cSAlexander Aring (unsigned char *)_saddr, IEEE802154_ADDR_LEN); 4512c6bed7cSAlexander Aring raw_dump_inline(__func__, "daddr", 4522c6bed7cSAlexander Aring (unsigned char *)_daddr, IEEE802154_ADDR_LEN); 4532c6bed7cSAlexander Aring 4547fc4cfdaSMarcel Holtmann raw_dump_table(__func__, "sending raw skb network uncompressed packet", 4552c6bed7cSAlexander Aring skb->data, skb->len); 4562c6bed7cSAlexander Aring 4573fd0202aSDavid S. Miller /* Traffic class, flow label 4582c6bed7cSAlexander Aring * If flow label is 0, compress it. If traffic class is 0, compress it 4592c6bed7cSAlexander Aring * We have to process both in the same time as the offset of traffic 4602c6bed7cSAlexander Aring * class depends on the presence of version and flow label 4612c6bed7cSAlexander Aring */ 4622c6bed7cSAlexander Aring 46384ca5e03SAlexander Aring /* hc format of TC is ECN | DSCP , original one is DSCP | ECN */ 4642c6bed7cSAlexander Aring tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4); 4652c6bed7cSAlexander Aring tmp = ((tmp & 0x03) << 6) | (tmp >> 2); 4662c6bed7cSAlexander Aring 4672c6bed7cSAlexander Aring if (((hdr->flow_lbl[0] & 0x0F) == 0) && 4682c6bed7cSAlexander Aring (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) { 4692c6bed7cSAlexander Aring /* flow label can be compressed */ 4702c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_FL_C; 4712c6bed7cSAlexander Aring if ((hdr->priority == 0) && 4722c6bed7cSAlexander Aring ((hdr->flow_lbl[0] & 0xF0) == 0)) { 4732c6bed7cSAlexander Aring /* compress (elide) all */ 4742c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TC_C; 4752c6bed7cSAlexander Aring } else { 4762c6bed7cSAlexander Aring /* compress only the flow label */ 47784ca5e03SAlexander Aring *hc_ptr = tmp; 47884ca5e03SAlexander Aring hc_ptr += 1; 4792c6bed7cSAlexander Aring } 4802c6bed7cSAlexander Aring } else { 4812c6bed7cSAlexander Aring /* Flow label cannot be compressed */ 4822c6bed7cSAlexander Aring if ((hdr->priority == 0) && 4832c6bed7cSAlexander Aring ((hdr->flow_lbl[0] & 0xF0) == 0)) { 4842c6bed7cSAlexander Aring /* compress only traffic class */ 4852c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TC_C; 48684ca5e03SAlexander Aring *hc_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F); 48784ca5e03SAlexander Aring memcpy(hc_ptr + 1, &hdr->flow_lbl[1], 2); 48884ca5e03SAlexander Aring hc_ptr += 3; 4892c6bed7cSAlexander Aring } else { 4902c6bed7cSAlexander Aring /* compress nothing */ 49184ca5e03SAlexander Aring memcpy(hc_ptr, hdr, 4); 4922c6bed7cSAlexander Aring /* replace the top byte with new ECN | DSCP format */ 49384ca5e03SAlexander Aring *hc_ptr = tmp; 49484ca5e03SAlexander Aring hc_ptr += 4; 4952c6bed7cSAlexander Aring } 4962c6bed7cSAlexander Aring } 4972c6bed7cSAlexander Aring 4982c6bed7cSAlexander Aring /* NOTE: payload length is always compressed */ 4992c6bed7cSAlexander Aring 500cc6ed268SAlexander Aring /* Check if we provide the nhc format for nexthdr and compression 501cc6ed268SAlexander Aring * functionality. If not nexthdr is handled inline and not compressed. 502cc6ed268SAlexander Aring */ 503cc6ed268SAlexander Aring ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr, &iphc0); 504cc6ed268SAlexander Aring if (ret < 0) 505cc6ed268SAlexander Aring return ret; 5062c6bed7cSAlexander Aring 5073fd0202aSDavid S. Miller /* Hop limit 5082c6bed7cSAlexander Aring * if 1: compress, encoding is 01 5092c6bed7cSAlexander Aring * if 64: compress, encoding is 10 5102c6bed7cSAlexander Aring * if 255: compress, encoding is 11 5112c6bed7cSAlexander Aring * else do not compress 5122c6bed7cSAlexander Aring */ 5132c6bed7cSAlexander Aring switch (hdr->hop_limit) { 5142c6bed7cSAlexander Aring case 1: 5152c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TTL_1; 5162c6bed7cSAlexander Aring break; 5172c6bed7cSAlexander Aring case 64: 5182c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TTL_64; 5192c6bed7cSAlexander Aring break; 5202c6bed7cSAlexander Aring case 255: 5212c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TTL_255; 5222c6bed7cSAlexander Aring break; 5232c6bed7cSAlexander Aring default: 52485c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit, 52585c71240SAlexander Aring sizeof(hdr->hop_limit)); 5262c6bed7cSAlexander Aring } 5272c6bed7cSAlexander Aring 528556a5bfcSAlexander Aring addr_type = ipv6_addr_type(&hdr->saddr); 5292c6bed7cSAlexander Aring /* source address compression */ 530556a5bfcSAlexander Aring if (addr_type == IPV6_ADDR_ANY) { 5312c6bed7cSAlexander Aring pr_debug("source address is unspecified, setting SAC\n"); 5322c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_SAC; 533556a5bfcSAlexander Aring } else { 534556a5bfcSAlexander Aring if (addr_type & IPV6_ADDR_LINKLOCAL) { 53584ca5e03SAlexander Aring iphc1 |= lowpan_compress_addr_64(&hc_ptr, 536556a5bfcSAlexander Aring LOWPAN_IPHC_SAM_BIT, 537556a5bfcSAlexander Aring &hdr->saddr, _saddr); 538556a5bfcSAlexander Aring pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n", 539556a5bfcSAlexander Aring &hdr->saddr, iphc1); 5402c6bed7cSAlexander Aring } else { 5412c6bed7cSAlexander Aring pr_debug("send the full source address\n"); 542556a5bfcSAlexander Aring lowpan_push_hc_data(&hc_ptr, hdr->saddr.s6_addr, 16); 543556a5bfcSAlexander Aring } 5442c6bed7cSAlexander Aring } 5452c6bed7cSAlexander Aring 546556a5bfcSAlexander Aring addr_type = ipv6_addr_type(&hdr->daddr); 5472c6bed7cSAlexander Aring /* destination address compression */ 548556a5bfcSAlexander Aring if (addr_type & IPV6_ADDR_MULTICAST) { 5492c6bed7cSAlexander Aring pr_debug("destination address is multicast: "); 5502c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_M; 5512c6bed7cSAlexander Aring if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) { 5522c6bed7cSAlexander Aring pr_debug("compressed to 1 octet\n"); 5532c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_11; 5542c6bed7cSAlexander Aring /* use last byte */ 55585c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 55685c71240SAlexander Aring &hdr->daddr.s6_addr[15], 1); 5572c6bed7cSAlexander Aring } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) { 5582c6bed7cSAlexander Aring pr_debug("compressed to 4 octets\n"); 5592c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_10; 5602c6bed7cSAlexander Aring /* second byte + the last three */ 56185c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 56285c71240SAlexander Aring &hdr->daddr.s6_addr[1], 1); 56385c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 56485c71240SAlexander Aring &hdr->daddr.s6_addr[13], 3); 5652c6bed7cSAlexander Aring } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) { 5662c6bed7cSAlexander Aring pr_debug("compressed to 6 octets\n"); 5672c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_01; 5682c6bed7cSAlexander Aring /* second byte + the last five */ 56985c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 57085c71240SAlexander Aring &hdr->daddr.s6_addr[1], 1); 57185c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 57285c71240SAlexander Aring &hdr->daddr.s6_addr[11], 5); 5732c6bed7cSAlexander Aring } else { 5742c6bed7cSAlexander Aring pr_debug("using full address\n"); 5752c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_00; 57685c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, hdr->daddr.s6_addr, 16); 5772c6bed7cSAlexander Aring } 5782c6bed7cSAlexander Aring } else { 579556a5bfcSAlexander Aring if (addr_type & IPV6_ADDR_LINKLOCAL) { 5802c6bed7cSAlexander Aring /* TODO: context lookup */ 58184ca5e03SAlexander Aring iphc1 |= lowpan_compress_addr_64(&hc_ptr, 5822c6bed7cSAlexander Aring LOWPAN_IPHC_DAM_BIT, &hdr->daddr, _daddr); 5832c6bed7cSAlexander Aring pr_debug("dest address unicast link-local %pI6c " 5842c6bed7cSAlexander Aring "iphc1 0x%02x\n", &hdr->daddr, iphc1); 5852c6bed7cSAlexander Aring } else { 5862c6bed7cSAlexander Aring pr_debug("dest address unicast %pI6c\n", &hdr->daddr); 58785c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, hdr->daddr.s6_addr, 16); 5882c6bed7cSAlexander Aring } 5892c6bed7cSAlexander Aring } 5902c6bed7cSAlexander Aring 591cc6ed268SAlexander Aring /* next header compression */ 592cc6ed268SAlexander Aring if (iphc0 & LOWPAN_IPHC_NH_C) { 593cc6ed268SAlexander Aring ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr); 594cc6ed268SAlexander Aring if (ret < 0) 595cc6ed268SAlexander Aring return ret; 596cc6ed268SAlexander Aring } 5972c6bed7cSAlexander Aring 5982c6bed7cSAlexander Aring head[0] = iphc0; 5992c6bed7cSAlexander Aring head[1] = iphc1; 6002c6bed7cSAlexander Aring 6012c6bed7cSAlexander Aring skb_pull(skb, sizeof(struct ipv6hdr)); 6022c6bed7cSAlexander Aring skb_reset_transport_header(skb); 60384ca5e03SAlexander Aring memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head); 6042c6bed7cSAlexander Aring skb_reset_network_header(skb); 6052c6bed7cSAlexander Aring 60684ca5e03SAlexander Aring pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len); 6072c6bed7cSAlexander Aring 6082c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb data dump compressed", 6092c6bed7cSAlexander Aring skb->data, skb->len); 6102c6bed7cSAlexander Aring return 0; 6112c6bed7cSAlexander Aring } 6122c6bed7cSAlexander Aring EXPORT_SYMBOL_GPL(lowpan_header_compress); 6132c6bed7cSAlexander Aring 614cc6ed268SAlexander Aring static int __init lowpan_module_init(void) 615cc6ed268SAlexander Aring { 616cc6ed268SAlexander Aring request_module_nowait("nhc_udp"); 617cc6ed268SAlexander Aring 618cc6ed268SAlexander Aring return 0; 619cc6ed268SAlexander Aring } 620cc6ed268SAlexander Aring module_init(lowpan_module_init); 621cc6ed268SAlexander Aring 6222c6bed7cSAlexander Aring MODULE_LICENSE("GPL"); 623