12c6bed7cSAlexander Aring /* 22c6bed7cSAlexander Aring * Copyright 2011, Siemens AG 32c6bed7cSAlexander Aring * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com> 42c6bed7cSAlexander Aring */ 52c6bed7cSAlexander Aring 62c6bed7cSAlexander Aring /* 72c6bed7cSAlexander Aring * Based on patches from Jon Smirl <jonsmirl@gmail.com> 82c6bed7cSAlexander Aring * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com> 92c6bed7cSAlexander Aring * 102c6bed7cSAlexander Aring * This program is free software; you can redistribute it and/or modify 112c6bed7cSAlexander Aring * it under the terms of the GNU General Public License version 2 122c6bed7cSAlexander Aring * as published by the Free Software Foundation. 132c6bed7cSAlexander Aring * 142c6bed7cSAlexander Aring * This program is distributed in the hope that it will be useful, 152c6bed7cSAlexander Aring * but WITHOUT ANY WARRANTY; without even the implied warranty of 162c6bed7cSAlexander Aring * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 172c6bed7cSAlexander Aring * GNU General Public License for more details. 182c6bed7cSAlexander Aring * 192c6bed7cSAlexander Aring * You should have received a copy of the GNU General Public License along 202c6bed7cSAlexander Aring * with this program; if not, write to the Free Software Foundation, Inc., 212c6bed7cSAlexander Aring * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 222c6bed7cSAlexander Aring */ 232c6bed7cSAlexander Aring 242c6bed7cSAlexander Aring /* Jon's code is based on 6lowpan implementation for Contiki which is: 252c6bed7cSAlexander Aring * Copyright (c) 2008, Swedish Institute of Computer Science. 262c6bed7cSAlexander Aring * All rights reserved. 272c6bed7cSAlexander Aring * 282c6bed7cSAlexander Aring * Redistribution and use in source and binary forms, with or without 292c6bed7cSAlexander Aring * modification, are permitted provided that the following conditions 302c6bed7cSAlexander Aring * are met: 312c6bed7cSAlexander Aring * 1. Redistributions of source code must retain the above copyright 322c6bed7cSAlexander Aring * notice, this list of conditions and the following disclaimer. 332c6bed7cSAlexander Aring * 2. Redistributions in binary form must reproduce the above copyright 342c6bed7cSAlexander Aring * notice, this list of conditions and the following disclaimer in the 352c6bed7cSAlexander Aring * documentation and/or other materials provided with the distribution. 362c6bed7cSAlexander Aring * 3. Neither the name of the Institute nor the names of its contributors 372c6bed7cSAlexander Aring * may be used to endorse or promote products derived from this software 382c6bed7cSAlexander Aring * without specific prior written permission. 392c6bed7cSAlexander Aring * 402c6bed7cSAlexander Aring * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND 412c6bed7cSAlexander Aring * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 422c6bed7cSAlexander Aring * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 432c6bed7cSAlexander Aring * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE 442c6bed7cSAlexander Aring * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 452c6bed7cSAlexander Aring * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 462c6bed7cSAlexander Aring * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 472c6bed7cSAlexander Aring * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 482c6bed7cSAlexander Aring * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 492c6bed7cSAlexander Aring * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 502c6bed7cSAlexander Aring * SUCH DAMAGE. 512c6bed7cSAlexander Aring */ 522c6bed7cSAlexander Aring 532c6bed7cSAlexander Aring #include <linux/bitops.h> 542c6bed7cSAlexander Aring #include <linux/if_arp.h> 552c6bed7cSAlexander Aring #include <linux/module.h> 562c6bed7cSAlexander Aring #include <linux/netdevice.h> 572c6bed7cSAlexander Aring #include <net/6lowpan.h> 582c6bed7cSAlexander Aring #include <net/ipv6.h> 592c6bed7cSAlexander Aring #include <net/af_ieee802154.h> 602c6bed7cSAlexander Aring 612c6bed7cSAlexander Aring /* 622c6bed7cSAlexander Aring * Uncompress address function for source and 632c6bed7cSAlexander Aring * destination address(non-multicast). 642c6bed7cSAlexander Aring * 652c6bed7cSAlexander Aring * address_mode is sam value or dam value. 662c6bed7cSAlexander Aring */ 672c6bed7cSAlexander Aring static int uncompress_addr(struct sk_buff *skb, 682c6bed7cSAlexander Aring struct in6_addr *ipaddr, const u8 address_mode, 692c6bed7cSAlexander Aring const u8 *lladdr, const u8 addr_type, 702c6bed7cSAlexander Aring const u8 addr_len) 712c6bed7cSAlexander Aring { 722c6bed7cSAlexander Aring bool fail; 732c6bed7cSAlexander Aring 742c6bed7cSAlexander Aring switch (address_mode) { 752c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_00: 762c6bed7cSAlexander Aring /* for global link addresses */ 772c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 782c6bed7cSAlexander Aring break; 792c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_01: 802c6bed7cSAlexander Aring /* fe:80::XXXX:XXXX:XXXX:XXXX */ 812c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 822c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 832c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); 842c6bed7cSAlexander Aring break; 852c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_02: 862c6bed7cSAlexander Aring /* fe:80::ff:fe00:XXXX */ 872c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 882c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 892c6bed7cSAlexander Aring ipaddr->s6_addr[11] = 0xFF; 902c6bed7cSAlexander Aring ipaddr->s6_addr[12] = 0xFE; 912c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); 922c6bed7cSAlexander Aring break; 932c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_03: 942c6bed7cSAlexander Aring fail = false; 952c6bed7cSAlexander Aring switch (addr_type) { 962c6bed7cSAlexander Aring case IEEE802154_ADDR_LONG: 972c6bed7cSAlexander Aring /* fe:80::XXXX:XXXX:XXXX:XXXX 982c6bed7cSAlexander Aring * \_________________/ 992c6bed7cSAlexander Aring * hwaddr 1002c6bed7cSAlexander Aring */ 1012c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 1022c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 1032c6bed7cSAlexander Aring memcpy(&ipaddr->s6_addr[8], lladdr, addr_len); 1042c6bed7cSAlexander Aring /* second bit-flip (Universe/Local) 1052c6bed7cSAlexander Aring * is done according RFC2464 1062c6bed7cSAlexander Aring */ 1072c6bed7cSAlexander Aring ipaddr->s6_addr[8] ^= 0x02; 1082c6bed7cSAlexander Aring break; 1092c6bed7cSAlexander Aring case IEEE802154_ADDR_SHORT: 1102c6bed7cSAlexander Aring /* fe:80::ff:fe00:XXXX 1112c6bed7cSAlexander Aring * \__/ 1122c6bed7cSAlexander Aring * short_addr 1132c6bed7cSAlexander Aring * 1142c6bed7cSAlexander Aring * Universe/Local bit is zero. 1152c6bed7cSAlexander Aring */ 1162c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFE; 1172c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x80; 1182c6bed7cSAlexander Aring ipaddr->s6_addr[11] = 0xFF; 1192c6bed7cSAlexander Aring ipaddr->s6_addr[12] = 0xFE; 1202c6bed7cSAlexander Aring ipaddr->s6_addr16[7] = htons(*((u16 *)lladdr)); 1212c6bed7cSAlexander Aring break; 1222c6bed7cSAlexander Aring default: 1232c6bed7cSAlexander Aring pr_debug("Invalid addr_type set\n"); 1242c6bed7cSAlexander Aring return -EINVAL; 1252c6bed7cSAlexander Aring } 1262c6bed7cSAlexander Aring break; 1272c6bed7cSAlexander Aring default: 1282c6bed7cSAlexander Aring pr_debug("Invalid address mode value: 0x%x\n", address_mode); 1292c6bed7cSAlexander Aring return -EINVAL; 1302c6bed7cSAlexander Aring } 1312c6bed7cSAlexander Aring 1322c6bed7cSAlexander Aring if (fail) { 1332c6bed7cSAlexander Aring pr_debug("Failed to fetch skb data\n"); 1342c6bed7cSAlexander Aring return -EIO; 1352c6bed7cSAlexander Aring } 1362c6bed7cSAlexander Aring 1372c6bed7cSAlexander Aring raw_dump_inline(NULL, "Reconstructed ipv6 addr is", 1382c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 1392c6bed7cSAlexander Aring 1402c6bed7cSAlexander Aring return 0; 1412c6bed7cSAlexander Aring } 1422c6bed7cSAlexander Aring 1432c6bed7cSAlexander Aring /* 1442c6bed7cSAlexander Aring * Uncompress address function for source context 1452c6bed7cSAlexander Aring * based address(non-multicast). 1462c6bed7cSAlexander Aring */ 1472c6bed7cSAlexander Aring static int uncompress_context_based_src_addr(struct sk_buff *skb, 1482c6bed7cSAlexander Aring struct in6_addr *ipaddr, 1492c6bed7cSAlexander Aring const u8 sam) 1502c6bed7cSAlexander Aring { 1512c6bed7cSAlexander Aring switch (sam) { 1522c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_00: 1532c6bed7cSAlexander Aring /* unspec address :: 1542c6bed7cSAlexander Aring * Do nothing, address is already :: 1552c6bed7cSAlexander Aring */ 1562c6bed7cSAlexander Aring break; 1572c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_01: 1582c6bed7cSAlexander Aring /* TODO */ 1592c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_02: 1602c6bed7cSAlexander Aring /* TODO */ 1612c6bed7cSAlexander Aring case LOWPAN_IPHC_ADDR_03: 1622c6bed7cSAlexander Aring /* TODO */ 1632c6bed7cSAlexander Aring netdev_warn(skb->dev, "SAM value 0x%x not supported\n", sam); 1642c6bed7cSAlexander Aring return -EINVAL; 1652c6bed7cSAlexander Aring default: 1662c6bed7cSAlexander Aring pr_debug("Invalid sam value: 0x%x\n", sam); 1672c6bed7cSAlexander Aring return -EINVAL; 1682c6bed7cSAlexander Aring } 1692c6bed7cSAlexander Aring 1702c6bed7cSAlexander Aring raw_dump_inline(NULL, 1712c6bed7cSAlexander Aring "Reconstructed context based ipv6 src addr is", 1722c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 1732c6bed7cSAlexander Aring 1742c6bed7cSAlexander Aring return 0; 1752c6bed7cSAlexander Aring } 1762c6bed7cSAlexander Aring 1772c6bed7cSAlexander Aring static int skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr, 1782c6bed7cSAlexander Aring struct net_device *dev, skb_delivery_cb deliver_skb) 1792c6bed7cSAlexander Aring { 1802c6bed7cSAlexander Aring struct sk_buff *new; 1812c6bed7cSAlexander Aring int stat; 1822c6bed7cSAlexander Aring 1832c6bed7cSAlexander Aring new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb), 1842c6bed7cSAlexander Aring GFP_ATOMIC); 1852c6bed7cSAlexander Aring kfree_skb(skb); 1862c6bed7cSAlexander Aring 1872c6bed7cSAlexander Aring if (!new) 1882c6bed7cSAlexander Aring return -ENOMEM; 1892c6bed7cSAlexander Aring 1902c6bed7cSAlexander Aring skb_push(new, sizeof(struct ipv6hdr)); 1912c6bed7cSAlexander Aring skb_reset_network_header(new); 1922c6bed7cSAlexander Aring skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr)); 1932c6bed7cSAlexander Aring 1942c6bed7cSAlexander Aring new->protocol = htons(ETH_P_IPV6); 1952c6bed7cSAlexander Aring new->pkt_type = PACKET_HOST; 1962c6bed7cSAlexander Aring new->dev = dev; 1972c6bed7cSAlexander Aring 1982c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb data dump before receiving", 1992c6bed7cSAlexander Aring new->data, new->len); 2002c6bed7cSAlexander Aring 2012c6bed7cSAlexander Aring stat = deliver_skb(new, dev); 2022c6bed7cSAlexander Aring 2032c6bed7cSAlexander Aring kfree_skb(new); 2042c6bed7cSAlexander Aring 2052c6bed7cSAlexander Aring return stat; 2062c6bed7cSAlexander Aring } 2072c6bed7cSAlexander Aring 2082c6bed7cSAlexander Aring /* Uncompress function for multicast destination address, 2092c6bed7cSAlexander Aring * when M bit is set. 2102c6bed7cSAlexander Aring */ 2112c6bed7cSAlexander Aring static int 2122c6bed7cSAlexander Aring lowpan_uncompress_multicast_daddr(struct sk_buff *skb, 2132c6bed7cSAlexander Aring struct in6_addr *ipaddr, 2142c6bed7cSAlexander Aring const u8 dam) 2152c6bed7cSAlexander Aring { 2162c6bed7cSAlexander Aring bool fail; 2172c6bed7cSAlexander Aring 2182c6bed7cSAlexander Aring switch (dam) { 2192c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_00: 2202c6bed7cSAlexander Aring /* 00: 128 bits. The full address 2212c6bed7cSAlexander Aring * is carried in-line. 2222c6bed7cSAlexander Aring */ 2232c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 2242c6bed7cSAlexander Aring break; 2252c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_01: 2262c6bed7cSAlexander Aring /* 01: 48 bits. The address takes 2272c6bed7cSAlexander Aring * the form ffXX::00XX:XXXX:XXXX. 2282c6bed7cSAlexander Aring */ 2292c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 2302c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 2312c6bed7cSAlexander Aring fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); 2322c6bed7cSAlexander Aring break; 2332c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_10: 2342c6bed7cSAlexander Aring /* 10: 32 bits. The address takes 2352c6bed7cSAlexander Aring * the form ffXX::00XX:XXXX. 2362c6bed7cSAlexander Aring */ 2372c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 2382c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 2392c6bed7cSAlexander Aring fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); 2402c6bed7cSAlexander Aring break; 2412c6bed7cSAlexander Aring case LOWPAN_IPHC_DAM_11: 2422c6bed7cSAlexander Aring /* 11: 8 bits. The address takes 2432c6bed7cSAlexander Aring * the form ff02::00XX. 2442c6bed7cSAlexander Aring */ 2452c6bed7cSAlexander Aring ipaddr->s6_addr[0] = 0xFF; 2462c6bed7cSAlexander Aring ipaddr->s6_addr[1] = 0x02; 2472c6bed7cSAlexander Aring fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); 2482c6bed7cSAlexander Aring break; 2492c6bed7cSAlexander Aring default: 2502c6bed7cSAlexander Aring pr_debug("DAM value has a wrong value: 0x%x\n", dam); 2512c6bed7cSAlexander Aring return -EINVAL; 2522c6bed7cSAlexander Aring } 2532c6bed7cSAlexander Aring 2542c6bed7cSAlexander Aring if (fail) { 2552c6bed7cSAlexander Aring pr_debug("Failed to fetch skb data\n"); 2562c6bed7cSAlexander Aring return -EIO; 2572c6bed7cSAlexander Aring } 2582c6bed7cSAlexander Aring 2592c6bed7cSAlexander Aring raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is", 2602c6bed7cSAlexander Aring ipaddr->s6_addr, 16); 2612c6bed7cSAlexander Aring 2622c6bed7cSAlexander Aring return 0; 2632c6bed7cSAlexander Aring } 2642c6bed7cSAlexander Aring 2652c6bed7cSAlexander Aring static int 2662c6bed7cSAlexander Aring uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh) 2672c6bed7cSAlexander Aring { 2682c6bed7cSAlexander Aring bool fail; 2692c6bed7cSAlexander Aring u8 tmp = 0, val = 0; 2702c6bed7cSAlexander Aring 2712c6bed7cSAlexander Aring if (!uh) 2722c6bed7cSAlexander Aring goto err; 2732c6bed7cSAlexander Aring 2748ec1d9beSAlexander Aring fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp)); 2752c6bed7cSAlexander Aring 2762c6bed7cSAlexander Aring if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) { 2772c6bed7cSAlexander Aring pr_debug("UDP header uncompression\n"); 2782c6bed7cSAlexander Aring switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { 2792c6bed7cSAlexander Aring case LOWPAN_NHC_UDP_CS_P_00: 2808ec1d9beSAlexander Aring fail |= lowpan_fetch_skb(skb, &uh->source, 2818ec1d9beSAlexander Aring sizeof(uh->source)); 2828ec1d9beSAlexander Aring fail |= lowpan_fetch_skb(skb, &uh->dest, 2838ec1d9beSAlexander Aring sizeof(uh->dest)); 2842c6bed7cSAlexander Aring break; 2852c6bed7cSAlexander Aring case LOWPAN_NHC_UDP_CS_P_01: 2868ec1d9beSAlexander Aring fail |= lowpan_fetch_skb(skb, &uh->source, 2878ec1d9beSAlexander Aring sizeof(uh->source)); 2888ec1d9beSAlexander Aring fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); 2892c6bed7cSAlexander Aring uh->dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); 2902c6bed7cSAlexander Aring break; 2912c6bed7cSAlexander Aring case LOWPAN_NHC_UDP_CS_P_10: 2928ec1d9beSAlexander Aring fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); 2932c6bed7cSAlexander Aring uh->source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); 2948ec1d9beSAlexander Aring fail |= lowpan_fetch_skb(skb, &uh->dest, 2958ec1d9beSAlexander Aring sizeof(uh->dest)); 2962c6bed7cSAlexander Aring break; 2972c6bed7cSAlexander Aring case LOWPAN_NHC_UDP_CS_P_11: 2988ec1d9beSAlexander Aring fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); 2992c6bed7cSAlexander Aring uh->source = htons(LOWPAN_NHC_UDP_4BIT_PORT + 3002c6bed7cSAlexander Aring (val >> 4)); 3012c6bed7cSAlexander Aring uh->dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + 3022c6bed7cSAlexander Aring (val & 0x0f)); 3032c6bed7cSAlexander Aring break; 3042c6bed7cSAlexander Aring default: 3052c6bed7cSAlexander Aring pr_debug("ERROR: unknown UDP format\n"); 3062c6bed7cSAlexander Aring goto err; 3072c6bed7cSAlexander Aring break; 3082c6bed7cSAlexander Aring } 3092c6bed7cSAlexander Aring 3102c6bed7cSAlexander Aring pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", 3112c6bed7cSAlexander Aring ntohs(uh->source), ntohs(uh->dest)); 3122c6bed7cSAlexander Aring 3132c6bed7cSAlexander Aring /* checksum */ 3142c6bed7cSAlexander Aring if (tmp & LOWPAN_NHC_UDP_CS_C) { 3152c6bed7cSAlexander Aring pr_debug_ratelimited("checksum elided currently not supported\n"); 3162c6bed7cSAlexander Aring goto err; 3172c6bed7cSAlexander Aring } else { 3188ec1d9beSAlexander Aring fail |= lowpan_fetch_skb(skb, &uh->check, 3198ec1d9beSAlexander Aring sizeof(uh->check)); 3202c6bed7cSAlexander Aring } 3212c6bed7cSAlexander Aring 3222c6bed7cSAlexander Aring /* 3232c6bed7cSAlexander Aring * UDP lenght needs to be infered from the lower layers 3242c6bed7cSAlexander Aring * here, we obtain the hint from the remaining size of the 3252c6bed7cSAlexander Aring * frame 3262c6bed7cSAlexander Aring */ 3272c6bed7cSAlexander Aring uh->len = htons(skb->len + sizeof(struct udphdr)); 3282c6bed7cSAlexander Aring pr_debug("uncompressed UDP length: src = %d", ntohs(uh->len)); 3292c6bed7cSAlexander Aring } else { 3302c6bed7cSAlexander Aring pr_debug("ERROR: unsupported NH format\n"); 3312c6bed7cSAlexander Aring goto err; 3322c6bed7cSAlexander Aring } 3332c6bed7cSAlexander Aring 3342c6bed7cSAlexander Aring if (fail) 3352c6bed7cSAlexander Aring goto err; 3362c6bed7cSAlexander Aring 3372c6bed7cSAlexander Aring return 0; 3382c6bed7cSAlexander Aring err: 3392c6bed7cSAlexander Aring return -EINVAL; 3402c6bed7cSAlexander Aring } 3412c6bed7cSAlexander Aring 3422c6bed7cSAlexander Aring /* TTL uncompression values */ 3432c6bed7cSAlexander Aring static const u8 lowpan_ttl_values[] = { 0, 1, 64, 255 }; 3442c6bed7cSAlexander Aring 3452c6bed7cSAlexander Aring int lowpan_process_data(struct sk_buff *skb, struct net_device *dev, 3462c6bed7cSAlexander Aring const u8 *saddr, const u8 saddr_type, const u8 saddr_len, 3472c6bed7cSAlexander Aring const u8 *daddr, const u8 daddr_type, const u8 daddr_len, 3482c6bed7cSAlexander Aring u8 iphc0, u8 iphc1, skb_delivery_cb deliver_skb) 3492c6bed7cSAlexander Aring { 3502c6bed7cSAlexander Aring struct ipv6hdr hdr = {}; 3512c6bed7cSAlexander Aring u8 tmp, num_context = 0; 3522c6bed7cSAlexander Aring int err; 3532c6bed7cSAlexander Aring 3542c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb data dump uncompressed", 3552c6bed7cSAlexander Aring skb->data, skb->len); 3562c6bed7cSAlexander Aring 3572c6bed7cSAlexander Aring /* another if the CID flag is set */ 3582c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_CID) { 3592c6bed7cSAlexander Aring pr_debug("CID flag is set, increase header with one\n"); 3604ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &num_context, sizeof(num_context))) 3612c6bed7cSAlexander Aring goto drop; 3622c6bed7cSAlexander Aring } 3632c6bed7cSAlexander Aring 3642c6bed7cSAlexander Aring hdr.version = 6; 3652c6bed7cSAlexander Aring 3662c6bed7cSAlexander Aring /* Traffic Class and Flow Label */ 3672c6bed7cSAlexander Aring switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) { 3682c6bed7cSAlexander Aring /* 3692c6bed7cSAlexander Aring * Traffic Class and FLow Label carried in-line 3702c6bed7cSAlexander Aring * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) 3712c6bed7cSAlexander Aring */ 3722c6bed7cSAlexander Aring case 0: /* 00b */ 3734ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &tmp, sizeof(tmp))) 3742c6bed7cSAlexander Aring goto drop; 3752c6bed7cSAlexander Aring 3762c6bed7cSAlexander Aring memcpy(&hdr.flow_lbl, &skb->data[0], 3); 3772c6bed7cSAlexander Aring skb_pull(skb, 3); 3782c6bed7cSAlexander Aring hdr.priority = ((tmp >> 2) & 0x0f); 3792c6bed7cSAlexander Aring hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) | 3802c6bed7cSAlexander Aring (hdr.flow_lbl[0] & 0x0f); 3812c6bed7cSAlexander Aring break; 3822c6bed7cSAlexander Aring /* 3832c6bed7cSAlexander Aring * Traffic class carried in-line 3842c6bed7cSAlexander Aring * ECN + DSCP (1 byte), Flow Label is elided 3852c6bed7cSAlexander Aring */ 3862c6bed7cSAlexander Aring case 2: /* 10b */ 3874ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &tmp, sizeof(tmp))) 3882c6bed7cSAlexander Aring goto drop; 3892c6bed7cSAlexander Aring 3902c6bed7cSAlexander Aring hdr.priority = ((tmp >> 2) & 0x0f); 3912c6bed7cSAlexander Aring hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30); 3922c6bed7cSAlexander Aring break; 3932c6bed7cSAlexander Aring /* 3942c6bed7cSAlexander Aring * Flow Label carried in-line 3952c6bed7cSAlexander Aring * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided 3962c6bed7cSAlexander Aring */ 3972c6bed7cSAlexander Aring case 1: /* 01b */ 3984ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &tmp, sizeof(tmp))) 3992c6bed7cSAlexander Aring goto drop; 4002c6bed7cSAlexander Aring 4012c6bed7cSAlexander Aring hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30); 4022c6bed7cSAlexander Aring memcpy(&hdr.flow_lbl[1], &skb->data[0], 2); 4032c6bed7cSAlexander Aring skb_pull(skb, 2); 4042c6bed7cSAlexander Aring break; 4052c6bed7cSAlexander Aring /* Traffic Class and Flow Label are elided */ 4062c6bed7cSAlexander Aring case 3: /* 11b */ 4072c6bed7cSAlexander Aring break; 4082c6bed7cSAlexander Aring default: 4092c6bed7cSAlexander Aring break; 4102c6bed7cSAlexander Aring } 4112c6bed7cSAlexander Aring 4122c6bed7cSAlexander Aring /* Next Header */ 4132c6bed7cSAlexander Aring if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) { 4142c6bed7cSAlexander Aring /* Next header is carried inline */ 4154ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr))) 4162c6bed7cSAlexander Aring goto drop; 4172c6bed7cSAlexander Aring 4182c6bed7cSAlexander Aring pr_debug("NH flag is set, next header carried inline: %02x\n", 4192c6bed7cSAlexander Aring hdr.nexthdr); 4202c6bed7cSAlexander Aring } 4212c6bed7cSAlexander Aring 4222c6bed7cSAlexander Aring /* Hop Limit */ 4232c6bed7cSAlexander Aring if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I) 4242c6bed7cSAlexander Aring hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03]; 4252c6bed7cSAlexander Aring else { 4264ebc960fSAlexander Aring if (lowpan_fetch_skb(skb, &hdr.hop_limit, 4274ebc960fSAlexander Aring sizeof(hdr.hop_limit))) 4282c6bed7cSAlexander Aring goto drop; 4292c6bed7cSAlexander Aring } 4302c6bed7cSAlexander Aring 4312c6bed7cSAlexander Aring /* Extract SAM to the tmp variable */ 4322c6bed7cSAlexander Aring tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03; 4332c6bed7cSAlexander Aring 4342c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_SAC) { 4352c6bed7cSAlexander Aring /* Source address context based uncompression */ 4362c6bed7cSAlexander Aring pr_debug("SAC bit is set. Handle context based source address.\n"); 4372c6bed7cSAlexander Aring err = uncompress_context_based_src_addr( 4382c6bed7cSAlexander Aring skb, &hdr.saddr, tmp); 4392c6bed7cSAlexander Aring } else { 4402c6bed7cSAlexander Aring /* Source address uncompression */ 4412c6bed7cSAlexander Aring pr_debug("source address stateless compression\n"); 4422c6bed7cSAlexander Aring err = uncompress_addr(skb, &hdr.saddr, tmp, saddr, 4432c6bed7cSAlexander Aring saddr_type, saddr_len); 4442c6bed7cSAlexander Aring } 4452c6bed7cSAlexander Aring 4462c6bed7cSAlexander Aring /* Check on error of previous branch */ 4472c6bed7cSAlexander Aring if (err) 4482c6bed7cSAlexander Aring goto drop; 4492c6bed7cSAlexander Aring 4502c6bed7cSAlexander Aring /* Extract DAM to the tmp variable */ 4512c6bed7cSAlexander Aring tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03; 4522c6bed7cSAlexander Aring 4532c6bed7cSAlexander Aring /* check for Multicast Compression */ 4542c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_M) { 4552c6bed7cSAlexander Aring if (iphc1 & LOWPAN_IPHC_DAC) { 4562c6bed7cSAlexander Aring pr_debug("dest: context-based mcast compression\n"); 4572c6bed7cSAlexander Aring /* TODO: implement this */ 4582c6bed7cSAlexander Aring } else { 4592c6bed7cSAlexander Aring err = lowpan_uncompress_multicast_daddr( 4602c6bed7cSAlexander Aring skb, &hdr.daddr, tmp); 4612c6bed7cSAlexander Aring if (err) 4622c6bed7cSAlexander Aring goto drop; 4632c6bed7cSAlexander Aring } 4642c6bed7cSAlexander Aring } else { 4652c6bed7cSAlexander Aring err = uncompress_addr(skb, &hdr.daddr, tmp, daddr, 4662c6bed7cSAlexander Aring daddr_type, daddr_len); 4672c6bed7cSAlexander Aring pr_debug("dest: stateless compression mode %d dest %pI6c\n", 4682c6bed7cSAlexander Aring tmp, &hdr.daddr); 4692c6bed7cSAlexander Aring if (err) 4702c6bed7cSAlexander Aring goto drop; 4712c6bed7cSAlexander Aring } 4722c6bed7cSAlexander Aring 4732c6bed7cSAlexander Aring /* UDP data uncompression */ 4742c6bed7cSAlexander Aring if (iphc0 & LOWPAN_IPHC_NH_C) { 4752c6bed7cSAlexander Aring struct udphdr uh; 4762c6bed7cSAlexander Aring struct sk_buff *new; 4772c6bed7cSAlexander Aring if (uncompress_udp_header(skb, &uh)) 4782c6bed7cSAlexander Aring goto drop; 4792c6bed7cSAlexander Aring 4802c6bed7cSAlexander Aring /* 4812c6bed7cSAlexander Aring * replace the compressed UDP head by the uncompressed UDP 4822c6bed7cSAlexander Aring * header 4832c6bed7cSAlexander Aring */ 4842c6bed7cSAlexander Aring new = skb_copy_expand(skb, sizeof(struct udphdr), 4852c6bed7cSAlexander Aring skb_tailroom(skb), GFP_ATOMIC); 4862c6bed7cSAlexander Aring kfree_skb(skb); 4872c6bed7cSAlexander Aring 4882c6bed7cSAlexander Aring if (!new) 4892c6bed7cSAlexander Aring return -ENOMEM; 4902c6bed7cSAlexander Aring 4912c6bed7cSAlexander Aring skb = new; 4922c6bed7cSAlexander Aring 4932c6bed7cSAlexander Aring skb_push(skb, sizeof(struct udphdr)); 4942c6bed7cSAlexander Aring skb_reset_transport_header(skb); 4952c6bed7cSAlexander Aring skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); 4962c6bed7cSAlexander Aring 4972c6bed7cSAlexander Aring raw_dump_table(__func__, "raw UDP header dump", 4982c6bed7cSAlexander Aring (u8 *)&uh, sizeof(uh)); 4992c6bed7cSAlexander Aring 5002c6bed7cSAlexander Aring hdr.nexthdr = UIP_PROTO_UDP; 5012c6bed7cSAlexander Aring } 5022c6bed7cSAlexander Aring 5032c6bed7cSAlexander Aring hdr.payload_len = htons(skb->len); 5042c6bed7cSAlexander Aring 5052c6bed7cSAlexander Aring pr_debug("skb headroom size = %d, data length = %d\n", 5062c6bed7cSAlexander Aring skb_headroom(skb), skb->len); 5072c6bed7cSAlexander Aring 5082c6bed7cSAlexander Aring pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t" 5092c6bed7cSAlexander Aring "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 5102c6bed7cSAlexander Aring hdr.version, ntohs(hdr.payload_len), hdr.nexthdr, 5112c6bed7cSAlexander Aring hdr.hop_limit, &hdr.daddr); 5122c6bed7cSAlexander Aring 5132c6bed7cSAlexander Aring raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, 5142c6bed7cSAlexander Aring sizeof(hdr)); 5152c6bed7cSAlexander Aring 5162c6bed7cSAlexander Aring return skb_deliver(skb, &hdr, dev, deliver_skb); 5172c6bed7cSAlexander Aring 5182c6bed7cSAlexander Aring drop: 5192c6bed7cSAlexander Aring kfree_skb(skb); 5202c6bed7cSAlexander Aring return -EINVAL; 5212c6bed7cSAlexander Aring } 5222c6bed7cSAlexander Aring EXPORT_SYMBOL_GPL(lowpan_process_data); 5232c6bed7cSAlexander Aring 52484ca5e03SAlexander Aring static u8 lowpan_compress_addr_64(u8 **hc_ptr, u8 shift, 5252c6bed7cSAlexander Aring const struct in6_addr *ipaddr, 5262c6bed7cSAlexander Aring const unsigned char *lladdr) 5272c6bed7cSAlexander Aring { 5282c6bed7cSAlexander Aring u8 val = 0; 5292c6bed7cSAlexander Aring 5302c6bed7cSAlexander Aring if (is_addr_mac_addr_based(ipaddr, lladdr)) { 5312c6bed7cSAlexander Aring val = 3; /* 0-bits */ 5322c6bed7cSAlexander Aring pr_debug("address compression 0 bits\n"); 5332c6bed7cSAlexander Aring } else if (lowpan_is_iid_16_bit_compressable(ipaddr)) { 5342c6bed7cSAlexander Aring /* compress IID to 16 bits xxxx::XXXX */ 53585c71240SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2); 5362c6bed7cSAlexander Aring val = 2; /* 16-bits */ 5372c6bed7cSAlexander Aring raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)", 53884ca5e03SAlexander Aring *hc_ptr - 2, 2); 5392c6bed7cSAlexander Aring } else { 5402c6bed7cSAlexander Aring /* do not compress IID => xxxx::IID */ 54185c71240SAlexander Aring lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8); 5422c6bed7cSAlexander Aring val = 1; /* 64-bits */ 5432c6bed7cSAlexander Aring raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)", 54484ca5e03SAlexander Aring *hc_ptr - 8, 8); 5452c6bed7cSAlexander Aring } 5462c6bed7cSAlexander Aring 5472c6bed7cSAlexander Aring return rol8(val, shift); 5482c6bed7cSAlexander Aring } 5492c6bed7cSAlexander Aring 55084ca5e03SAlexander Aring static void compress_udp_header(u8 **hc_ptr, struct sk_buff *skb) 5512c6bed7cSAlexander Aring { 5522c6bed7cSAlexander Aring struct udphdr *uh = udp_hdr(skb); 5532c6bed7cSAlexander Aring u8 tmp; 5542c6bed7cSAlexander Aring 5552c6bed7cSAlexander Aring if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) == 5562c6bed7cSAlexander Aring LOWPAN_NHC_UDP_4BIT_PORT) && 5572c6bed7cSAlexander Aring ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) == 5582c6bed7cSAlexander Aring LOWPAN_NHC_UDP_4BIT_PORT)) { 5592c6bed7cSAlexander Aring pr_debug("UDP header: both ports compression to 4 bits\n"); 5602c6bed7cSAlexander Aring /* compression value */ 5612c6bed7cSAlexander Aring tmp = LOWPAN_NHC_UDP_CS_P_11; 56284ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 5632c6bed7cSAlexander Aring /* source and destination port */ 5642c6bed7cSAlexander Aring tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT + 5652c6bed7cSAlexander Aring ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4); 56684ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 5672c6bed7cSAlexander Aring } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) == 5682c6bed7cSAlexander Aring LOWPAN_NHC_UDP_8BIT_PORT) { 5692c6bed7cSAlexander Aring pr_debug("UDP header: remove 8 bits of dest\n"); 5702c6bed7cSAlexander Aring /* compression value */ 5712c6bed7cSAlexander Aring tmp = LOWPAN_NHC_UDP_CS_P_01; 57284ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 5732c6bed7cSAlexander Aring /* source port */ 57484ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); 5752c6bed7cSAlexander Aring /* destination port */ 5762c6bed7cSAlexander Aring tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT; 57784ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 5782c6bed7cSAlexander Aring } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) == 5792c6bed7cSAlexander Aring LOWPAN_NHC_UDP_8BIT_PORT) { 5802c6bed7cSAlexander Aring pr_debug("UDP header: remove 8 bits of source\n"); 5812c6bed7cSAlexander Aring /* compression value */ 5822c6bed7cSAlexander Aring tmp = LOWPAN_NHC_UDP_CS_P_10; 58384ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 5842c6bed7cSAlexander Aring /* source port */ 5852c6bed7cSAlexander Aring tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT; 58684ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 5872c6bed7cSAlexander Aring /* destination port */ 58884ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); 5892c6bed7cSAlexander Aring } else { 5902c6bed7cSAlexander Aring pr_debug("UDP header: can't compress\n"); 5912c6bed7cSAlexander Aring /* compression value */ 5922c6bed7cSAlexander Aring tmp = LOWPAN_NHC_UDP_CS_P_00; 59384ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 5942c6bed7cSAlexander Aring /* source port */ 59584ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); 5962c6bed7cSAlexander Aring /* destination port */ 59784ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); 5982c6bed7cSAlexander Aring } 5992c6bed7cSAlexander Aring 6002c6bed7cSAlexander Aring /* checksum is always inline */ 60184ca5e03SAlexander Aring lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check)); 6022c6bed7cSAlexander Aring 6032c6bed7cSAlexander Aring /* skip the UDP header */ 6042c6bed7cSAlexander Aring skb_pull(skb, sizeof(struct udphdr)); 6052c6bed7cSAlexander Aring } 6062c6bed7cSAlexander Aring 6072c6bed7cSAlexander Aring int lowpan_header_compress(struct sk_buff *skb, struct net_device *dev, 6082c6bed7cSAlexander Aring unsigned short type, const void *_daddr, 6092c6bed7cSAlexander Aring const void *_saddr, unsigned int len) 6102c6bed7cSAlexander Aring { 61184ca5e03SAlexander Aring u8 tmp, iphc0, iphc1, *hc_ptr; 6122c6bed7cSAlexander Aring struct ipv6hdr *hdr; 6132c6bed7cSAlexander Aring u8 head[100] = {}; 614556a5bfcSAlexander Aring int addr_type; 6152c6bed7cSAlexander Aring 6162c6bed7cSAlexander Aring if (type != ETH_P_IPV6) 6172c6bed7cSAlexander Aring return -EINVAL; 6182c6bed7cSAlexander Aring 6192c6bed7cSAlexander Aring hdr = ipv6_hdr(skb); 62084ca5e03SAlexander Aring hc_ptr = head + 2; 6212c6bed7cSAlexander Aring 6222c6bed7cSAlexander Aring pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" 6232c6bed7cSAlexander Aring "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 6242c6bed7cSAlexander Aring hdr->version, ntohs(hdr->payload_len), hdr->nexthdr, 6252c6bed7cSAlexander Aring hdr->hop_limit, &hdr->daddr); 6262c6bed7cSAlexander Aring 6272c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb network header dump", 6282c6bed7cSAlexander Aring skb_network_header(skb), sizeof(struct ipv6hdr)); 6292c6bed7cSAlexander Aring 6302c6bed7cSAlexander Aring /* 6312c6bed7cSAlexander Aring * As we copy some bit-length fields, in the IPHC encoding bytes, 6322c6bed7cSAlexander Aring * we sometimes use |= 6332c6bed7cSAlexander Aring * If the field is 0, and the current bit value in memory is 1, 6342c6bed7cSAlexander Aring * this does not work. We therefore reset the IPHC encoding here 6352c6bed7cSAlexander Aring */ 6362c6bed7cSAlexander Aring iphc0 = LOWPAN_DISPATCH_IPHC; 6372c6bed7cSAlexander Aring iphc1 = 0; 6382c6bed7cSAlexander Aring 6392c6bed7cSAlexander Aring /* TODO: context lookup */ 6402c6bed7cSAlexander Aring 6412c6bed7cSAlexander Aring raw_dump_inline(__func__, "saddr", 6422c6bed7cSAlexander Aring (unsigned char *)_saddr, IEEE802154_ADDR_LEN); 6432c6bed7cSAlexander Aring raw_dump_inline(__func__, "daddr", 6442c6bed7cSAlexander Aring (unsigned char *)_daddr, IEEE802154_ADDR_LEN); 6452c6bed7cSAlexander Aring 6462c6bed7cSAlexander Aring raw_dump_table(__func__, 6472c6bed7cSAlexander Aring "sending raw skb network uncompressed packet", 6482c6bed7cSAlexander Aring skb->data, skb->len); 6492c6bed7cSAlexander Aring 6502c6bed7cSAlexander Aring /* 6512c6bed7cSAlexander Aring * Traffic class, flow label 6522c6bed7cSAlexander Aring * If flow label is 0, compress it. If traffic class is 0, compress it 6532c6bed7cSAlexander Aring * We have to process both in the same time as the offset of traffic 6542c6bed7cSAlexander Aring * class depends on the presence of version and flow label 6552c6bed7cSAlexander Aring */ 6562c6bed7cSAlexander Aring 65784ca5e03SAlexander Aring /* hc format of TC is ECN | DSCP , original one is DSCP | ECN */ 6582c6bed7cSAlexander Aring tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4); 6592c6bed7cSAlexander Aring tmp = ((tmp & 0x03) << 6) | (tmp >> 2); 6602c6bed7cSAlexander Aring 6612c6bed7cSAlexander Aring if (((hdr->flow_lbl[0] & 0x0F) == 0) && 6622c6bed7cSAlexander Aring (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) { 6632c6bed7cSAlexander Aring /* flow label can be compressed */ 6642c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_FL_C; 6652c6bed7cSAlexander Aring if ((hdr->priority == 0) && 6662c6bed7cSAlexander Aring ((hdr->flow_lbl[0] & 0xF0) == 0)) { 6672c6bed7cSAlexander Aring /* compress (elide) all */ 6682c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TC_C; 6692c6bed7cSAlexander Aring } else { 6702c6bed7cSAlexander Aring /* compress only the flow label */ 67184ca5e03SAlexander Aring *hc_ptr = tmp; 67284ca5e03SAlexander Aring hc_ptr += 1; 6732c6bed7cSAlexander Aring } 6742c6bed7cSAlexander Aring } else { 6752c6bed7cSAlexander Aring /* Flow label cannot be compressed */ 6762c6bed7cSAlexander Aring if ((hdr->priority == 0) && 6772c6bed7cSAlexander Aring ((hdr->flow_lbl[0] & 0xF0) == 0)) { 6782c6bed7cSAlexander Aring /* compress only traffic class */ 6792c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TC_C; 68084ca5e03SAlexander Aring *hc_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F); 68184ca5e03SAlexander Aring memcpy(hc_ptr + 1, &hdr->flow_lbl[1], 2); 68284ca5e03SAlexander Aring hc_ptr += 3; 6832c6bed7cSAlexander Aring } else { 6842c6bed7cSAlexander Aring /* compress nothing */ 68584ca5e03SAlexander Aring memcpy(hc_ptr, hdr, 4); 6862c6bed7cSAlexander Aring /* replace the top byte with new ECN | DSCP format */ 68784ca5e03SAlexander Aring *hc_ptr = tmp; 68884ca5e03SAlexander Aring hc_ptr += 4; 6892c6bed7cSAlexander Aring } 6902c6bed7cSAlexander Aring } 6912c6bed7cSAlexander Aring 6922c6bed7cSAlexander Aring /* NOTE: payload length is always compressed */ 6932c6bed7cSAlexander Aring 6942c6bed7cSAlexander Aring /* Next Header is compress if UDP */ 6952c6bed7cSAlexander Aring if (hdr->nexthdr == UIP_PROTO_UDP) 6962c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_NH_C; 6972c6bed7cSAlexander Aring 69885c71240SAlexander Aring if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) 69985c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr, 70085c71240SAlexander Aring sizeof(hdr->nexthdr)); 7012c6bed7cSAlexander Aring 7022c6bed7cSAlexander Aring /* 7032c6bed7cSAlexander Aring * Hop limit 7042c6bed7cSAlexander Aring * if 1: compress, encoding is 01 7052c6bed7cSAlexander Aring * if 64: compress, encoding is 10 7062c6bed7cSAlexander Aring * if 255: compress, encoding is 11 7072c6bed7cSAlexander Aring * else do not compress 7082c6bed7cSAlexander Aring */ 7092c6bed7cSAlexander Aring switch (hdr->hop_limit) { 7102c6bed7cSAlexander Aring case 1: 7112c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TTL_1; 7122c6bed7cSAlexander Aring break; 7132c6bed7cSAlexander Aring case 64: 7142c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TTL_64; 7152c6bed7cSAlexander Aring break; 7162c6bed7cSAlexander Aring case 255: 7172c6bed7cSAlexander Aring iphc0 |= LOWPAN_IPHC_TTL_255; 7182c6bed7cSAlexander Aring break; 7192c6bed7cSAlexander Aring default: 72085c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit, 72185c71240SAlexander Aring sizeof(hdr->hop_limit)); 7222c6bed7cSAlexander Aring } 7232c6bed7cSAlexander Aring 724556a5bfcSAlexander Aring addr_type = ipv6_addr_type(&hdr->saddr); 7252c6bed7cSAlexander Aring /* source address compression */ 726556a5bfcSAlexander Aring if (addr_type == IPV6_ADDR_ANY) { 7272c6bed7cSAlexander Aring pr_debug("source address is unspecified, setting SAC\n"); 7282c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_SAC; 729556a5bfcSAlexander Aring } else { 730556a5bfcSAlexander Aring if (addr_type & IPV6_ADDR_LINKLOCAL) { 73184ca5e03SAlexander Aring iphc1 |= lowpan_compress_addr_64(&hc_ptr, 732556a5bfcSAlexander Aring LOWPAN_IPHC_SAM_BIT, 733556a5bfcSAlexander Aring &hdr->saddr, _saddr); 734556a5bfcSAlexander Aring pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n", 735556a5bfcSAlexander Aring &hdr->saddr, iphc1); 7362c6bed7cSAlexander Aring } else { 7372c6bed7cSAlexander Aring pr_debug("send the full source address\n"); 738556a5bfcSAlexander Aring lowpan_push_hc_data(&hc_ptr, hdr->saddr.s6_addr, 16); 739556a5bfcSAlexander Aring } 7402c6bed7cSAlexander Aring } 7412c6bed7cSAlexander Aring 742556a5bfcSAlexander Aring addr_type = ipv6_addr_type(&hdr->daddr); 7432c6bed7cSAlexander Aring /* destination address compression */ 744556a5bfcSAlexander Aring if (addr_type & IPV6_ADDR_MULTICAST) { 7452c6bed7cSAlexander Aring pr_debug("destination address is multicast: "); 7462c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_M; 7472c6bed7cSAlexander Aring if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) { 7482c6bed7cSAlexander Aring pr_debug("compressed to 1 octet\n"); 7492c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_11; 7502c6bed7cSAlexander Aring /* use last byte */ 75185c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 75285c71240SAlexander Aring &hdr->daddr.s6_addr[15], 1); 7532c6bed7cSAlexander Aring } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) { 7542c6bed7cSAlexander Aring pr_debug("compressed to 4 octets\n"); 7552c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_10; 7562c6bed7cSAlexander Aring /* second byte + the last three */ 75785c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 75885c71240SAlexander Aring &hdr->daddr.s6_addr[1], 1); 75985c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 76085c71240SAlexander Aring &hdr->daddr.s6_addr[13], 3); 7612c6bed7cSAlexander Aring } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) { 7622c6bed7cSAlexander Aring pr_debug("compressed to 6 octets\n"); 7632c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_01; 7642c6bed7cSAlexander Aring /* second byte + the last five */ 76585c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 76685c71240SAlexander Aring &hdr->daddr.s6_addr[1], 1); 76785c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, 76885c71240SAlexander Aring &hdr->daddr.s6_addr[11], 5); 7692c6bed7cSAlexander Aring } else { 7702c6bed7cSAlexander Aring pr_debug("using full address\n"); 7712c6bed7cSAlexander Aring iphc1 |= LOWPAN_IPHC_DAM_00; 77285c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, hdr->daddr.s6_addr, 16); 7732c6bed7cSAlexander Aring } 7742c6bed7cSAlexander Aring } else { 775556a5bfcSAlexander Aring if (addr_type & IPV6_ADDR_LINKLOCAL) { 7762c6bed7cSAlexander Aring /* TODO: context lookup */ 77784ca5e03SAlexander Aring iphc1 |= lowpan_compress_addr_64(&hc_ptr, 7782c6bed7cSAlexander Aring LOWPAN_IPHC_DAM_BIT, &hdr->daddr, _daddr); 7792c6bed7cSAlexander Aring pr_debug("dest address unicast link-local %pI6c " 7802c6bed7cSAlexander Aring "iphc1 0x%02x\n", &hdr->daddr, iphc1); 7812c6bed7cSAlexander Aring } else { 7822c6bed7cSAlexander Aring pr_debug("dest address unicast %pI6c\n", &hdr->daddr); 78385c71240SAlexander Aring lowpan_push_hc_data(&hc_ptr, hdr->daddr.s6_addr, 16); 7842c6bed7cSAlexander Aring } 7852c6bed7cSAlexander Aring } 7862c6bed7cSAlexander Aring 7872c6bed7cSAlexander Aring /* UDP header compression */ 7882c6bed7cSAlexander Aring if (hdr->nexthdr == UIP_PROTO_UDP) 78984ca5e03SAlexander Aring compress_udp_header(&hc_ptr, skb); 7902c6bed7cSAlexander Aring 7912c6bed7cSAlexander Aring head[0] = iphc0; 7922c6bed7cSAlexander Aring head[1] = iphc1; 7932c6bed7cSAlexander Aring 7942c6bed7cSAlexander Aring skb_pull(skb, sizeof(struct ipv6hdr)); 7952c6bed7cSAlexander Aring skb_reset_transport_header(skb); 79684ca5e03SAlexander Aring memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head); 7972c6bed7cSAlexander Aring skb_reset_network_header(skb); 7982c6bed7cSAlexander Aring 79984ca5e03SAlexander Aring pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len); 8002c6bed7cSAlexander Aring 8012c6bed7cSAlexander Aring raw_dump_table(__func__, "raw skb data dump compressed", 8022c6bed7cSAlexander Aring skb->data, skb->len); 8032c6bed7cSAlexander Aring return 0; 8042c6bed7cSAlexander Aring } 8052c6bed7cSAlexander Aring EXPORT_SYMBOL_GPL(lowpan_header_compress); 8062c6bed7cSAlexander Aring 8072c6bed7cSAlexander Aring MODULE_LICENSE("GPL"); 808