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 565609c185SAlexander Aring #include "6lowpan_i.h" 57cc6ed268SAlexander Aring #include "nhc.h" 58cc6ed268SAlexander Aring 59c8a3e7ebSAlexander Aring /* Values of fields within the IPHC encoding first byte */ 60c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_TF_MASK 0x18 61c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_TF_00 0x00 62c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_TF_01 0x08 63c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_TF_10 0x10 64c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_TF_11 0x18 656350047eSAlexander Aring 66c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_NH 0x04 67c8a3e7ebSAlexander Aring 68c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_HLIM_MASK 0x03 69c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_HLIM_00 0x00 70c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_HLIM_01 0x01 71c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_HLIM_10 0x02 72c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_HLIM_11 0x03 736350047eSAlexander Aring 746350047eSAlexander Aring /* Values of fields within the IPHC encoding second byte */ 756350047eSAlexander Aring #define LOWPAN_IPHC_CID 0x80 766350047eSAlexander Aring 776350047eSAlexander Aring #define LOWPAN_IPHC_SAC 0x40 786350047eSAlexander Aring 79c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_SAM_MASK 0x30 80c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_SAM_00 0x00 81c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_SAM_01 0x10 82c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_SAM_10 0x20 83c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_SAM_11 0x30 846350047eSAlexander Aring 856350047eSAlexander Aring #define LOWPAN_IPHC_M 0x08 86c8a3e7ebSAlexander Aring 876350047eSAlexander Aring #define LOWPAN_IPHC_DAC 0x04 88c8a3e7ebSAlexander Aring 89c8a3e7ebSAlexander Aring #define LOWPAN_IPHC_DAM_MASK 0x03 906350047eSAlexander Aring #define LOWPAN_IPHC_DAM_00 0x00 916350047eSAlexander Aring #define LOWPAN_IPHC_DAM_01 0x01 926350047eSAlexander Aring #define LOWPAN_IPHC_DAM_10 0x02 936350047eSAlexander Aring #define LOWPAN_IPHC_DAM_11 0x03 946350047eSAlexander Aring 956350047eSAlexander Aring /* ipv6 address based on mac 966350047eSAlexander Aring * second bit-flip (Universe/Local) is done according RFC2464 976350047eSAlexander Aring */ 986350047eSAlexander Aring #define is_addr_mac_addr_based(a, m) \ 996350047eSAlexander Aring ((((a)->s6_addr[8]) == (((m)[0]) ^ 0x02)) && \ 1006350047eSAlexander Aring (((a)->s6_addr[9]) == (m)[1]) && \ 1016350047eSAlexander Aring (((a)->s6_addr[10]) == (m)[2]) && \ 1026350047eSAlexander Aring (((a)->s6_addr[11]) == (m)[3]) && \ 1036350047eSAlexander Aring (((a)->s6_addr[12]) == (m)[4]) && \ 1046350047eSAlexander Aring (((a)->s6_addr[13]) == (m)[5]) && \ 1056350047eSAlexander Aring (((a)->s6_addr[14]) == (m)[6]) && \ 1066350047eSAlexander Aring (((a)->s6_addr[15]) == (m)[7])) 1076350047eSAlexander Aring 1086350047eSAlexander Aring /* check whether we can compress the IID to 16 bits, 1096350047eSAlexander Aring * it's possible for unicast addresses with first 49 bits are zero only. 1106350047eSAlexander Aring */ 1116350047eSAlexander Aring #define lowpan_is_iid_16_bit_compressable(a) \ 1126350047eSAlexander Aring ((((a)->s6_addr16[4]) == 0) && \ 1136350047eSAlexander Aring (((a)->s6_addr[10]) == 0) && \ 1146350047eSAlexander Aring (((a)->s6_addr[11]) == 0xff) && \ 1156350047eSAlexander Aring (((a)->s6_addr[12]) == 0xfe) && \ 1166350047eSAlexander Aring (((a)->s6_addr[13]) == 0)) 1176350047eSAlexander Aring 1186350047eSAlexander Aring /* check whether the 112-bit gid of the multicast address is mappable to: */ 1196350047eSAlexander Aring 1206350047eSAlexander Aring /* 48 bits, FFXX::00XX:XXXX:XXXX */ 1216350047eSAlexander Aring #define lowpan_is_mcast_addr_compressable48(a) \ 1226350047eSAlexander Aring ((((a)->s6_addr16[1]) == 0) && \ 1236350047eSAlexander Aring (((a)->s6_addr16[2]) == 0) && \ 1246350047eSAlexander Aring (((a)->s6_addr16[3]) == 0) && \ 1256350047eSAlexander Aring (((a)->s6_addr16[4]) == 0) && \ 1266350047eSAlexander Aring (((a)->s6_addr[10]) == 0)) 1276350047eSAlexander Aring 1286350047eSAlexander Aring /* 32 bits, FFXX::00XX:XXXX */ 1296350047eSAlexander Aring #define lowpan_is_mcast_addr_compressable32(a) \ 1306350047eSAlexander Aring ((((a)->s6_addr16[1]) == 0) && \ 1316350047eSAlexander Aring (((a)->s6_addr16[2]) == 0) && \ 1326350047eSAlexander Aring (((a)->s6_addr16[3]) == 0) && \ 1336350047eSAlexander Aring (((a)->s6_addr16[4]) == 0) && \ 1346350047eSAlexander Aring (((a)->s6_addr16[5]) == 0) && \ 1356350047eSAlexander Aring (((a)->s6_addr[12]) == 0)) 1366350047eSAlexander Aring 1376350047eSAlexander Aring /* 8 bits, FF02::00XX */ 1386350047eSAlexander Aring #define lowpan_is_mcast_addr_compressable8(a) \ 1396350047eSAlexander Aring ((((a)->s6_addr[1]) == 2) && \ 1406350047eSAlexander Aring (((a)->s6_addr16[1]) == 0) && \ 1416350047eSAlexander Aring (((a)->s6_addr16[2]) == 0) && \ 1426350047eSAlexander Aring (((a)->s6_addr16[3]) == 0) && \ 1436350047eSAlexander Aring (((a)->s6_addr16[4]) == 0) && \ 1446350047eSAlexander Aring (((a)->s6_addr16[5]) == 0) && \ 1456350047eSAlexander Aring (((a)->s6_addr16[6]) == 0) && \ 1466350047eSAlexander Aring (((a)->s6_addr[14]) == 0)) 1476350047eSAlexander Aring 148feb2add3SAlexander Aring #define lowpan_is_linklocal_zero_padded(a) \ 149feb2add3SAlexander Aring (!(hdr->saddr.s6_addr[1] & 0x3f) && \ 150feb2add3SAlexander Aring !hdr->saddr.s6_addr16[1] && \ 151feb2add3SAlexander Aring !hdr->saddr.s6_addr32[1]) 152feb2add3SAlexander Aring 1535609c185SAlexander Aring #define LOWPAN_IPHC_CID_DCI(cid) (cid & 0x0f) 1545609c185SAlexander Aring #define LOWPAN_IPHC_CID_SCI(cid) ((cid & 0xf0) >> 4) 1555609c185SAlexander Aring 1567115a968SAlexander Aring static inline void 1577115a968SAlexander Aring lowpan_iphc_uncompress_802154_lladdr(struct in6_addr *ipaddr, 1588911d774SAlexander Aring const void *lladdr) 1598911d774SAlexander Aring { 1608911d774SAlexander Aring const struct ieee802154_addr *addr = lladdr; 1612bc068c3SAlexander Aring u8 eui64[EUI64_ADDR_LEN]; 1628911d774SAlexander Aring 1638911d774SAlexander Aring switch (addr->mode) { 1648911d774SAlexander Aring case IEEE802154_ADDR_LONG: 1658911d774SAlexander Aring ieee802154_le64_to_be64(eui64, &addr->extended_addr); 1667115a968SAlexander Aring lowpan_iphc_uncompress_eui64_lladdr(ipaddr, eui64); 1678911d774SAlexander Aring break; 1688911d774SAlexander Aring case IEEE802154_ADDR_SHORT: 1698911d774SAlexander Aring /* fe:80::ff:fe00:XXXX 1708911d774SAlexander Aring * \__/ 1718911d774SAlexander Aring * short_addr 1728911d774SAlexander Aring * 1738911d774SAlexander Aring * Universe/Local bit is zero. 1748911d774SAlexander Aring */ 1758911d774SAlexander Aring ipaddr->s6_addr[0] = 0xFE; 1768911d774SAlexander Aring ipaddr->s6_addr[1] = 0x80; 1778911d774SAlexander Aring ipaddr->s6_addr[11] = 0xFF; 1788911d774SAlexander Aring ipaddr->s6_addr[12] = 0xFE; 1798911d774SAlexander Aring ieee802154_le16_to_be16(&ipaddr->s6_addr16[7], 1808911d774SAlexander Aring &addr->short_addr); 1818911d774SAlexander Aring break; 1828911d774SAlexander Aring default: 1838911d774SAlexander Aring /* should never handled and filtered by 802154 6lowpan */ 1848911d774SAlexander Aring WARN_ON_ONCE(1); 1858911d774SAlexander Aring break; 1868911d774SAlexander Aring } 1878911d774SAlexander Aring } 1888911d774SAlexander Aring 1895609c185SAlexander Aring static struct lowpan_iphc_ctx * 1905609c185SAlexander Aring lowpan_iphc_ctx_get_by_id(const struct net_device *dev, u8 id) 1915609c185SAlexander Aring { 1922e4d60cbSAlexander Aring struct lowpan_iphc_ctx *ret = &lowpan_dev(dev)->ctx.table[id]; 1935609c185SAlexander Aring 1945609c185SAlexander Aring if (!lowpan_iphc_ctx_is_active(ret)) 1955609c185SAlexander Aring return NULL; 1965609c185SAlexander Aring 1975609c185SAlexander Aring return ret; 1985609c185SAlexander Aring } 1995609c185SAlexander Aring 2005609c185SAlexander Aring static struct lowpan_iphc_ctx * 2015609c185SAlexander Aring lowpan_iphc_ctx_get_by_addr(const struct net_device *dev, 2025609c185SAlexander Aring const struct in6_addr *addr) 2035609c185SAlexander Aring { 2042e4d60cbSAlexander Aring struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table; 2055609c185SAlexander Aring struct lowpan_iphc_ctx *ret = NULL; 2065609c185SAlexander Aring struct in6_addr addr_pfx; 2075609c185SAlexander Aring u8 addr_plen; 2085609c185SAlexander Aring int i; 2095609c185SAlexander Aring 2105609c185SAlexander Aring for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) { 2115609c185SAlexander Aring /* Check if context is valid. A context that is not valid 2125609c185SAlexander Aring * MUST NOT be used for compression. 2135609c185SAlexander Aring */ 2145609c185SAlexander Aring if (!lowpan_iphc_ctx_is_active(&table[i]) || 2155609c185SAlexander Aring !lowpan_iphc_ctx_is_compression(&table[i])) 2165609c185SAlexander Aring continue; 2175609c185SAlexander Aring 2185609c185SAlexander Aring ipv6_addr_prefix(&addr_pfx, addr, table[i].plen); 2195609c185SAlexander Aring 2205609c185SAlexander Aring /* if prefix len < 64, the remaining bits until 64th bit is 2215609c185SAlexander Aring * zero. Otherwise we use table[i]->plen. 2225609c185SAlexander Aring */ 2235609c185SAlexander Aring if (table[i].plen < 64) 2245609c185SAlexander Aring addr_plen = 64; 2255609c185SAlexander Aring else 2265609c185SAlexander Aring addr_plen = table[i].plen; 2275609c185SAlexander Aring 2285609c185SAlexander Aring if (ipv6_prefix_equal(&addr_pfx, &table[i].pfx, addr_plen)) { 2295609c185SAlexander Aring /* remember first match */ 2305609c185SAlexander Aring if (!ret) { 2315609c185SAlexander Aring ret = &table[i]; 2325609c185SAlexander Aring continue; 2335609c185SAlexander Aring } 2345609c185SAlexander Aring 2355609c185SAlexander Aring /* get the context with longest prefix len */ 2365609c185SAlexander Aring if (table[i].plen > ret->plen) 2375609c185SAlexander Aring ret = &table[i]; 2385609c185SAlexander Aring } 2395609c185SAlexander Aring } 2405609c185SAlexander Aring 2415609c185SAlexander Aring return ret; 2425609c185SAlexander Aring } 2435609c185SAlexander Aring 2445609c185SAlexander Aring static struct lowpan_iphc_ctx * 2455609c185SAlexander Aring lowpan_iphc_ctx_get_by_mcast_addr(const struct net_device *dev, 2465609c185SAlexander Aring const struct in6_addr *addr) 2475609c185SAlexander Aring { 2482e4d60cbSAlexander Aring struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table; 2495609c185SAlexander Aring struct lowpan_iphc_ctx *ret = NULL; 2505609c185SAlexander Aring struct in6_addr addr_mcast, network_pfx = {}; 2515609c185SAlexander Aring int i; 2525609c185SAlexander Aring 2535609c185SAlexander Aring /* init mcast address with */ 2545609c185SAlexander Aring memcpy(&addr_mcast, addr, sizeof(*addr)); 2555609c185SAlexander Aring 2565609c185SAlexander Aring for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) { 2575609c185SAlexander Aring /* Check if context is valid. A context that is not valid 2585609c185SAlexander Aring * MUST NOT be used for compression. 2595609c185SAlexander Aring */ 2605609c185SAlexander Aring if (!lowpan_iphc_ctx_is_active(&table[i]) || 2615609c185SAlexander Aring !lowpan_iphc_ctx_is_compression(&table[i])) 2625609c185SAlexander Aring continue; 2635609c185SAlexander Aring 2645609c185SAlexander Aring /* setting plen */ 2655609c185SAlexander Aring addr_mcast.s6_addr[3] = table[i].plen; 2665609c185SAlexander Aring /* get network prefix to copy into multicast address */ 2675609c185SAlexander Aring ipv6_addr_prefix(&network_pfx, &table[i].pfx, 2685609c185SAlexander Aring table[i].plen); 2695609c185SAlexander Aring /* setting network prefix */ 2705609c185SAlexander Aring memcpy(&addr_mcast.s6_addr[4], &network_pfx, 8); 2715609c185SAlexander Aring 2725609c185SAlexander Aring if (ipv6_addr_equal(addr, &addr_mcast)) { 2735609c185SAlexander Aring ret = &table[i]; 2745609c185SAlexander Aring break; 2755609c185SAlexander Aring } 2765609c185SAlexander Aring } 2775609c185SAlexander Aring 2785609c185SAlexander Aring return ret; 2795609c185SAlexander Aring } 2805609c185SAlexander Aring 2813fd0202aSDavid S. Miller /* Uncompress address function for source and 2822c6bed7cSAlexander Aring * destination address(non-multicast). 2832c6bed7cSAlexander Aring * 284c8a3e7ebSAlexander Aring * address_mode is the masked value for sam or dam value 2852c6bed7cSAlexander Aring */ 2867115a968SAlexander Aring static int lowpan_iphc_uncompress_addr(struct sk_buff *skb, 2877115a968SAlexander Aring const struct net_device *dev, 2887115a968SAlexander Aring struct in6_addr *ipaddr, 2897115a968SAlexander Aring u8 address_mode, const void *lladdr) 2902c6bed7cSAlexander Aring { 2912c6bed7cSAlexander Aring bool fail; 2922c6bed7cSAlexander Aring 2932c6bed7cSAlexander Aring switch (address_mode) { 294c8a3e7ebSAlexander Aring /* SAM and DAM are the same here */ 295c8a3e7ebSAlexander Aring case LOWPAN_IPHC_DAM_00: 2962c6bed7cSAlexander Aring /* for global link addresses */ 2972c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 2982c6bed7cSAlexander Aring break; 299c8a3e7ebSAlexander Aring case LOWPAN_IPHC_SAM_01: 300c8a3e7ebSAlexander Aring case LOWPAN_IPHC_DAM_01: 3012c6bed7cSAlexander Aring /* fe:80::XXXX:XXXX:XXXX:XXXX */ 3022c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 3032c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 3042c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); 3052c6bed7cSAlexander Aring break; 306c8a3e7ebSAlexander Aring case LOWPAN_IPHC_SAM_10: 307c8a3e7ebSAlexander Aring case LOWPAN_IPHC_DAM_10: 3082c6bed7cSAlexander Aring /* fe:80::ff:fe00:XXXX */ 3092c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 3102c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 3112c6bed7cSAlexander Aring ipaddr->s6_addr[11] = 0xFF; 3122c6bed7cSAlexander Aring ipaddr->s6_addr[12] = 0xFE; 3132c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); 3142c6bed7cSAlexander Aring break; 315c8a3e7ebSAlexander Aring case LOWPAN_IPHC_SAM_11: 316c8a3e7ebSAlexander Aring case LOWPAN_IPHC_DAM_11: 3172c6bed7cSAlexander Aring fail = false; 3182e4d60cbSAlexander Aring switch (lowpan_dev(dev)->lltype) { 3198911d774SAlexander Aring case LOWPAN_LLTYPE_IEEE802154: 3207115a968SAlexander Aring lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr); 3212c6bed7cSAlexander Aring break; 3222c6bed7cSAlexander Aring default: 3237115a968SAlexander Aring lowpan_iphc_uncompress_eui64_lladdr(ipaddr, lladdr); 3248911d774SAlexander Aring break; 3252c6bed7cSAlexander Aring } 3262c6bed7cSAlexander Aring break; 3272c6bed7cSAlexander Aring default: 3282c6bed7cSAlexander Aring pr_debug("Invalid address mode value: 0x%x\n", address_mode); 3292c6bed7cSAlexander Aring return -EINVAL; 3302c6bed7cSAlexander Aring } 3312c6bed7cSAlexander Aring 3322c6bed7cSAlexander Aring if (fail) { 3332c6bed7cSAlexander Aring pr_debug("Failed to fetch skb data\n"); 3342c6bed7cSAlexander Aring return -EIO; 3352c6bed7cSAlexander Aring } 3362c6bed7cSAlexander Aring 3372c6bed7cSAlexander Aring raw_dump_inline(NULL, "Reconstructed ipv6 addr is", 3382c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 3392c6bed7cSAlexander Aring 3402c6bed7cSAlexander Aring return 0; 3412c6bed7cSAlexander Aring } 3422c6bed7cSAlexander Aring 3433fd0202aSDavid S. Miller /* Uncompress address function for source context 3442c6bed7cSAlexander Aring * based address(non-multicast). 3452c6bed7cSAlexander Aring */ 3467115a968SAlexander Aring static int lowpan_iphc_uncompress_ctx_addr(struct sk_buff *skb, 3475609c185SAlexander Aring const struct net_device *dev, 3485609c185SAlexander Aring const struct lowpan_iphc_ctx *ctx, 3497115a968SAlexander Aring struct in6_addr *ipaddr, 3507115a968SAlexander Aring u8 address_mode, const void *lladdr) 3512c6bed7cSAlexander Aring { 3525609c185SAlexander Aring bool fail; 3535609c185SAlexander Aring 354c8a3e7ebSAlexander Aring switch (address_mode) { 3555609c185SAlexander Aring /* SAM and DAM are the same here */ 3565609c185SAlexander Aring case LOWPAN_IPHC_DAM_00: 3575609c185SAlexander Aring fail = false; 3585609c185SAlexander Aring /* SAM_00 -> unspec address :: 3592c6bed7cSAlexander Aring * Do nothing, address is already :: 3605609c185SAlexander Aring * 3615609c185SAlexander Aring * DAM 00 -> reserved should never occur. 3622c6bed7cSAlexander Aring */ 3632c6bed7cSAlexander Aring break; 364c8a3e7ebSAlexander Aring case LOWPAN_IPHC_SAM_01: 3655609c185SAlexander Aring case LOWPAN_IPHC_DAM_01: 3665609c185SAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); 3675609c185SAlexander Aring ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); 3685609c185SAlexander Aring break; 369c8a3e7ebSAlexander Aring case LOWPAN_IPHC_SAM_10: 3705609c185SAlexander Aring case LOWPAN_IPHC_DAM_10: 3715609c185SAlexander Aring ipaddr->s6_addr[11] = 0xFF; 3725609c185SAlexander Aring ipaddr->s6_addr[12] = 0xFE; 3735609c185SAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); 3745609c185SAlexander Aring ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); 3755609c185SAlexander Aring break; 376c8a3e7ebSAlexander Aring case LOWPAN_IPHC_SAM_11: 3775609c185SAlexander Aring case LOWPAN_IPHC_DAM_11: 3785609c185SAlexander Aring fail = false; 3792e4d60cbSAlexander Aring switch (lowpan_dev(dev)->lltype) { 3805609c185SAlexander Aring case LOWPAN_LLTYPE_IEEE802154: 3817115a968SAlexander Aring lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr); 3825609c185SAlexander Aring break; 3835609c185SAlexander Aring default: 3847115a968SAlexander Aring lowpan_iphc_uncompress_eui64_lladdr(ipaddr, lladdr); 3855609c185SAlexander Aring break; 3865609c185SAlexander Aring } 3875609c185SAlexander Aring ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); 3885609c185SAlexander Aring break; 3892c6bed7cSAlexander Aring default: 390c8a3e7ebSAlexander Aring pr_debug("Invalid sam value: 0x%x\n", address_mode); 3912c6bed7cSAlexander Aring return -EINVAL; 3922c6bed7cSAlexander Aring } 3932c6bed7cSAlexander Aring 3945609c185SAlexander Aring if (fail) { 3955609c185SAlexander Aring pr_debug("Failed to fetch skb data\n"); 3965609c185SAlexander Aring return -EIO; 3975609c185SAlexander Aring } 3985609c185SAlexander Aring 3992c6bed7cSAlexander Aring raw_dump_inline(NULL, 4002c6bed7cSAlexander Aring "Reconstructed context based ipv6 src addr is", 4012c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 4022c6bed7cSAlexander Aring 4032c6bed7cSAlexander Aring return 0; 4042c6bed7cSAlexander Aring } 4052c6bed7cSAlexander Aring 4062c6bed7cSAlexander Aring /* Uncompress function for multicast destination address, 4072c6bed7cSAlexander Aring * when M bit is set. 4082c6bed7cSAlexander Aring */ 4097fc4cfdaSMarcel Holtmann static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb, 4102c6bed7cSAlexander Aring struct in6_addr *ipaddr, 411c8a3e7ebSAlexander Aring u8 address_mode) 4122c6bed7cSAlexander Aring { 4132c6bed7cSAlexander Aring bool fail; 4142c6bed7cSAlexander Aring 415c8a3e7ebSAlexander Aring switch (address_mode) { 4162c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_00: 4172c6bed7cSAlexander Aring /* 00: 128 bits. The full address 4182c6bed7cSAlexander Aring * is carried in-line. 4192c6bed7cSAlexander Aring */ 4202c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 4212c6bed7cSAlexander Aring break; 4222c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_01: 4232c6bed7cSAlexander Aring /* 01: 48 bits. The address takes 4242c6bed7cSAlexander Aring * the form ffXX::00XX:XXXX:XXXX. 4252c6bed7cSAlexander Aring */ 4262c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 4272c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 4282c6bed7cSAlexander Aring fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); 4292c6bed7cSAlexander Aring break; 4302c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_10: 4312c6bed7cSAlexander Aring /* 10: 32 bits. The address takes 4322c6bed7cSAlexander Aring * the form ffXX::00XX:XXXX. 4332c6bed7cSAlexander Aring */ 4342c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 4352c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 4362c6bed7cSAlexander Aring fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); 4372c6bed7cSAlexander Aring break; 4382c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_11: 4392c6bed7cSAlexander Aring /* 11: 8 bits. The address takes 4402c6bed7cSAlexander Aring * the form ff02::00XX. 4412c6bed7cSAlexander Aring */ 4422c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 4432c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x02; 4442c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); 4452c6bed7cSAlexander Aring break; 4462c6bed7cSAlexander Aring default: 447c8a3e7ebSAlexander Aring pr_debug("DAM value has a wrong value: 0x%x\n", address_mode); 4482c6bed7cSAlexander Aring return -EINVAL; 4492c6bed7cSAlexander Aring } 4502c6bed7cSAlexander Aring 4512c6bed7cSAlexander Aring if (fail) { 4522c6bed7cSAlexander Aring pr_debug("Failed to fetch skb data\n"); 4532c6bed7cSAlexander Aring return -EIO; 4542c6bed7cSAlexander Aring } 4552c6bed7cSAlexander Aring 4562c6bed7cSAlexander Aring raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is", 4572c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 4582c6bed7cSAlexander Aring 4592c6bed7cSAlexander Aring return 0; 4602c6bed7cSAlexander Aring } 4612c6bed7cSAlexander Aring 4625609c185SAlexander Aring static int lowpan_uncompress_multicast_ctx_daddr(struct sk_buff *skb, 4635609c185SAlexander Aring struct lowpan_iphc_ctx *ctx, 4645609c185SAlexander Aring struct in6_addr *ipaddr, 4655609c185SAlexander Aring u8 address_mode) 4665609c185SAlexander Aring { 4675609c185SAlexander Aring struct in6_addr network_pfx = {}; 4685609c185SAlexander Aring bool fail; 4695609c185SAlexander Aring 4705609c185SAlexander Aring ipaddr->s6_addr[0] = 0xFF; 4715609c185SAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 2); 4725609c185SAlexander Aring fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[12], 4); 4734c23d874SAndrzej Hajda if (fail) 4745609c185SAlexander Aring return -EIO; 4755609c185SAlexander Aring 4765609c185SAlexander Aring /* take prefix_len and network prefix from the context */ 4775609c185SAlexander Aring ipaddr->s6_addr[3] = ctx->plen; 4785609c185SAlexander Aring /* get network prefix to copy into multicast address */ 4795609c185SAlexander Aring ipv6_addr_prefix(&network_pfx, &ctx->pfx, ctx->plen); 4805609c185SAlexander Aring /* setting network prefix */ 4815609c185SAlexander Aring memcpy(&ipaddr->s6_addr[4], &network_pfx, 8); 4825609c185SAlexander Aring 4835609c185SAlexander Aring return 0; 4845609c185SAlexander Aring } 4855609c185SAlexander Aring 486b5af9bdbSAlexander Aring /* get the ecn values from iphc tf format and set it to ipv6hdr */ 487b5af9bdbSAlexander Aring static inline void lowpan_iphc_tf_set_ecn(struct ipv6hdr *hdr, const u8 *tf) 488b5af9bdbSAlexander Aring { 489b5af9bdbSAlexander Aring /* get the two higher bits which is ecn */ 490b5af9bdbSAlexander Aring u8 ecn = tf[0] & 0xc0; 491b5af9bdbSAlexander Aring 492b5af9bdbSAlexander Aring /* ECN takes 0x30 in hdr->flow_lbl[0] */ 493b5af9bdbSAlexander Aring hdr->flow_lbl[0] |= (ecn >> 2); 494b5af9bdbSAlexander Aring } 495b5af9bdbSAlexander Aring 496b5af9bdbSAlexander Aring /* get the dscp values from iphc tf format and set it to ipv6hdr */ 497b5af9bdbSAlexander Aring static inline void lowpan_iphc_tf_set_dscp(struct ipv6hdr *hdr, const u8 *tf) 498b5af9bdbSAlexander Aring { 499b5af9bdbSAlexander Aring /* DSCP is at place after ECN */ 500b5af9bdbSAlexander Aring u8 dscp = tf[0] & 0x3f; 501b5af9bdbSAlexander Aring 502b5af9bdbSAlexander Aring /* The four highest bits need to be set at hdr->priority */ 503b5af9bdbSAlexander Aring hdr->priority |= ((dscp & 0x3c) >> 2); 504b5af9bdbSAlexander Aring /* The two lower bits is part of hdr->flow_lbl[0] */ 505b5af9bdbSAlexander Aring hdr->flow_lbl[0] |= ((dscp & 0x03) << 6); 506b5af9bdbSAlexander Aring } 507b5af9bdbSAlexander Aring 508b5af9bdbSAlexander Aring /* get the flow label values from iphc tf format and set it to ipv6hdr */ 509b5af9bdbSAlexander Aring static inline void lowpan_iphc_tf_set_lbl(struct ipv6hdr *hdr, const u8 *lbl) 510b5af9bdbSAlexander Aring { 511b5af9bdbSAlexander Aring /* flow label is always some array started with lower nibble of 512b5af9bdbSAlexander Aring * flow_lbl[0] and followed with two bytes afterwards. Inside inline 513b5af9bdbSAlexander Aring * data the flow_lbl position can be different, which will be handled 514b5af9bdbSAlexander Aring * by lbl pointer. E.g. case "01" vs "00" the traffic class is 8 bit 515b5af9bdbSAlexander Aring * shifted, the different lbl pointer will handle that. 516b5af9bdbSAlexander Aring * 517b5af9bdbSAlexander Aring * The flow label will started at lower nibble of flow_lbl[0], the 518b5af9bdbSAlexander Aring * higher nibbles are part of DSCP + ECN. 519b5af9bdbSAlexander Aring */ 520b5af9bdbSAlexander Aring hdr->flow_lbl[0] |= lbl[0] & 0x0f; 521b5af9bdbSAlexander Aring memcpy(&hdr->flow_lbl[1], &lbl[1], 2); 522b5af9bdbSAlexander Aring } 523b5af9bdbSAlexander Aring 524b5af9bdbSAlexander Aring /* lowpan_iphc_tf_decompress - decompress the traffic class. 525b5af9bdbSAlexander Aring * This function will return zero on success, a value lower than zero if 526b5af9bdbSAlexander Aring * failed. 527b5af9bdbSAlexander Aring */ 528b5af9bdbSAlexander Aring static int lowpan_iphc_tf_decompress(struct sk_buff *skb, struct ipv6hdr *hdr, 529b5af9bdbSAlexander Aring u8 val) 530b5af9bdbSAlexander Aring { 531b5af9bdbSAlexander Aring u8 tf[4]; 532b5af9bdbSAlexander Aring 533b5af9bdbSAlexander Aring /* Traffic Class and Flow Label */ 534b5af9bdbSAlexander Aring switch (val) { 535b5af9bdbSAlexander Aring case LOWPAN_IPHC_TF_00: 536b5af9bdbSAlexander Aring /* ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) */ 537b5af9bdbSAlexander Aring if (lowpan_fetch_skb(skb, tf, 4)) 538b5af9bdbSAlexander Aring return -EINVAL; 539b5af9bdbSAlexander Aring 540b5af9bdbSAlexander Aring /* 1 2 3 541b5af9bdbSAlexander Aring * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 542b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 543b5af9bdbSAlexander Aring * |ECN| DSCP | rsv | Flow Label | 544b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 545b5af9bdbSAlexander Aring */ 546b5af9bdbSAlexander Aring lowpan_iphc_tf_set_ecn(hdr, tf); 547b5af9bdbSAlexander Aring lowpan_iphc_tf_set_dscp(hdr, tf); 548b5af9bdbSAlexander Aring lowpan_iphc_tf_set_lbl(hdr, &tf[1]); 549b5af9bdbSAlexander Aring break; 550b5af9bdbSAlexander Aring case LOWPAN_IPHC_TF_01: 551b5af9bdbSAlexander Aring /* ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided. */ 552b5af9bdbSAlexander Aring if (lowpan_fetch_skb(skb, tf, 3)) 553b5af9bdbSAlexander Aring return -EINVAL; 554b5af9bdbSAlexander Aring 555b5af9bdbSAlexander Aring /* 1 2 556b5af9bdbSAlexander Aring * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 557b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 558b5af9bdbSAlexander Aring * |ECN|rsv| Flow Label | 559b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 560b5af9bdbSAlexander Aring */ 561b5af9bdbSAlexander Aring lowpan_iphc_tf_set_ecn(hdr, tf); 562b5af9bdbSAlexander Aring lowpan_iphc_tf_set_lbl(hdr, &tf[0]); 563b5af9bdbSAlexander Aring break; 564b5af9bdbSAlexander Aring case LOWPAN_IPHC_TF_10: 565b5af9bdbSAlexander Aring /* ECN + DSCP (1 byte), Flow Label is elided. */ 566b5af9bdbSAlexander Aring if (lowpan_fetch_skb(skb, tf, 1)) 567b5af9bdbSAlexander Aring return -EINVAL; 568b5af9bdbSAlexander Aring 569b5af9bdbSAlexander Aring /* 0 1 2 3 4 5 6 7 570b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+ 571b5af9bdbSAlexander Aring * |ECN| DSCP | 572b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+ 573b5af9bdbSAlexander Aring */ 574b5af9bdbSAlexander Aring lowpan_iphc_tf_set_ecn(hdr, tf); 575b5af9bdbSAlexander Aring lowpan_iphc_tf_set_dscp(hdr, tf); 576b5af9bdbSAlexander Aring break; 577b5af9bdbSAlexander Aring case LOWPAN_IPHC_TF_11: 578b5af9bdbSAlexander Aring /* Traffic Class and Flow Label are elided */ 579b5af9bdbSAlexander Aring break; 580b5af9bdbSAlexander Aring default: 581b5af9bdbSAlexander Aring WARN_ON_ONCE(1); 582b5af9bdbSAlexander Aring return -EINVAL; 583b5af9bdbSAlexander Aring } 584b5af9bdbSAlexander Aring 585b5af9bdbSAlexander Aring return 0; 586b5af9bdbSAlexander Aring } 587b5af9bdbSAlexander Aring 5882c6bed7cSAlexander Aring /* TTL uncompression values */ 589c8a3e7ebSAlexander Aring static const u8 lowpan_ttl_values[] = { 590c8a3e7ebSAlexander Aring [LOWPAN_IPHC_HLIM_01] = 1, 591c8a3e7ebSAlexander Aring [LOWPAN_IPHC_HLIM_10] = 64, 592c8a3e7ebSAlexander Aring [LOWPAN_IPHC_HLIM_11] = 255, 593c8a3e7ebSAlexander Aring }; 5942c6bed7cSAlexander Aring 5958911d774SAlexander Aring int lowpan_header_decompress(struct sk_buff *skb, const struct net_device *dev, 5968911d774SAlexander Aring const void *daddr, const void *saddr) 5972c6bed7cSAlexander Aring { 5982c6bed7cSAlexander Aring struct ipv6hdr hdr = {}; 5995609c185SAlexander Aring struct lowpan_iphc_ctx *ci; 6005609c185SAlexander Aring u8 iphc0, iphc1, cid = 0; 6012c6bed7cSAlexander Aring int err; 6022c6bed7cSAlexander Aring 6032c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb data dump uncompressed", 6042c6bed7cSAlexander Aring skb->data, skb->len); 6052c6bed7cSAlexander Aring 606478208e3SAlexander Aring if (lowpan_fetch_skb(skb, &iphc0, sizeof(iphc0)) || 607478208e3SAlexander Aring lowpan_fetch_skb(skb, &iphc1, sizeof(iphc1))) 6088911d774SAlexander Aring return -EINVAL; 6098911d774SAlexander Aring 6102c6bed7cSAlexander Aring hdr.version = 6; 6112c6bed7cSAlexander Aring 6125609c185SAlexander Aring /* default CID = 0, another if the CID flag is set */ 6135609c185SAlexander Aring if (iphc1 & LOWPAN_IPHC_CID) { 6145609c185SAlexander Aring if (lowpan_fetch_skb(skb, &cid, sizeof(cid))) 6155609c185SAlexander Aring return -EINVAL; 6165609c185SAlexander Aring } 6175609c185SAlexander Aring 618b5af9bdbSAlexander Aring err = lowpan_iphc_tf_decompress(skb, &hdr, 619b5af9bdbSAlexander Aring iphc0 & LOWPAN_IPHC_TF_MASK); 620b5af9bdbSAlexander Aring if (err < 0) 621b5af9bdbSAlexander Aring return err; 6222c6bed7cSAlexander Aring 6232c6bed7cSAlexander Aring /* Next Header */ 624c8a3e7ebSAlexander Aring if (!(iphc0 & LOWPAN_IPHC_NH)) { 6252c6bed7cSAlexander Aring /* Next header is carried inline */ 6264ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr))) 62756b2c3eeSMartin Townsend return -EINVAL; 6282c6bed7cSAlexander Aring 6292c6bed7cSAlexander Aring pr_debug("NH flag is set, next header carried inline: %02x\n", 6302c6bed7cSAlexander Aring hdr.nexthdr); 6312c6bed7cSAlexander Aring } 6322c6bed7cSAlexander Aring 6332c6bed7cSAlexander Aring /* Hop Limit */ 634c8a3e7ebSAlexander Aring if ((iphc0 & LOWPAN_IPHC_HLIM_MASK) != LOWPAN_IPHC_HLIM_00) { 635c8a3e7ebSAlexander Aring hdr.hop_limit = lowpan_ttl_values[iphc0 & LOWPAN_IPHC_HLIM_MASK]; 6363fd0202aSDavid S. Miller } else { 6374ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &hdr.hop_limit, 6384ebc960fSAlexander Aring sizeof(hdr.hop_limit))) 63956b2c3eeSMartin Townsend return -EINVAL; 6402c6bed7cSAlexander Aring } 6412c6bed7cSAlexander Aring 6422c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_SAC) { 6432e4d60cbSAlexander Aring spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 6445609c185SAlexander Aring ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_SCI(cid)); 6455609c185SAlexander Aring if (!ci) { 6462e4d60cbSAlexander Aring spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 6475609c185SAlexander Aring return -EINVAL; 6485609c185SAlexander Aring } 6495609c185SAlexander Aring 6502c6bed7cSAlexander Aring pr_debug("SAC bit is set. Handle context based source address.\n"); 6517115a968SAlexander Aring err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.saddr, 6527115a968SAlexander Aring iphc1 & LOWPAN_IPHC_SAM_MASK, 6537115a968SAlexander Aring saddr); 6542e4d60cbSAlexander Aring spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 6552c6bed7cSAlexander Aring } else { 6562c6bed7cSAlexander Aring /* Source address uncompression */ 6572c6bed7cSAlexander Aring pr_debug("source address stateless compression\n"); 6587115a968SAlexander Aring err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.saddr, 6597115a968SAlexander Aring iphc1 & LOWPAN_IPHC_SAM_MASK, 6607115a968SAlexander Aring saddr); 6612c6bed7cSAlexander Aring } 6622c6bed7cSAlexander Aring 6632c6bed7cSAlexander Aring /* Check on error of previous branch */ 6642c6bed7cSAlexander Aring if (err) 66556b2c3eeSMartin Townsend return -EINVAL; 6662c6bed7cSAlexander Aring 6675609c185SAlexander Aring switch (iphc1 & (LOWPAN_IPHC_M | LOWPAN_IPHC_DAC)) { 6685609c185SAlexander Aring case LOWPAN_IPHC_M | LOWPAN_IPHC_DAC: 6692e4d60cbSAlexander Aring spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 6705609c185SAlexander Aring ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid)); 6715609c185SAlexander Aring if (!ci) { 6722e4d60cbSAlexander Aring spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 67356b2c3eeSMartin Townsend return -EINVAL; 6742c6bed7cSAlexander Aring } 6755609c185SAlexander Aring 6765609c185SAlexander Aring /* multicast with context */ 6775609c185SAlexander Aring pr_debug("dest: context-based mcast compression\n"); 6785609c185SAlexander Aring err = lowpan_uncompress_multicast_ctx_daddr(skb, ci, 6795609c185SAlexander Aring &hdr.daddr, 6805609c185SAlexander Aring iphc1 & LOWPAN_IPHC_DAM_MASK); 6812e4d60cbSAlexander Aring spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 6825609c185SAlexander Aring break; 6835609c185SAlexander Aring case LOWPAN_IPHC_M: 6845609c185SAlexander Aring /* multicast */ 6855609c185SAlexander Aring err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr, 6865609c185SAlexander Aring iphc1 & LOWPAN_IPHC_DAM_MASK); 6875609c185SAlexander Aring break; 6885609c185SAlexander Aring case LOWPAN_IPHC_DAC: 6892e4d60cbSAlexander Aring spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 6905609c185SAlexander Aring ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid)); 6915609c185SAlexander Aring if (!ci) { 6922e4d60cbSAlexander Aring spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 6935609c185SAlexander Aring return -EINVAL; 6945609c185SAlexander Aring } 6955609c185SAlexander Aring 6965609c185SAlexander Aring /* Destination address context based uncompression */ 6975609c185SAlexander Aring pr_debug("DAC bit is set. Handle context based destination address.\n"); 6987115a968SAlexander Aring err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.daddr, 6997115a968SAlexander Aring iphc1 & LOWPAN_IPHC_DAM_MASK, 7007115a968SAlexander Aring daddr); 7012e4d60cbSAlexander Aring spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 7025609c185SAlexander Aring break; 7035609c185SAlexander Aring default: 7047115a968SAlexander Aring err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.daddr, 7057115a968SAlexander Aring iphc1 & LOWPAN_IPHC_DAM_MASK, 7067115a968SAlexander Aring daddr); 7072c6bed7cSAlexander Aring pr_debug("dest: stateless compression mode %d dest %pI6c\n", 708c8a3e7ebSAlexander Aring iphc1 & LOWPAN_IPHC_DAM_MASK, &hdr.daddr); 7095609c185SAlexander Aring break; 7105609c185SAlexander Aring } 7115609c185SAlexander Aring 7122c6bed7cSAlexander Aring if (err) 71356b2c3eeSMartin Townsend return -EINVAL; 7142c6bed7cSAlexander Aring 715cc6ed268SAlexander Aring /* Next header data uncompression */ 716c8a3e7ebSAlexander Aring if (iphc0 & LOWPAN_IPHC_NH) { 717cc6ed268SAlexander Aring err = lowpan_nhc_do_uncompression(skb, dev, &hdr); 718cc6ed268SAlexander Aring if (err < 0) 71911e3ff70SMartin Townsend return err; 72011e3ff70SMartin Townsend } else { 72111e3ff70SMartin Townsend err = skb_cow(skb, sizeof(hdr)); 72256b2c3eeSMartin Townsend if (unlikely(err)) 72311e3ff70SMartin Townsend return err; 72411e3ff70SMartin Townsend } 7252c6bed7cSAlexander Aring 7262e4d60cbSAlexander Aring switch (lowpan_dev(dev)->lltype) { 72772a5e6bbSAlexander Aring case LOWPAN_LLTYPE_IEEE802154: 72872a5e6bbSAlexander Aring if (lowpan_802154_cb(skb)->d_size) 72972a5e6bbSAlexander Aring hdr.payload_len = htons(lowpan_802154_cb(skb)->d_size - 73072a5e6bbSAlexander Aring sizeof(struct ipv6hdr)); 73172a5e6bbSAlexander Aring else 7322c6bed7cSAlexander Aring hdr.payload_len = htons(skb->len); 73372a5e6bbSAlexander Aring break; 73472a5e6bbSAlexander Aring default: 73572a5e6bbSAlexander Aring hdr.payload_len = htons(skb->len); 73672a5e6bbSAlexander Aring break; 73772a5e6bbSAlexander Aring } 7382c6bed7cSAlexander Aring 7392c6bed7cSAlexander Aring pr_debug("skb headroom size = %d, data length = %d\n", 7402c6bed7cSAlexander Aring skb_headroom(skb), skb->len); 7412c6bed7cSAlexander Aring 7422c6bed7cSAlexander Aring pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t" 7432c6bed7cSAlexander Aring "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 7442c6bed7cSAlexander Aring hdr.version, ntohs(hdr.payload_len), hdr.nexthdr, 7452c6bed7cSAlexander Aring hdr.hop_limit, &hdr.daddr); 7462c6bed7cSAlexander Aring 747f8b36176SMartin Townsend skb_push(skb, sizeof(hdr)); 748f8b36176SMartin Townsend skb_reset_network_header(skb); 749f8b36176SMartin Townsend skb_copy_to_linear_data(skb, &hdr, sizeof(hdr)); 750f8b36176SMartin Townsend 7517fc4cfdaSMarcel Holtmann raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, sizeof(hdr)); 7522c6bed7cSAlexander Aring 753f8b36176SMartin Townsend return 0; 7542c6bed7cSAlexander Aring } 75501141234SMartin Townsend EXPORT_SYMBOL_GPL(lowpan_header_decompress); 7562c6bed7cSAlexander Aring 757c8a3e7ebSAlexander Aring static const u8 lowpan_iphc_dam_to_sam_value[] = { 758c8a3e7ebSAlexander Aring [LOWPAN_IPHC_DAM_00] = LOWPAN_IPHC_SAM_00, 759c8a3e7ebSAlexander Aring [LOWPAN_IPHC_DAM_01] = LOWPAN_IPHC_SAM_01, 760c8a3e7ebSAlexander Aring [LOWPAN_IPHC_DAM_10] = LOWPAN_IPHC_SAM_10, 761c8a3e7ebSAlexander Aring [LOWPAN_IPHC_DAM_11] = LOWPAN_IPHC_SAM_11, 762c8a3e7ebSAlexander Aring }; 763c8a3e7ebSAlexander Aring 764eab560e5SAlexander Aring static inline bool 765eab560e5SAlexander Aring lowpan_iphc_compress_ctx_802154_lladdr(const struct in6_addr *ipaddr, 766eab560e5SAlexander Aring const struct lowpan_iphc_ctx *ctx, 767eab560e5SAlexander Aring const void *lladdr) 768eab560e5SAlexander Aring { 769eab560e5SAlexander Aring const struct ieee802154_addr *addr = lladdr; 770eab560e5SAlexander Aring unsigned char extended_addr[EUI64_ADDR_LEN]; 771eab560e5SAlexander Aring bool lladdr_compress = false; 772eab560e5SAlexander Aring struct in6_addr tmp = {}; 773eab560e5SAlexander Aring 774eab560e5SAlexander Aring switch (addr->mode) { 775eab560e5SAlexander Aring case IEEE802154_ADDR_LONG: 776eab560e5SAlexander Aring ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr); 777eab560e5SAlexander Aring /* check for SAM/DAM = 11 */ 778eab560e5SAlexander Aring memcpy(&tmp.s6_addr[8], &extended_addr, EUI64_ADDR_LEN); 779eab560e5SAlexander Aring /* second bit-flip (Universe/Local) is done according RFC2464 */ 780eab560e5SAlexander Aring tmp.s6_addr[8] ^= 0x02; 781eab560e5SAlexander Aring /* context information are always used */ 782eab560e5SAlexander Aring ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 783eab560e5SAlexander Aring if (ipv6_addr_equal(&tmp, ipaddr)) 784eab560e5SAlexander Aring lladdr_compress = true; 785eab560e5SAlexander Aring break; 786eab560e5SAlexander Aring case IEEE802154_ADDR_SHORT: 787eab560e5SAlexander Aring tmp.s6_addr[11] = 0xFF; 788eab560e5SAlexander Aring tmp.s6_addr[12] = 0xFE; 789eab560e5SAlexander Aring ieee802154_le16_to_be16(&tmp.s6_addr16[7], 790eab560e5SAlexander Aring &addr->short_addr); 791eab560e5SAlexander Aring /* context information are always used */ 792eab560e5SAlexander Aring ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 793eab560e5SAlexander Aring if (ipv6_addr_equal(&tmp, ipaddr)) 794eab560e5SAlexander Aring lladdr_compress = true; 795eab560e5SAlexander Aring break; 796eab560e5SAlexander Aring default: 797eab560e5SAlexander Aring /* should never handled and filtered by 802154 6lowpan */ 798eab560e5SAlexander Aring WARN_ON_ONCE(1); 799eab560e5SAlexander Aring break; 800eab560e5SAlexander Aring } 801eab560e5SAlexander Aring 802eab560e5SAlexander Aring return lladdr_compress; 803eab560e5SAlexander Aring } 804eab560e5SAlexander Aring 805eab560e5SAlexander Aring static u8 lowpan_compress_ctx_addr(u8 **hc_ptr, const struct net_device *dev, 806eab560e5SAlexander Aring const struct in6_addr *ipaddr, 8075609c185SAlexander Aring const struct lowpan_iphc_ctx *ctx, 8085609c185SAlexander Aring const unsigned char *lladdr, bool sam) 8095609c185SAlexander Aring { 8105609c185SAlexander Aring struct in6_addr tmp = {}; 8115609c185SAlexander Aring u8 dam; 8125609c185SAlexander Aring 813eab560e5SAlexander Aring switch (lowpan_dev(dev)->lltype) { 814eab560e5SAlexander Aring case LOWPAN_LLTYPE_IEEE802154: 815eab560e5SAlexander Aring if (lowpan_iphc_compress_ctx_802154_lladdr(ipaddr, ctx, 816eab560e5SAlexander Aring lladdr)) { 817eab560e5SAlexander Aring dam = LOWPAN_IPHC_DAM_11; 818eab560e5SAlexander Aring goto out; 819eab560e5SAlexander Aring } 820eab560e5SAlexander Aring break; 821eab560e5SAlexander Aring default: 8225609c185SAlexander Aring /* check for SAM/DAM = 11 */ 823eab560e5SAlexander Aring memcpy(&tmp.s6_addr[8], lladdr, EUI64_ADDR_LEN); 8245609c185SAlexander Aring /* second bit-flip (Universe/Local) is done according RFC2464 */ 8255609c185SAlexander Aring tmp.s6_addr[8] ^= 0x02; 8265609c185SAlexander Aring /* context information are always used */ 8275609c185SAlexander Aring ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 8285609c185SAlexander Aring if (ipv6_addr_equal(&tmp, ipaddr)) { 8295609c185SAlexander Aring dam = LOWPAN_IPHC_DAM_11; 8305609c185SAlexander Aring goto out; 8315609c185SAlexander Aring } 832eab560e5SAlexander Aring break; 833eab560e5SAlexander Aring } 8345609c185SAlexander Aring 8355609c185SAlexander Aring memset(&tmp, 0, sizeof(tmp)); 83624c4a81dSAlexander Aring /* check for SAM/DAM = 10 */ 8375609c185SAlexander Aring tmp.s6_addr[11] = 0xFF; 8385609c185SAlexander Aring tmp.s6_addr[12] = 0xFE; 8395609c185SAlexander Aring memcpy(&tmp.s6_addr[14], &ipaddr->s6_addr[14], 2); 8405609c185SAlexander Aring /* context information are always used */ 8415609c185SAlexander Aring ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 8425609c185SAlexander Aring if (ipv6_addr_equal(&tmp, ipaddr)) { 8435609c185SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[14], 2); 8445609c185SAlexander Aring dam = LOWPAN_IPHC_DAM_10; 8455609c185SAlexander Aring goto out; 8465609c185SAlexander Aring } 8475609c185SAlexander Aring 8485609c185SAlexander Aring memset(&tmp, 0, sizeof(tmp)); 84924c4a81dSAlexander Aring /* check for SAM/DAM = 01, should always match */ 8505609c185SAlexander Aring memcpy(&tmp.s6_addr[8], &ipaddr->s6_addr[8], 8); 8515609c185SAlexander Aring /* context information are always used */ 8525609c185SAlexander Aring ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 8535609c185SAlexander Aring if (ipv6_addr_equal(&tmp, ipaddr)) { 8545609c185SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[8], 8); 8555609c185SAlexander Aring dam = LOWPAN_IPHC_DAM_01; 8565609c185SAlexander Aring goto out; 8575609c185SAlexander Aring } 8585609c185SAlexander Aring 8592306f656SAlexander Aring WARN_ONCE(1, "context found but no address mode matched\n"); 8602306f656SAlexander Aring return LOWPAN_IPHC_DAM_00; 8615609c185SAlexander Aring out: 8625609c185SAlexander Aring 8635609c185SAlexander Aring if (sam) 8645609c185SAlexander Aring return lowpan_iphc_dam_to_sam_value[dam]; 8655609c185SAlexander Aring else 8665609c185SAlexander Aring return dam; 8675609c185SAlexander Aring } 8685609c185SAlexander Aring 869eab560e5SAlexander Aring static inline bool 870eab560e5SAlexander Aring lowpan_iphc_compress_802154_lladdr(const struct in6_addr *ipaddr, 871eab560e5SAlexander Aring const void *lladdr) 872eab560e5SAlexander Aring { 873eab560e5SAlexander Aring const struct ieee802154_addr *addr = lladdr; 874eab560e5SAlexander Aring unsigned char extended_addr[EUI64_ADDR_LEN]; 875eab560e5SAlexander Aring bool lladdr_compress = false; 876eab560e5SAlexander Aring struct in6_addr tmp = {}; 877eab560e5SAlexander Aring 878eab560e5SAlexander Aring switch (addr->mode) { 879eab560e5SAlexander Aring case IEEE802154_ADDR_LONG: 880eab560e5SAlexander Aring ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr); 881eab560e5SAlexander Aring if (is_addr_mac_addr_based(ipaddr, extended_addr)) 882eab560e5SAlexander Aring lladdr_compress = true; 883eab560e5SAlexander Aring break; 884eab560e5SAlexander Aring case IEEE802154_ADDR_SHORT: 885eab560e5SAlexander Aring /* fe:80::ff:fe00:XXXX 886eab560e5SAlexander Aring * \__/ 887eab560e5SAlexander Aring * short_addr 888eab560e5SAlexander Aring * 889eab560e5SAlexander Aring * Universe/Local bit is zero. 890eab560e5SAlexander Aring */ 891eab560e5SAlexander Aring tmp.s6_addr[0] = 0xFE; 892eab560e5SAlexander Aring tmp.s6_addr[1] = 0x80; 893eab560e5SAlexander Aring tmp.s6_addr[11] = 0xFF; 894eab560e5SAlexander Aring tmp.s6_addr[12] = 0xFE; 895eab560e5SAlexander Aring ieee802154_le16_to_be16(&tmp.s6_addr16[7], 896eab560e5SAlexander Aring &addr->short_addr); 897eab560e5SAlexander Aring if (ipv6_addr_equal(&tmp, ipaddr)) 898eab560e5SAlexander Aring lladdr_compress = true; 899eab560e5SAlexander Aring break; 900eab560e5SAlexander Aring default: 901eab560e5SAlexander Aring /* should never handled and filtered by 802154 6lowpan */ 902eab560e5SAlexander Aring WARN_ON_ONCE(1); 903eab560e5SAlexander Aring break; 904eab560e5SAlexander Aring } 905eab560e5SAlexander Aring 906eab560e5SAlexander Aring return lladdr_compress; 907eab560e5SAlexander Aring } 908eab560e5SAlexander Aring 909eab560e5SAlexander Aring static u8 lowpan_compress_addr_64(u8 **hc_ptr, const struct net_device *dev, 910eab560e5SAlexander Aring const struct in6_addr *ipaddr, 911c8a3e7ebSAlexander Aring const unsigned char *lladdr, bool sam) 9122c6bed7cSAlexander Aring { 913eab560e5SAlexander Aring u8 dam = LOWPAN_IPHC_DAM_01; 9142c6bed7cSAlexander Aring 915eab560e5SAlexander Aring switch (lowpan_dev(dev)->lltype) { 916eab560e5SAlexander Aring case LOWPAN_LLTYPE_IEEE802154: 917eab560e5SAlexander Aring if (lowpan_iphc_compress_802154_lladdr(ipaddr, lladdr)) { 918eab560e5SAlexander Aring dam = LOWPAN_IPHC_DAM_11; /* 0-bits */ 919eab560e5SAlexander Aring pr_debug("address compression 0 bits\n"); 920eab560e5SAlexander Aring goto out; 921eab560e5SAlexander Aring } 922eab560e5SAlexander Aring break; 923eab560e5SAlexander Aring default: 9242c6bed7cSAlexander Aring if (is_addr_mac_addr_based(ipaddr, lladdr)) { 925c8a3e7ebSAlexander Aring dam = LOWPAN_IPHC_DAM_11; /* 0-bits */ 9262c6bed7cSAlexander Aring pr_debug("address compression 0 bits\n"); 927eab560e5SAlexander Aring goto out; 928eab560e5SAlexander Aring } 929eab560e5SAlexander Aring break; 930eab560e5SAlexander Aring } 931eab560e5SAlexander Aring 932eab560e5SAlexander Aring if (lowpan_is_iid_16_bit_compressable(ipaddr)) { 9332c6bed7cSAlexander Aring /* compress IID to 16 bits xxxx::XXXX */ 93485c71240SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2); 935c8a3e7ebSAlexander Aring dam = LOWPAN_IPHC_DAM_10; /* 16-bits */ 9362c6bed7cSAlexander Aring raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)", 93784ca5e03SAlexander Aring *hc_ptr - 2, 2); 938eab560e5SAlexander Aring goto out; 939eab560e5SAlexander Aring } 940eab560e5SAlexander Aring 9412c6bed7cSAlexander Aring /* do not compress IID => xxxx::IID */ 94285c71240SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8); 9432c6bed7cSAlexander Aring raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)", 94484ca5e03SAlexander Aring *hc_ptr - 8, 8); 945eab560e5SAlexander Aring 946eab560e5SAlexander Aring out: 9472c6bed7cSAlexander Aring 948c8a3e7ebSAlexander Aring if (sam) 949c8a3e7ebSAlexander Aring return lowpan_iphc_dam_to_sam_value[dam]; 950c8a3e7ebSAlexander Aring else 951c8a3e7ebSAlexander Aring return dam; 9522c6bed7cSAlexander Aring } 9532c6bed7cSAlexander Aring 954b5af9bdbSAlexander Aring /* lowpan_iphc_get_tc - get the ECN + DCSP fields in hc format */ 955b5af9bdbSAlexander Aring static inline u8 lowpan_iphc_get_tc(const struct ipv6hdr *hdr) 956b5af9bdbSAlexander Aring { 957b5af9bdbSAlexander Aring u8 dscp, ecn; 958b5af9bdbSAlexander Aring 959b5af9bdbSAlexander Aring /* hdr->priority contains the higher bits of dscp, lower are part of 960b5af9bdbSAlexander Aring * flow_lbl[0]. Note ECN, DCSP is swapped in ipv6 hdr. 961b5af9bdbSAlexander Aring */ 962b5af9bdbSAlexander Aring dscp = (hdr->priority << 2) | ((hdr->flow_lbl[0] & 0xc0) >> 6); 963b5af9bdbSAlexander Aring /* ECN is at the two lower bits from first nibble of flow_lbl[0] */ 964b5af9bdbSAlexander Aring ecn = (hdr->flow_lbl[0] & 0x30); 965b5af9bdbSAlexander Aring /* for pretty debug output, also shift ecn to get the ecn value */ 966b5af9bdbSAlexander Aring pr_debug("ecn 0x%02x dscp 0x%02x\n", ecn >> 4, dscp); 967b5af9bdbSAlexander Aring /* ECN is at 0x30 now, shift it to have ECN + DCSP */ 968b5af9bdbSAlexander Aring return (ecn << 2) | dscp; 969b5af9bdbSAlexander Aring } 970b5af9bdbSAlexander Aring 971b5af9bdbSAlexander Aring /* lowpan_iphc_is_flow_lbl_zero - check if flow label is zero */ 972b5af9bdbSAlexander Aring static inline bool lowpan_iphc_is_flow_lbl_zero(const struct ipv6hdr *hdr) 973b5af9bdbSAlexander Aring { 974b5af9bdbSAlexander Aring return ((!(hdr->flow_lbl[0] & 0x0f)) && 975b5af9bdbSAlexander Aring !hdr->flow_lbl[1] && !hdr->flow_lbl[2]); 976b5af9bdbSAlexander Aring } 977b5af9bdbSAlexander Aring 978b5af9bdbSAlexander Aring /* lowpan_iphc_tf_compress - compress the traffic class which is set by 979b5af9bdbSAlexander Aring * ipv6hdr. Return the corresponding format identifier which is used. 980b5af9bdbSAlexander Aring */ 981b5af9bdbSAlexander Aring static u8 lowpan_iphc_tf_compress(u8 **hc_ptr, const struct ipv6hdr *hdr) 982b5af9bdbSAlexander Aring { 983b5af9bdbSAlexander Aring /* get ecn dscp data in a byteformat as: ECN(hi) + DSCP(lo) */ 984b5af9bdbSAlexander Aring u8 tc = lowpan_iphc_get_tc(hdr), tf[4], val; 985b5af9bdbSAlexander Aring 986b5af9bdbSAlexander Aring /* printout the traffic class in hc format */ 987b5af9bdbSAlexander Aring pr_debug("tc 0x%02x\n", tc); 988b5af9bdbSAlexander Aring 989b5af9bdbSAlexander Aring if (lowpan_iphc_is_flow_lbl_zero(hdr)) { 990b5af9bdbSAlexander Aring if (!tc) { 991b5af9bdbSAlexander Aring /* 11: Traffic Class and Flow Label are elided. */ 992b5af9bdbSAlexander Aring val = LOWPAN_IPHC_TF_11; 993b5af9bdbSAlexander Aring } else { 994b5af9bdbSAlexander Aring /* 10: ECN + DSCP (1 byte), Flow Label is elided. 995b5af9bdbSAlexander Aring * 996b5af9bdbSAlexander Aring * 0 1 2 3 4 5 6 7 997b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+ 998b5af9bdbSAlexander Aring * |ECN| DSCP | 999b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+ 1000b5af9bdbSAlexander Aring */ 1001b5af9bdbSAlexander Aring lowpan_push_hc_data(hc_ptr, &tc, sizeof(tc)); 1002b5af9bdbSAlexander Aring val = LOWPAN_IPHC_TF_10; 1003b5af9bdbSAlexander Aring } 1004b5af9bdbSAlexander Aring } else { 1005b5af9bdbSAlexander Aring /* check if dscp is zero, it's after the first two bit */ 1006b5af9bdbSAlexander Aring if (!(tc & 0x3f)) { 1007b5af9bdbSAlexander Aring /* 01: ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided 1008b5af9bdbSAlexander Aring * 1009b5af9bdbSAlexander Aring * 1 2 1010b5af9bdbSAlexander Aring * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 1011b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1012b5af9bdbSAlexander Aring * |ECN|rsv| Flow Label | 1013b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1014b5af9bdbSAlexander Aring */ 1015b5af9bdbSAlexander Aring memcpy(&tf[0], &hdr->flow_lbl[0], 3); 1016b5af9bdbSAlexander Aring /* zero the highest 4-bits, contains DCSP + ECN */ 1017b5af9bdbSAlexander Aring tf[0] &= ~0xf0; 1018b5af9bdbSAlexander Aring /* set ECN */ 1019b5af9bdbSAlexander Aring tf[0] |= (tc & 0xc0); 1020b5af9bdbSAlexander Aring 1021b5af9bdbSAlexander Aring lowpan_push_hc_data(hc_ptr, tf, 3); 1022b5af9bdbSAlexander Aring val = LOWPAN_IPHC_TF_01; 1023b5af9bdbSAlexander Aring } else { 1024b5af9bdbSAlexander Aring /* 00: ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) 1025b5af9bdbSAlexander Aring * 1026b5af9bdbSAlexander Aring * 1 2 3 1027b5af9bdbSAlexander Aring * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 1028b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1029b5af9bdbSAlexander Aring * |ECN| DSCP | rsv | Flow Label | 1030b5af9bdbSAlexander Aring * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1031b5af9bdbSAlexander Aring */ 1032b5af9bdbSAlexander Aring memcpy(&tf[0], &tc, sizeof(tc)); 1033b5af9bdbSAlexander Aring /* highest nibble of flow_lbl[0] is part of DSCP + ECN 1034b5af9bdbSAlexander Aring * which will be the 4-bit pad and will be filled with 1035b5af9bdbSAlexander Aring * zeros afterwards. 1036b5af9bdbSAlexander Aring */ 1037b5af9bdbSAlexander Aring memcpy(&tf[1], &hdr->flow_lbl[0], 3); 1038b5af9bdbSAlexander Aring /* zero the 4-bit pad, which is reserved */ 1039b5af9bdbSAlexander Aring tf[1] &= ~0xf0; 1040b5af9bdbSAlexander Aring 1041b5af9bdbSAlexander Aring lowpan_push_hc_data(hc_ptr, tf, 4); 1042b5af9bdbSAlexander Aring val = LOWPAN_IPHC_TF_00; 1043b5af9bdbSAlexander Aring } 1044b5af9bdbSAlexander Aring } 1045b5af9bdbSAlexander Aring 1046b5af9bdbSAlexander Aring return val; 1047b5af9bdbSAlexander Aring } 1048b5af9bdbSAlexander Aring 10495609c185SAlexander Aring static u8 lowpan_iphc_mcast_ctx_addr_compress(u8 **hc_ptr, 10505609c185SAlexander Aring const struct lowpan_iphc_ctx *ctx, 10515609c185SAlexander Aring const struct in6_addr *ipaddr) 10525609c185SAlexander Aring { 10535609c185SAlexander Aring u8 data[6]; 10545609c185SAlexander Aring 10555609c185SAlexander Aring /* flags/scope, reserved (RIID) */ 10565609c185SAlexander Aring memcpy(data, &ipaddr->s6_addr[1], 2); 10575609c185SAlexander Aring /* group ID */ 10585609c185SAlexander Aring memcpy(&data[1], &ipaddr->s6_addr[11], 4); 10595609c185SAlexander Aring lowpan_push_hc_data(hc_ptr, data, 6); 10605609c185SAlexander Aring 10615609c185SAlexander Aring return LOWPAN_IPHC_DAM_00; 10625609c185SAlexander Aring } 10635609c185SAlexander Aring 106409bf420fSAlexander Aring static u8 lowpan_iphc_mcast_addr_compress(u8 **hc_ptr, 106509bf420fSAlexander Aring const struct in6_addr *ipaddr) 106609bf420fSAlexander Aring { 106709bf420fSAlexander Aring u8 val; 106809bf420fSAlexander Aring 106909bf420fSAlexander Aring if (lowpan_is_mcast_addr_compressable8(ipaddr)) { 107009bf420fSAlexander Aring pr_debug("compressed to 1 octet\n"); 107109bf420fSAlexander Aring /* use last byte */ 107209bf420fSAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[15], 1); 107309bf420fSAlexander Aring val = LOWPAN_IPHC_DAM_11; 107409bf420fSAlexander Aring } else if (lowpan_is_mcast_addr_compressable32(ipaddr)) { 107509bf420fSAlexander Aring pr_debug("compressed to 4 octets\n"); 107609bf420fSAlexander Aring /* second byte + the last three */ 107709bf420fSAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1); 107809bf420fSAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[13], 3); 107909bf420fSAlexander Aring val = LOWPAN_IPHC_DAM_10; 108009bf420fSAlexander Aring } else if (lowpan_is_mcast_addr_compressable48(ipaddr)) { 108109bf420fSAlexander Aring pr_debug("compressed to 6 octets\n"); 108209bf420fSAlexander Aring /* second byte + the last five */ 108309bf420fSAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1); 108409bf420fSAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[11], 5); 108509bf420fSAlexander Aring val = LOWPAN_IPHC_DAM_01; 108609bf420fSAlexander Aring } else { 108709bf420fSAlexander Aring pr_debug("using full address\n"); 108809bf420fSAlexander Aring lowpan_push_hc_data(hc_ptr, ipaddr->s6_addr, 16); 108909bf420fSAlexander Aring val = LOWPAN_IPHC_DAM_00; 109009bf420fSAlexander Aring } 109109bf420fSAlexander Aring 109209bf420fSAlexander Aring return val; 109309bf420fSAlexander Aring } 109409bf420fSAlexander Aring 1095a6f77389SAlexander Aring int lowpan_header_compress(struct sk_buff *skb, const struct net_device *dev, 1096a6f77389SAlexander Aring const void *daddr, const void *saddr) 10972c6bed7cSAlexander Aring { 10985609c185SAlexander Aring u8 iphc0, iphc1, *hc_ptr, cid = 0; 10992c6bed7cSAlexander Aring struct ipv6hdr *hdr; 1100bf513fd6SAlexander Aring u8 head[LOWPAN_IPHC_MAX_HC_BUF_LEN] = {}; 11015609c185SAlexander Aring struct lowpan_iphc_ctx *dci, *sci, dci_entry, sci_entry; 11025609c185SAlexander Aring int ret, ipv6_daddr_type, ipv6_saddr_type; 11032c6bed7cSAlexander Aring 1104a6f77389SAlexander Aring if (skb->protocol != htons(ETH_P_IPV6)) 11052c6bed7cSAlexander Aring return -EINVAL; 11062c6bed7cSAlexander Aring 11072c6bed7cSAlexander Aring hdr = ipv6_hdr(skb); 110884ca5e03SAlexander Aring hc_ptr = head + 2; 11092c6bed7cSAlexander Aring 11102c6bed7cSAlexander Aring pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" 11112c6bed7cSAlexander Aring "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 11122c6bed7cSAlexander Aring hdr->version, ntohs(hdr->payload_len), hdr->nexthdr, 11132c6bed7cSAlexander Aring hdr->hop_limit, &hdr->daddr); 11142c6bed7cSAlexander Aring 11152c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb network header dump", 11162c6bed7cSAlexander Aring skb_network_header(skb), sizeof(struct ipv6hdr)); 11172c6bed7cSAlexander Aring 11183fd0202aSDavid S. Miller /* As we copy some bit-length fields, in the IPHC encoding bytes, 11192c6bed7cSAlexander Aring * we sometimes use |= 11202c6bed7cSAlexander Aring * If the field is 0, and the current bit value in memory is 1, 11212c6bed7cSAlexander Aring * this does not work. We therefore reset the IPHC encoding here 11222c6bed7cSAlexander Aring */ 11232c6bed7cSAlexander Aring iphc0 = LOWPAN_DISPATCH_IPHC; 11242c6bed7cSAlexander Aring iphc1 = 0; 11252c6bed7cSAlexander Aring 11267fc4cfdaSMarcel Holtmann raw_dump_table(__func__, "sending raw skb network uncompressed packet", 11272c6bed7cSAlexander Aring skb->data, skb->len); 11282c6bed7cSAlexander Aring 11295609c185SAlexander Aring ipv6_daddr_type = ipv6_addr_type(&hdr->daddr); 11302e4d60cbSAlexander Aring spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 11315609c185SAlexander Aring if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) 11325609c185SAlexander Aring dci = lowpan_iphc_ctx_get_by_mcast_addr(dev, &hdr->daddr); 11335609c185SAlexander Aring else 11345609c185SAlexander Aring dci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->daddr); 11355609c185SAlexander Aring if (dci) { 11365609c185SAlexander Aring memcpy(&dci_entry, dci, sizeof(*dci)); 11375609c185SAlexander Aring cid |= dci->id; 11385609c185SAlexander Aring } 11392e4d60cbSAlexander Aring spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 11405609c185SAlexander Aring 11412e4d60cbSAlexander Aring spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 11425609c185SAlexander Aring sci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->saddr); 11435609c185SAlexander Aring if (sci) { 11445609c185SAlexander Aring memcpy(&sci_entry, sci, sizeof(*sci)); 11455609c185SAlexander Aring cid |= (sci->id << 4); 11465609c185SAlexander Aring } 11472e4d60cbSAlexander Aring spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 11485609c185SAlexander Aring 11495609c185SAlexander Aring /* if cid is zero it will be compressed */ 11505609c185SAlexander Aring if (cid) { 11515609c185SAlexander Aring iphc1 |= LOWPAN_IPHC_CID; 11525609c185SAlexander Aring lowpan_push_hc_data(&hc_ptr, &cid, sizeof(cid)); 11535609c185SAlexander Aring } 11545609c185SAlexander Aring 1155b5af9bdbSAlexander Aring /* Traffic Class, Flow Label compression */ 1156b5af9bdbSAlexander Aring iphc0 |= lowpan_iphc_tf_compress(&hc_ptr, hdr); 11572c6bed7cSAlexander Aring 11582c6bed7cSAlexander Aring /* NOTE: payload length is always compressed */ 11592c6bed7cSAlexander Aring 1160cc6ed268SAlexander Aring /* Check if we provide the nhc format for nexthdr and compression 1161cc6ed268SAlexander Aring * functionality. If not nexthdr is handled inline and not compressed. 1162cc6ed268SAlexander Aring */ 1163607b0bd3SAlexander Aring ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr); 1164607b0bd3SAlexander Aring if (ret == -ENOENT) 1165607b0bd3SAlexander Aring lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr, 1166607b0bd3SAlexander Aring sizeof(hdr->nexthdr)); 1167607b0bd3SAlexander Aring else 1168c8a3e7ebSAlexander Aring iphc0 |= LOWPAN_IPHC_NH; 11692c6bed7cSAlexander Aring 11703fd0202aSDavid S. Miller /* Hop limit 11712c6bed7cSAlexander Aring * if 1: compress, encoding is 01 11722c6bed7cSAlexander Aring * if 64: compress, encoding is 10 11732c6bed7cSAlexander Aring * if 255: compress, encoding is 11 11742c6bed7cSAlexander Aring * else do not compress 11752c6bed7cSAlexander Aring */ 11762c6bed7cSAlexander Aring switch (hdr->hop_limit) { 11772c6bed7cSAlexander Aring case 1: 1178c8a3e7ebSAlexander Aring iphc0 |= LOWPAN_IPHC_HLIM_01; 11792c6bed7cSAlexander Aring break; 11802c6bed7cSAlexander Aring case 64: 1181c8a3e7ebSAlexander Aring iphc0 |= LOWPAN_IPHC_HLIM_10; 11822c6bed7cSAlexander Aring break; 11832c6bed7cSAlexander Aring case 255: 1184c8a3e7ebSAlexander Aring iphc0 |= LOWPAN_IPHC_HLIM_11; 11852c6bed7cSAlexander Aring break; 11862c6bed7cSAlexander Aring default: 118785c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit, 118885c71240SAlexander Aring sizeof(hdr->hop_limit)); 11892c6bed7cSAlexander Aring } 11902c6bed7cSAlexander Aring 11915609c185SAlexander Aring ipv6_saddr_type = ipv6_addr_type(&hdr->saddr); 11922c6bed7cSAlexander Aring /* source address compression */ 11935609c185SAlexander Aring if (ipv6_saddr_type == IPV6_ADDR_ANY) { 11942c6bed7cSAlexander Aring pr_debug("source address is unspecified, setting SAC\n"); 11952c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_SAC; 1196556a5bfcSAlexander Aring } else { 11975609c185SAlexander Aring if (sci) { 1198eab560e5SAlexander Aring iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev, 1199eab560e5SAlexander Aring &hdr->saddr, 12005609c185SAlexander Aring &sci_entry, saddr, 12015609c185SAlexander Aring true); 12025609c185SAlexander Aring iphc1 |= LOWPAN_IPHC_SAC; 12035609c185SAlexander Aring } else { 1204feb2add3SAlexander Aring if (ipv6_saddr_type & IPV6_ADDR_LINKLOCAL && 1205feb2add3SAlexander Aring lowpan_is_linklocal_zero_padded(hdr->saddr)) { 1206eab560e5SAlexander Aring iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev, 12075609c185SAlexander Aring &hdr->saddr, 1208c8a3e7ebSAlexander Aring saddr, true); 1209556a5bfcSAlexander Aring pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n", 1210556a5bfcSAlexander Aring &hdr->saddr, iphc1); 12112c6bed7cSAlexander Aring } else { 12122c6bed7cSAlexander Aring pr_debug("send the full source address\n"); 12135609c185SAlexander Aring lowpan_push_hc_data(&hc_ptr, 12145609c185SAlexander Aring hdr->saddr.s6_addr, 16); 12155609c185SAlexander Aring } 1216556a5bfcSAlexander Aring } 12172c6bed7cSAlexander Aring } 12182c6bed7cSAlexander Aring 12192c6bed7cSAlexander Aring /* destination address compression */ 12205609c185SAlexander Aring if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) { 12212c6bed7cSAlexander Aring pr_debug("destination address is multicast: "); 12228790404dSAlexander Aring iphc1 |= LOWPAN_IPHC_M; 12235609c185SAlexander Aring if (dci) { 12245609c185SAlexander Aring iphc1 |= lowpan_iphc_mcast_ctx_addr_compress(&hc_ptr, 12255609c185SAlexander Aring &dci_entry, 12265609c185SAlexander Aring &hdr->daddr); 12278790404dSAlexander Aring iphc1 |= LOWPAN_IPHC_DAC; 12285609c185SAlexander Aring } else { 12295609c185SAlexander Aring iphc1 |= lowpan_iphc_mcast_addr_compress(&hc_ptr, 12305609c185SAlexander Aring &hdr->daddr); 12315609c185SAlexander Aring } 12322c6bed7cSAlexander Aring } else { 12335609c185SAlexander Aring if (dci) { 1234eab560e5SAlexander Aring iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev, 1235eab560e5SAlexander Aring &hdr->daddr, 12365609c185SAlexander Aring &dci_entry, daddr, 12375609c185SAlexander Aring false); 12385609c185SAlexander Aring iphc1 |= LOWPAN_IPHC_DAC; 12395609c185SAlexander Aring } else { 1240feb2add3SAlexander Aring if (ipv6_daddr_type & IPV6_ADDR_LINKLOCAL && 1241feb2add3SAlexander Aring lowpan_is_linklocal_zero_padded(hdr->daddr)) { 1242eab560e5SAlexander Aring iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev, 12435609c185SAlexander Aring &hdr->daddr, 1244c8a3e7ebSAlexander Aring daddr, false); 12455609c185SAlexander Aring pr_debug("dest address unicast link-local %pI6c iphc1 0x%02x\n", 12465609c185SAlexander Aring &hdr->daddr, iphc1); 12472c6bed7cSAlexander Aring } else { 12485609c185SAlexander Aring pr_debug("dest address unicast %pI6c\n", 12495609c185SAlexander Aring &hdr->daddr); 12505609c185SAlexander Aring lowpan_push_hc_data(&hc_ptr, 12515609c185SAlexander Aring hdr->daddr.s6_addr, 16); 12525609c185SAlexander Aring } 12532c6bed7cSAlexander Aring } 12542c6bed7cSAlexander Aring } 12552c6bed7cSAlexander Aring 1256cc6ed268SAlexander Aring /* next header compression */ 1257c8a3e7ebSAlexander Aring if (iphc0 & LOWPAN_IPHC_NH) { 1258cc6ed268SAlexander Aring ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr); 1259cc6ed268SAlexander Aring if (ret < 0) 1260cc6ed268SAlexander Aring return ret; 1261cc6ed268SAlexander Aring } 12622c6bed7cSAlexander Aring 12632c6bed7cSAlexander Aring head[0] = iphc0; 12642c6bed7cSAlexander Aring head[1] = iphc1; 12652c6bed7cSAlexander Aring 12662c6bed7cSAlexander Aring skb_pull(skb, sizeof(struct ipv6hdr)); 12672c6bed7cSAlexander Aring skb_reset_transport_header(skb); 126884ca5e03SAlexander Aring memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head); 12692c6bed7cSAlexander Aring skb_reset_network_header(skb); 12702c6bed7cSAlexander Aring 127184ca5e03SAlexander Aring pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len); 12722c6bed7cSAlexander Aring 12732c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb data dump compressed", 12742c6bed7cSAlexander Aring skb->data, skb->len); 12752c6bed7cSAlexander Aring return 0; 12762c6bed7cSAlexander Aring } 12772c6bed7cSAlexander Aring EXPORT_SYMBOL_GPL(lowpan_header_compress); 1278