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/netdevice.h> 528911d774SAlexander Aring 532c6bed7cSAlexander Aring #include <net/6lowpan.h> 542c6bed7cSAlexander Aring #include <net/ipv6.h> 558911d774SAlexander Aring 568911d774SAlexander Aring /* special link-layer handling */ 578911d774SAlexander Aring #include <net/mac802154.h> 582c6bed7cSAlexander Aring 59cc6ed268SAlexander Aring #include "nhc.h" 60cc6ed268SAlexander Aring 618911d774SAlexander Aring static inline void iphc_uncompress_eui64_lladdr(struct in6_addr *ipaddr, 628911d774SAlexander Aring const void *lladdr) 638911d774SAlexander Aring { 648911d774SAlexander Aring /* fe:80::XXXX:XXXX:XXXX:XXXX 658911d774SAlexander Aring * \_________________/ 668911d774SAlexander Aring * hwaddr 678911d774SAlexander Aring */ 688911d774SAlexander Aring ipaddr->s6_addr[0] = 0xFE; 698911d774SAlexander Aring ipaddr->s6_addr[1] = 0x80; 708911d774SAlexander Aring memcpy(&ipaddr->s6_addr[8], lladdr, EUI64_ADDR_LEN); 718911d774SAlexander Aring /* second bit-flip (Universe/Local) 728911d774SAlexander Aring * is done according RFC2464 738911d774SAlexander Aring */ 748911d774SAlexander Aring ipaddr->s6_addr[8] ^= 0x02; 758911d774SAlexander Aring } 768911d774SAlexander Aring 778911d774SAlexander Aring static inline void iphc_uncompress_802154_lladdr(struct in6_addr *ipaddr, 788911d774SAlexander Aring const void *lladdr) 798911d774SAlexander Aring { 808911d774SAlexander Aring const struct ieee802154_addr *addr = lladdr; 818911d774SAlexander Aring u8 eui64[EUI64_ADDR_LEN] = { }; 828911d774SAlexander Aring 838911d774SAlexander Aring switch (addr->mode) { 848911d774SAlexander Aring case IEEE802154_ADDR_LONG: 858911d774SAlexander Aring ieee802154_le64_to_be64(eui64, &addr->extended_addr); 868911d774SAlexander Aring iphc_uncompress_eui64_lladdr(ipaddr, eui64); 878911d774SAlexander Aring break; 888911d774SAlexander Aring case IEEE802154_ADDR_SHORT: 898911d774SAlexander Aring /* fe:80::ff:fe00:XXXX 908911d774SAlexander Aring * \__/ 918911d774SAlexander Aring * short_addr 928911d774SAlexander Aring * 938911d774SAlexander Aring * Universe/Local bit is zero. 948911d774SAlexander Aring */ 958911d774SAlexander Aring ipaddr->s6_addr[0] = 0xFE; 968911d774SAlexander Aring ipaddr->s6_addr[1] = 0x80; 978911d774SAlexander Aring ipaddr->s6_addr[11] = 0xFF; 988911d774SAlexander Aring ipaddr->s6_addr[12] = 0xFE; 998911d774SAlexander Aring ieee802154_le16_to_be16(&ipaddr->s6_addr16[7], 1008911d774SAlexander Aring &addr->short_addr); 1018911d774SAlexander Aring break; 1028911d774SAlexander Aring default: 1038911d774SAlexander Aring /* should never handled and filtered by 802154 6lowpan */ 1048911d774SAlexander Aring WARN_ON_ONCE(1); 1058911d774SAlexander Aring break; 1068911d774SAlexander Aring } 1078911d774SAlexander Aring } 1088911d774SAlexander Aring 1093fd0202aSDavid S. Miller /* Uncompress address function for source and 1102c6bed7cSAlexander Aring * destination address(non-multicast). 1112c6bed7cSAlexander Aring * 1122c6bed7cSAlexander Aring * address_mode is sam value or dam value. 1132c6bed7cSAlexander Aring */ 1148911d774SAlexander Aring static int uncompress_addr(struct sk_buff *skb, const struct net_device *dev, 1158911d774SAlexander Aring struct in6_addr *ipaddr, u8 address_mode, 1168911d774SAlexander Aring const void *lladdr) 1172c6bed7cSAlexander Aring { 1182c6bed7cSAlexander Aring bool fail; 1192c6bed7cSAlexander Aring 1202c6bed7cSAlexander Aring switch (address_mode) { 1212c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_00: 1222c6bed7cSAlexander Aring /* for global link addresses */ 1232c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 1242c6bed7cSAlexander Aring break; 1252c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_01: 1262c6bed7cSAlexander Aring /* fe:80::XXXX:XXXX:XXXX:XXXX */ 1272c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 1282c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 1292c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); 1302c6bed7cSAlexander Aring break; 1312c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_02: 1322c6bed7cSAlexander Aring /* fe:80::ff:fe00:XXXX */ 1332c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 1342c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 1352c6bed7cSAlexander Aring ipaddr->s6_addr[11] = 0xFF; 1362c6bed7cSAlexander Aring ipaddr->s6_addr[12] = 0xFE; 1372c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); 1382c6bed7cSAlexander Aring break; 1392c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_03: 1402c6bed7cSAlexander Aring fail = false; 1418911d774SAlexander Aring switch (lowpan_priv(dev)->lltype) { 1428911d774SAlexander Aring case LOWPAN_LLTYPE_IEEE802154: 1438911d774SAlexander Aring iphc_uncompress_802154_lladdr(ipaddr, lladdr); 1442c6bed7cSAlexander Aring break; 1452c6bed7cSAlexander Aring default: 1468911d774SAlexander Aring iphc_uncompress_eui64_lladdr(ipaddr, lladdr); 1478911d774SAlexander Aring break; 1482c6bed7cSAlexander Aring } 1492c6bed7cSAlexander Aring break; 1502c6bed7cSAlexander Aring default: 1512c6bed7cSAlexander Aring pr_debug("Invalid address mode value: 0x%x\n", address_mode); 1522c6bed7cSAlexander Aring return -EINVAL; 1532c6bed7cSAlexander Aring } 1542c6bed7cSAlexander Aring 1552c6bed7cSAlexander Aring if (fail) { 1562c6bed7cSAlexander Aring pr_debug("Failed to fetch skb data\n"); 1572c6bed7cSAlexander Aring return -EIO; 1582c6bed7cSAlexander Aring } 1592c6bed7cSAlexander Aring 1602c6bed7cSAlexander Aring raw_dump_inline(NULL, "Reconstructed ipv6 addr is", 1612c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 1622c6bed7cSAlexander Aring 1632c6bed7cSAlexander Aring return 0; 1642c6bed7cSAlexander Aring } 1652c6bed7cSAlexander Aring 1663fd0202aSDavid S. Miller /* Uncompress address function for source context 1672c6bed7cSAlexander Aring * based address(non-multicast). 1682c6bed7cSAlexander Aring */ 1692c6bed7cSAlexander Aring static int uncompress_context_based_src_addr(struct sk_buff *skb, 1702c6bed7cSAlexander Aring struct in6_addr *ipaddr, 1712c6bed7cSAlexander Aring const u8 sam) 1722c6bed7cSAlexander Aring { 1732c6bed7cSAlexander Aring switch (sam) { 1742c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_00: 1752c6bed7cSAlexander Aring /* unspec address :: 1762c6bed7cSAlexander Aring * Do nothing, address is already :: 1772c6bed7cSAlexander Aring */ 1782c6bed7cSAlexander Aring break; 1792c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_01: 1802c6bed7cSAlexander Aring /* TODO */ 1812c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_02: 1822c6bed7cSAlexander Aring /* TODO */ 1832c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_03: 1842c6bed7cSAlexander Aring /* TODO */ 1852c6bed7cSAlexander Aring netdev_warn(skb->dev, "SAM value 0x%x not supported\n", sam); 1862c6bed7cSAlexander Aring return -EINVAL; 1872c6bed7cSAlexander Aring default: 1882c6bed7cSAlexander Aring pr_debug("Invalid sam value: 0x%x\n", sam); 1892c6bed7cSAlexander Aring return -EINVAL; 1902c6bed7cSAlexander Aring } 1912c6bed7cSAlexander Aring 1922c6bed7cSAlexander Aring raw_dump_inline(NULL, 1932c6bed7cSAlexander Aring "Reconstructed context based ipv6 src addr is", 1942c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 1952c6bed7cSAlexander Aring 1962c6bed7cSAlexander Aring return 0; 1972c6bed7cSAlexander Aring } 1982c6bed7cSAlexander Aring 1992c6bed7cSAlexander Aring /* Uncompress function for multicast destination address, 2002c6bed7cSAlexander Aring * when M bit is set. 2012c6bed7cSAlexander Aring */ 2027fc4cfdaSMarcel Holtmann static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb, 2032c6bed7cSAlexander Aring struct in6_addr *ipaddr, 2042c6bed7cSAlexander Aring const u8 dam) 2052c6bed7cSAlexander Aring { 2062c6bed7cSAlexander Aring bool fail; 2072c6bed7cSAlexander Aring 2082c6bed7cSAlexander Aring switch (dam) { 2092c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_00: 2102c6bed7cSAlexander Aring /* 00: 128 bits. The full address 2112c6bed7cSAlexander Aring * is carried in-line. 2122c6bed7cSAlexander Aring */ 2132c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 2142c6bed7cSAlexander Aring break; 2152c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_01: 2162c6bed7cSAlexander Aring /* 01: 48 bits. The address takes 2172c6bed7cSAlexander Aring * the form ffXX::00XX:XXXX:XXXX. 2182c6bed7cSAlexander Aring */ 2192c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 2202c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 2212c6bed7cSAlexander Aring fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); 2222c6bed7cSAlexander Aring break; 2232c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_10: 2242c6bed7cSAlexander Aring /* 10: 32 bits. The address takes 2252c6bed7cSAlexander Aring * the form ffXX::00XX:XXXX. 2262c6bed7cSAlexander Aring */ 2272c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 2282c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 2292c6bed7cSAlexander Aring fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); 2302c6bed7cSAlexander Aring break; 2312c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_11: 2322c6bed7cSAlexander Aring /* 11: 8 bits. The address takes 2332c6bed7cSAlexander Aring * the form ff02::00XX. 2342c6bed7cSAlexander Aring */ 2352c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 2362c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x02; 2372c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); 2382c6bed7cSAlexander Aring break; 2392c6bed7cSAlexander Aring default: 2402c6bed7cSAlexander Aring pr_debug("DAM value has a wrong value: 0x%x\n", dam); 2412c6bed7cSAlexander Aring return -EINVAL; 2422c6bed7cSAlexander Aring } 2432c6bed7cSAlexander Aring 2442c6bed7cSAlexander Aring if (fail) { 2452c6bed7cSAlexander Aring pr_debug("Failed to fetch skb data\n"); 2462c6bed7cSAlexander Aring return -EIO; 2472c6bed7cSAlexander Aring } 2482c6bed7cSAlexander Aring 2492c6bed7cSAlexander Aring raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is", 2502c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 2512c6bed7cSAlexander Aring 2522c6bed7cSAlexander Aring return 0; 2532c6bed7cSAlexander Aring } 2542c6bed7cSAlexander Aring 2552c6bed7cSAlexander Aring /* TTL uncompression values */ 2562c6bed7cSAlexander Aring static const u8 lowpan_ttl_values[] = { 0, 1, 64, 255 }; 2572c6bed7cSAlexander Aring 2588911d774SAlexander Aring int lowpan_header_decompress(struct sk_buff *skb, const struct net_device *dev, 2598911d774SAlexander Aring const void *daddr, const void *saddr) 2602c6bed7cSAlexander Aring { 2612c6bed7cSAlexander Aring struct ipv6hdr hdr = {}; 2628911d774SAlexander Aring u8 iphc0, iphc1, tmp, num_context = 0; 2632c6bed7cSAlexander Aring int err; 2642c6bed7cSAlexander Aring 2652c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb data dump uncompressed", 2662c6bed7cSAlexander Aring skb->data, skb->len); 2672c6bed7cSAlexander Aring 268478208e3SAlexander Aring if (lowpan_fetch_skb(skb, &iphc0, sizeof(iphc0)) || 269478208e3SAlexander Aring lowpan_fetch_skb(skb, &iphc1, sizeof(iphc1))) 2708911d774SAlexander Aring return -EINVAL; 2718911d774SAlexander Aring 2722c6bed7cSAlexander Aring /* another if the CID flag is set */ 2732c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_CID) { 2742c6bed7cSAlexander Aring pr_debug("CID flag is set, increase header with one\n"); 2754ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &num_context, sizeof(num_context))) 27656b2c3eeSMartin Townsend return -EINVAL; 2772c6bed7cSAlexander Aring } 2782c6bed7cSAlexander Aring 2792c6bed7cSAlexander Aring hdr.version = 6; 2802c6bed7cSAlexander Aring 2812c6bed7cSAlexander Aring /* Traffic Class and Flow Label */ 2822c6bed7cSAlexander Aring switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) { 2833fd0202aSDavid S. Miller /* Traffic Class and FLow Label carried in-line 2842c6bed7cSAlexander Aring * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) 2852c6bed7cSAlexander Aring */ 2862c6bed7cSAlexander Aring case 0: /* 00b */ 2874ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &tmp, sizeof(tmp))) 28856b2c3eeSMartin Townsend return -EINVAL; 2892c6bed7cSAlexander Aring 2902c6bed7cSAlexander Aring memcpy(&hdr.flow_lbl, &skb->data[0], 3); 2912c6bed7cSAlexander Aring skb_pull(skb, 3); 2922c6bed7cSAlexander Aring hdr.priority = ((tmp >> 2) & 0x0f); 2932c6bed7cSAlexander Aring hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) | 2942c6bed7cSAlexander Aring (hdr.flow_lbl[0] & 0x0f); 2952c6bed7cSAlexander Aring break; 2963fd0202aSDavid S. Miller /* Traffic class carried in-line 2972c6bed7cSAlexander Aring * ECN + DSCP (1 byte), Flow Label is elided 2982c6bed7cSAlexander Aring */ 2992c6bed7cSAlexander Aring case 2: /* 10b */ 3004ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &tmp, sizeof(tmp))) 30156b2c3eeSMartin Townsend return -EINVAL; 3022c6bed7cSAlexander Aring 3032c6bed7cSAlexander Aring hdr.priority = ((tmp >> 2) & 0x0f); 3042c6bed7cSAlexander Aring hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30); 3052c6bed7cSAlexander Aring break; 3063fd0202aSDavid S. Miller /* Flow Label carried in-line 3072c6bed7cSAlexander Aring * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided 3082c6bed7cSAlexander Aring */ 3092c6bed7cSAlexander Aring case 1: /* 01b */ 3104ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &tmp, sizeof(tmp))) 31156b2c3eeSMartin Townsend return -EINVAL; 3122c6bed7cSAlexander Aring 31377e867b5SLukasz Duda hdr.flow_lbl[0] = (tmp & 0x0F) | ((tmp >> 2) & 0x30); 3142c6bed7cSAlexander Aring memcpy(&hdr.flow_lbl[1], &skb->data[0], 2); 3152c6bed7cSAlexander Aring skb_pull(skb, 2); 3162c6bed7cSAlexander Aring break; 3172c6bed7cSAlexander Aring /* Traffic Class and Flow Label are elided */ 3182c6bed7cSAlexander Aring case 3: /* 11b */ 3192c6bed7cSAlexander Aring break; 3202c6bed7cSAlexander Aring default: 3212c6bed7cSAlexander Aring break; 3222c6bed7cSAlexander Aring } 3232c6bed7cSAlexander Aring 3242c6bed7cSAlexander Aring /* Next Header */ 3252c6bed7cSAlexander Aring if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { 3262c6bed7cSAlexander Aring /* Next header is carried inline */ 3274ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr))) 32856b2c3eeSMartin Townsend return -EINVAL; 3292c6bed7cSAlexander Aring 3302c6bed7cSAlexander Aring pr_debug("NH flag is set, next header carried inline: %02x\n", 3312c6bed7cSAlexander Aring hdr.nexthdr); 3322c6bed7cSAlexander Aring } 3332c6bed7cSAlexander Aring 3342c6bed7cSAlexander Aring /* Hop Limit */ 3353fd0202aSDavid S. Miller if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I) { 3362c6bed7cSAlexander Aring hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03]; 3373fd0202aSDavid S. Miller } else { 3384ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &hdr.hop_limit, 3394ebc960fSAlexander Aring sizeof(hdr.hop_limit))) 34056b2c3eeSMartin Townsend return -EINVAL; 3412c6bed7cSAlexander Aring } 3422c6bed7cSAlexander Aring 3432c6bed7cSAlexander Aring /* Extract SAM to the tmp variable */ 3442c6bed7cSAlexander Aring tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03; 3452c6bed7cSAlexander Aring 3462c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_SAC) { 3472c6bed7cSAlexander Aring /* Source address context based uncompression */ 3482c6bed7cSAlexander Aring pr_debug("SAC bit is set. Handle context based source address.\n"); 3497fc4cfdaSMarcel Holtmann err = uncompress_context_based_src_addr(skb, &hdr.saddr, tmp); 3502c6bed7cSAlexander Aring } else { 3512c6bed7cSAlexander Aring /* Source address uncompression */ 3522c6bed7cSAlexander Aring pr_debug("source address stateless compression\n"); 3538911d774SAlexander Aring err = uncompress_addr(skb, dev, &hdr.saddr, tmp, saddr); 3542c6bed7cSAlexander Aring } 3552c6bed7cSAlexander Aring 3562c6bed7cSAlexander Aring /* Check on error of previous branch */ 3572c6bed7cSAlexander Aring if (err) 35856b2c3eeSMartin Townsend return -EINVAL; 3592c6bed7cSAlexander Aring 3602c6bed7cSAlexander Aring /* Extract DAM to the tmp variable */ 3612c6bed7cSAlexander Aring tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03; 3622c6bed7cSAlexander Aring 3632c6bed7cSAlexander Aring /* check for Multicast Compression */ 3642c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_M) { 3652c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_DAC) { 3662c6bed7cSAlexander Aring pr_debug("dest: context-based mcast compression\n"); 3672c6bed7cSAlexander Aring /* TODO: implement this */ 3682c6bed7cSAlexander Aring } else { 3697fc4cfdaSMarcel Holtmann err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr, 3707fc4cfdaSMarcel Holtmann tmp); 3717fc4cfdaSMarcel Holtmann 3722c6bed7cSAlexander Aring if (err) 37356b2c3eeSMartin Townsend return -EINVAL; 3742c6bed7cSAlexander Aring } 3752c6bed7cSAlexander Aring } else { 3768911d774SAlexander Aring err = uncompress_addr(skb, dev, &hdr.daddr, tmp, daddr); 3772c6bed7cSAlexander Aring pr_debug("dest: stateless compression mode %d dest %pI6c\n", 3782c6bed7cSAlexander Aring tmp, &hdr.daddr); 3792c6bed7cSAlexander Aring if (err) 38056b2c3eeSMartin Townsend return -EINVAL; 3812c6bed7cSAlexander Aring } 3822c6bed7cSAlexander Aring 383cc6ed268SAlexander Aring /* Next header data uncompression */ 3842c6bed7cSAlexander Aring if (iphc0 & LOWPAN_IPHC_NH_C) { 385cc6ed268SAlexander Aring err = lowpan_nhc_do_uncompression(skb, dev, &hdr); 386cc6ed268SAlexander Aring if (err < 0) 38711e3ff70SMartin Townsend return err; 38811e3ff70SMartin Townsend } else { 38911e3ff70SMartin Townsend err = skb_cow(skb, sizeof(hdr)); 39056b2c3eeSMartin Townsend if (unlikely(err)) 39111e3ff70SMartin Townsend return err; 39211e3ff70SMartin Townsend } 3932c6bed7cSAlexander Aring 39472a5e6bbSAlexander Aring switch (lowpan_priv(dev)->lltype) { 39572a5e6bbSAlexander Aring case LOWPAN_LLTYPE_IEEE802154: 39672a5e6bbSAlexander Aring if (lowpan_802154_cb(skb)->d_size) 39772a5e6bbSAlexander Aring hdr.payload_len = htons(lowpan_802154_cb(skb)->d_size - 39872a5e6bbSAlexander Aring sizeof(struct ipv6hdr)); 39972a5e6bbSAlexander Aring else 4002c6bed7cSAlexander Aring hdr.payload_len = htons(skb->len); 40172a5e6bbSAlexander Aring break; 40272a5e6bbSAlexander Aring default: 40372a5e6bbSAlexander Aring hdr.payload_len = htons(skb->len); 40472a5e6bbSAlexander Aring break; 40572a5e6bbSAlexander Aring } 4062c6bed7cSAlexander Aring 4072c6bed7cSAlexander Aring pr_debug("skb headroom size = %d, data length = %d\n", 4082c6bed7cSAlexander Aring skb_headroom(skb), skb->len); 4092c6bed7cSAlexander Aring 4102c6bed7cSAlexander Aring pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t" 4112c6bed7cSAlexander Aring "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 4122c6bed7cSAlexander Aring hdr.version, ntohs(hdr.payload_len), hdr.nexthdr, 4132c6bed7cSAlexander Aring hdr.hop_limit, &hdr.daddr); 4142c6bed7cSAlexander Aring 415f8b36176SMartin Townsend skb_push(skb, sizeof(hdr)); 416f8b36176SMartin Townsend skb_reset_network_header(skb); 417f8b36176SMartin Townsend skb_copy_to_linear_data(skb, &hdr, sizeof(hdr)); 418f8b36176SMartin Townsend 4197fc4cfdaSMarcel Holtmann raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, sizeof(hdr)); 4202c6bed7cSAlexander Aring 421f8b36176SMartin Townsend return 0; 4222c6bed7cSAlexander Aring } 42301141234SMartin Townsend EXPORT_SYMBOL_GPL(lowpan_header_decompress); 4242c6bed7cSAlexander Aring 42584ca5e03SAlexander Aring static u8 lowpan_compress_addr_64(u8 **hc_ptr, u8 shift, 4262c6bed7cSAlexander Aring const struct in6_addr *ipaddr, 4272c6bed7cSAlexander Aring const unsigned char *lladdr) 4282c6bed7cSAlexander Aring { 4292c6bed7cSAlexander Aring u8 val = 0; 4302c6bed7cSAlexander Aring 4312c6bed7cSAlexander Aring if (is_addr_mac_addr_based(ipaddr, lladdr)) { 4322c6bed7cSAlexander Aring val = 3; /* 0-bits */ 4332c6bed7cSAlexander Aring pr_debug("address compression 0 bits\n"); 4342c6bed7cSAlexander Aring } else if (lowpan_is_iid_16_bit_compressable(ipaddr)) { 4352c6bed7cSAlexander Aring /* compress IID to 16 bits xxxx::XXXX */ 43685c71240SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2); 4372c6bed7cSAlexander Aring val = 2; /* 16-bits */ 4382c6bed7cSAlexander Aring raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)", 43984ca5e03SAlexander Aring *hc_ptr - 2, 2); 4402c6bed7cSAlexander Aring } else { 4412c6bed7cSAlexander Aring /* do not compress IID => xxxx::IID */ 44285c71240SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8); 4432c6bed7cSAlexander Aring val = 1; /* 64-bits */ 4442c6bed7cSAlexander Aring raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)", 44584ca5e03SAlexander Aring *hc_ptr - 8, 8); 4462c6bed7cSAlexander Aring } 4472c6bed7cSAlexander Aring 4482c6bed7cSAlexander Aring return rol8(val, shift); 4492c6bed7cSAlexander Aring } 4502c6bed7cSAlexander Aring 451a6f77389SAlexander Aring int lowpan_header_compress(struct sk_buff *skb, const struct net_device *dev, 452a6f77389SAlexander Aring const void *daddr, const void *saddr) 4532c6bed7cSAlexander Aring { 45484ca5e03SAlexander Aring u8 tmp, iphc0, iphc1, *hc_ptr; 4552c6bed7cSAlexander Aring struct ipv6hdr *hdr; 456bf513fd6SAlexander Aring u8 head[LOWPAN_IPHC_MAX_HC_BUF_LEN] = {}; 457cc6ed268SAlexander Aring int ret, addr_type; 4582c6bed7cSAlexander Aring 459a6f77389SAlexander Aring if (skb->protocol != htons(ETH_P_IPV6)) 4602c6bed7cSAlexander Aring return -EINVAL; 4612c6bed7cSAlexander Aring 4622c6bed7cSAlexander Aring hdr = ipv6_hdr(skb); 46384ca5e03SAlexander Aring hc_ptr = head + 2; 4642c6bed7cSAlexander Aring 4652c6bed7cSAlexander Aring pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" 4662c6bed7cSAlexander Aring "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 4672c6bed7cSAlexander Aring hdr->version, ntohs(hdr->payload_len), hdr->nexthdr, 4682c6bed7cSAlexander Aring hdr->hop_limit, &hdr->daddr); 4692c6bed7cSAlexander Aring 4702c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb network header dump", 4712c6bed7cSAlexander Aring skb_network_header(skb), sizeof(struct ipv6hdr)); 4722c6bed7cSAlexander Aring 4733fd0202aSDavid S. Miller /* As we copy some bit-length fields, in the IPHC encoding bytes, 4742c6bed7cSAlexander Aring * we sometimes use |= 4752c6bed7cSAlexander Aring * If the field is 0, and the current bit value in memory is 1, 4762c6bed7cSAlexander Aring * this does not work. We therefore reset the IPHC encoding here 4772c6bed7cSAlexander Aring */ 4782c6bed7cSAlexander Aring iphc0 = LOWPAN_DISPATCH_IPHC; 4792c6bed7cSAlexander Aring iphc1 = 0; 4802c6bed7cSAlexander Aring 4812c6bed7cSAlexander Aring /* TODO: context lookup */ 4822c6bed7cSAlexander Aring 483a6f77389SAlexander Aring raw_dump_inline(__func__, "saddr", saddr, EUI64_ADDR_LEN); 484a6f77389SAlexander Aring raw_dump_inline(__func__, "daddr", daddr, EUI64_ADDR_LEN); 4852c6bed7cSAlexander Aring 4867fc4cfdaSMarcel Holtmann raw_dump_table(__func__, "sending raw skb network uncompressed packet", 4872c6bed7cSAlexander Aring skb->data, skb->len); 4882c6bed7cSAlexander Aring 4893fd0202aSDavid S. Miller /* Traffic class, flow label 4902c6bed7cSAlexander Aring * If flow label is 0, compress it. If traffic class is 0, compress it 4912c6bed7cSAlexander Aring * We have to process both in the same time as the offset of traffic 4922c6bed7cSAlexander Aring * class depends on the presence of version and flow label 4932c6bed7cSAlexander Aring */ 4942c6bed7cSAlexander Aring 49584ca5e03SAlexander Aring /* hc format of TC is ECN | DSCP , original one is DSCP | ECN */ 4962c6bed7cSAlexander Aring tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4); 4972c6bed7cSAlexander Aring tmp = ((tmp & 0x03) << 6) | (tmp >> 2); 4982c6bed7cSAlexander Aring 4992c6bed7cSAlexander Aring if (((hdr->flow_lbl[0] & 0x0F) == 0) && 5002c6bed7cSAlexander Aring (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) { 5012c6bed7cSAlexander Aring /* flow label can be compressed */ 5022c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_FL_C; 5032c6bed7cSAlexander Aring if ((hdr->priority == 0) && 5042c6bed7cSAlexander Aring ((hdr->flow_lbl[0] & 0xF0) == 0)) { 5052c6bed7cSAlexander Aring /* compress (elide) all */ 5062c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TC_C; 5072c6bed7cSAlexander Aring } else { 5082c6bed7cSAlexander Aring /* compress only the flow label */ 50984ca5e03SAlexander Aring *hc_ptr = tmp; 51084ca5e03SAlexander Aring hc_ptr += 1; 5112c6bed7cSAlexander Aring } 5122c6bed7cSAlexander Aring } else { 5132c6bed7cSAlexander Aring /* Flow label cannot be compressed */ 5142c6bed7cSAlexander Aring if ((hdr->priority == 0) && 5152c6bed7cSAlexander Aring ((hdr->flow_lbl[0] & 0xF0) == 0)) { 5162c6bed7cSAlexander Aring /* compress only traffic class */ 5172c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TC_C; 51884ca5e03SAlexander Aring *hc_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F); 51984ca5e03SAlexander Aring memcpy(hc_ptr + 1, &hdr->flow_lbl[1], 2); 52084ca5e03SAlexander Aring hc_ptr += 3; 5212c6bed7cSAlexander Aring } else { 5222c6bed7cSAlexander Aring /* compress nothing */ 52384ca5e03SAlexander Aring memcpy(hc_ptr, hdr, 4); 5242c6bed7cSAlexander Aring /* replace the top byte with new ECN | DSCP format */ 52584ca5e03SAlexander Aring *hc_ptr = tmp; 52684ca5e03SAlexander Aring hc_ptr += 4; 5272c6bed7cSAlexander Aring } 5282c6bed7cSAlexander Aring } 5292c6bed7cSAlexander Aring 5302c6bed7cSAlexander Aring /* NOTE: payload length is always compressed */ 5312c6bed7cSAlexander Aring 532cc6ed268SAlexander Aring /* Check if we provide the nhc format for nexthdr and compression 533cc6ed268SAlexander Aring * functionality. If not nexthdr is handled inline and not compressed. 534cc6ed268SAlexander Aring */ 535607b0bd3SAlexander Aring ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr); 536607b0bd3SAlexander Aring if (ret == -ENOENT) 537607b0bd3SAlexander Aring lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr, 538607b0bd3SAlexander Aring sizeof(hdr->nexthdr)); 539607b0bd3SAlexander Aring else 540607b0bd3SAlexander Aring iphc0 |= LOWPAN_IPHC_NH_C; 5412c6bed7cSAlexander Aring 5423fd0202aSDavid S. Miller /* Hop limit 5432c6bed7cSAlexander Aring * if 1: compress, encoding is 01 5442c6bed7cSAlexander Aring * if 64: compress, encoding is 10 5452c6bed7cSAlexander Aring * if 255: compress, encoding is 11 5462c6bed7cSAlexander Aring * else do not compress 5472c6bed7cSAlexander Aring */ 5482c6bed7cSAlexander Aring switch (hdr->hop_limit) { 5492c6bed7cSAlexander Aring case 1: 5502c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TTL_1; 5512c6bed7cSAlexander Aring break; 5522c6bed7cSAlexander Aring case 64: 5532c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TTL_64; 5542c6bed7cSAlexander Aring break; 5552c6bed7cSAlexander Aring case 255: 5562c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TTL_255; 5572c6bed7cSAlexander Aring break; 5582c6bed7cSAlexander Aring default: 55985c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit, 56085c71240SAlexander Aring sizeof(hdr->hop_limit)); 5612c6bed7cSAlexander Aring } 5622c6bed7cSAlexander Aring 563556a5bfcSAlexander Aring addr_type = ipv6_addr_type(&hdr->saddr); 5642c6bed7cSAlexander Aring /* source address compression */ 565556a5bfcSAlexander Aring if (addr_type == IPV6_ADDR_ANY) { 5662c6bed7cSAlexander Aring pr_debug("source address is unspecified, setting SAC\n"); 5672c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_SAC; 568556a5bfcSAlexander Aring } else { 569556a5bfcSAlexander Aring if (addr_type & IPV6_ADDR_LINKLOCAL) { 57084ca5e03SAlexander Aring iphc1 |= lowpan_compress_addr_64(&hc_ptr, 571556a5bfcSAlexander Aring LOWPAN_IPHC_SAM_BIT, 572a6f77389SAlexander Aring &hdr->saddr, saddr); 573556a5bfcSAlexander Aring pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n", 574556a5bfcSAlexander Aring &hdr->saddr, iphc1); 5752c6bed7cSAlexander Aring } else { 5762c6bed7cSAlexander Aring pr_debug("send the full source address\n"); 577556a5bfcSAlexander Aring lowpan_push_hc_data(&hc_ptr, hdr->saddr.s6_addr, 16); 578556a5bfcSAlexander Aring } 5792c6bed7cSAlexander Aring } 5802c6bed7cSAlexander Aring 581556a5bfcSAlexander Aring addr_type = ipv6_addr_type(&hdr->daddr); 5822c6bed7cSAlexander Aring /* destination address compression */ 583556a5bfcSAlexander Aring if (addr_type & IPV6_ADDR_MULTICAST) { 5842c6bed7cSAlexander Aring pr_debug("destination address is multicast: "); 5852c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_M; 5862c6bed7cSAlexander Aring if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) { 5872c6bed7cSAlexander Aring pr_debug("compressed to 1 octet\n"); 5882c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_11; 5892c6bed7cSAlexander Aring /* use last byte */ 59085c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 59185c71240SAlexander Aring &hdr->daddr.s6_addr[15], 1); 5922c6bed7cSAlexander Aring } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) { 5932c6bed7cSAlexander Aring pr_debug("compressed to 4 octets\n"); 5942c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_10; 5952c6bed7cSAlexander Aring /* second byte + the last three */ 59685c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 59785c71240SAlexander Aring &hdr->daddr.s6_addr[1], 1); 59885c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 59985c71240SAlexander Aring &hdr->daddr.s6_addr[13], 3); 6002c6bed7cSAlexander Aring } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) { 6012c6bed7cSAlexander Aring pr_debug("compressed to 6 octets\n"); 6022c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_01; 6032c6bed7cSAlexander Aring /* second byte + the last five */ 60485c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 60585c71240SAlexander Aring &hdr->daddr.s6_addr[1], 1); 60685c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 60785c71240SAlexander Aring &hdr->daddr.s6_addr[11], 5); 6082c6bed7cSAlexander Aring } else { 6092c6bed7cSAlexander Aring pr_debug("using full address\n"); 6102c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_00; 61185c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, hdr->daddr.s6_addr, 16); 6122c6bed7cSAlexander Aring } 6132c6bed7cSAlexander Aring } else { 614556a5bfcSAlexander Aring if (addr_type & IPV6_ADDR_LINKLOCAL) { 6152c6bed7cSAlexander Aring /* TODO: context lookup */ 61684ca5e03SAlexander Aring iphc1 |= lowpan_compress_addr_64(&hc_ptr, 617a6f77389SAlexander Aring LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr); 6182c6bed7cSAlexander Aring pr_debug("dest address unicast link-local %pI6c " 6192c6bed7cSAlexander Aring "iphc1 0x%02x\n", &hdr->daddr, iphc1); 6202c6bed7cSAlexander Aring } else { 6212c6bed7cSAlexander Aring pr_debug("dest address unicast %pI6c\n", &hdr->daddr); 62285c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, hdr->daddr.s6_addr, 16); 6232c6bed7cSAlexander Aring } 6242c6bed7cSAlexander Aring } 6252c6bed7cSAlexander Aring 626cc6ed268SAlexander Aring /* next header compression */ 627cc6ed268SAlexander Aring if (iphc0 & LOWPAN_IPHC_NH_C) { 628cc6ed268SAlexander Aring ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr); 629cc6ed268SAlexander Aring if (ret < 0) 630cc6ed268SAlexander Aring return ret; 631cc6ed268SAlexander Aring } 6322c6bed7cSAlexander Aring 6332c6bed7cSAlexander Aring head[0] = iphc0; 6342c6bed7cSAlexander Aring head[1] = iphc1; 6352c6bed7cSAlexander Aring 6362c6bed7cSAlexander Aring skb_pull(skb, sizeof(struct ipv6hdr)); 6372c6bed7cSAlexander Aring skb_reset_transport_header(skb); 63884ca5e03SAlexander Aring memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head); 6392c6bed7cSAlexander Aring skb_reset_network_header(skb); 6402c6bed7cSAlexander Aring 64184ca5e03SAlexander Aring pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len); 6422c6bed7cSAlexander Aring 6432c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb data dump compressed", 6442c6bed7cSAlexander Aring skb->data, skb->len); 6452c6bed7cSAlexander Aring return 0; 6462c6bed7cSAlexander Aring } 6472c6bed7cSAlexander Aring EXPORT_SYMBOL_GPL(lowpan_header_compress); 648