net/6lowpan/iphc.c

   1 /*
   2  * Copyright 2011, Siemens AG
   3  * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
   4  */
   5
   6 /* Based on patches from Jon Smirl <jonsmirl@gmail.com>
   7  * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License version 2
  11  * as published by the Free Software Foundation.
  12  *
  13  * This program is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  */
  19
  20 /* Jon's code is based on 6lowpan implementation for Contiki which is:
  21  * Copyright (c) 2008, Swedish Institute of Computer Science.
  22  * All rights reserved.
  23  *
  24  * Redistribution and use in source and binary forms, with or without
  25  * modification, are permitted provided that the following conditions
  26  * are met:
  27  * 1. Redistributions of source code must retain the above copyright
  28  *    notice, this list of conditions and the following disclaimer.
  29  * 2. Redistributions in binary form must reproduce the above copyright
  30  *    notice, this list of conditions and the following disclaimer in the
  31  *    documentation and/or other materials provided with the distribution.
  32  * 3. Neither the name of the Institute nor the names of its contributors
  33  *    may be used to endorse or promote products derived from this software
  34  *    without specific prior written permission.
  35  *
  36  * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
  37  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  38  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  39  * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
  40  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  41  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  42  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  43  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  44  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  45  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  46  * SUCH DAMAGE.
  47  */
  48
  49 #include <linux/bitops.h>
  50 #include <linux/if_arp.h>
  51 #include <linux/netdevice.h>
  52
  53 #include <net/6lowpan.h>
  54 #include <net/ipv6.h>
  55
  56 /* special link-layer handling */
  57 #include <net/mac802154.h>
  58
  59 #include "6lowpan_i.h"
  60 #include "nhc.h"
  61
  62 /* Values of fields within the IPHC encoding first byte */
  63 #define LOWPAN_IPHC_TF_MASK     0x18
  64 #define LOWPAN_IPHC_TF_00       0x00
  65 #define LOWPAN_IPHC_TF_01       0x08
  66 #define LOWPAN_IPHC_TF_10       0x10
  67 #define LOWPAN_IPHC_TF_11       0x18
  68
  69 #define LOWPAN_IPHC_NH          0x04
  70
  71 #define LOWPAN_IPHC_HLIM_MASK   0x03
  72 #define LOWPAN_IPHC_HLIM_00     0x00
  73 #define LOWPAN_IPHC_HLIM_01     0x01
  74 #define LOWPAN_IPHC_HLIM_10     0x02
  75 #define LOWPAN_IPHC_HLIM_11     0x03
  76
  77 /* Values of fields within the IPHC encoding second byte */
  78 #define LOWPAN_IPHC_CID         0x80
  79
  80 #define LOWPAN_IPHC_SAC         0x40
  81
  82 #define LOWPAN_IPHC_SAM_MASK    0x30
  83 #define LOWPAN_IPHC_SAM_00      0x00
  84 #define LOWPAN_IPHC_SAM_01      0x10
  85 #define LOWPAN_IPHC_SAM_10      0x20
  86 #define LOWPAN_IPHC_SAM_11      0x30
  87
  88 #define LOWPAN_IPHC_M           0x08
  89
  90 #define LOWPAN_IPHC_DAC         0x04
  91
  92 #define LOWPAN_IPHC_DAM_MASK    0x03
  93 #define LOWPAN_IPHC_DAM_00      0x00
  94 #define LOWPAN_IPHC_DAM_01      0x01
  95 #define LOWPAN_IPHC_DAM_10      0x02
  96 #define LOWPAN_IPHC_DAM_11      0x03
  97
  98 /* ipv6 address based on mac
  99  * second bit-flip (Universe/Local) is done according RFC2464
 100  */
 101 #define is_addr_mac_addr_based(a, m) \
 102         ((((a)->s6_addr[8])  == (((m)[0]) ^ 0x02)) &&   \
 103          (((a)->s6_addr[9])  == (m)[1]) &&              \
 104          (((a)->s6_addr[10]) == (m)[2]) &&              \
 105          (((a)->s6_addr[11]) == (m)[3]) &&              \
 106          (((a)->s6_addr[12]) == (m)[4]) &&              \
 107          (((a)->s6_addr[13]) == (m)[5]) &&              \
 108          (((a)->s6_addr[14]) == (m)[6]) &&              \
 109          (((a)->s6_addr[15]) == (m)[7]))
 110
 111 /* check whether we can compress the IID to 16 bits,
 112  * it's possible for unicast addresses with first 49 bits are zero only.
 113  */
 114 #define lowpan_is_iid_16_bit_compressable(a)    \
 115         ((((a)->s6_addr16[4]) == 0) &&          \
 116          (((a)->s6_addr[10]) == 0) &&           \
 117          (((a)->s6_addr[11]) == 0xff) &&        \
 118          (((a)->s6_addr[12]) == 0xfe) &&        \
 119          (((a)->s6_addr[13]) == 0))
 120
 121 /* check whether the 112-bit gid of the multicast address is mappable to: */
 122
 123 /* 48 bits, FFXX::00XX:XXXX:XXXX */
 124 #define lowpan_is_mcast_addr_compressable48(a)  \
 125         ((((a)->s6_addr16[1]) == 0) &&          \
 126          (((a)->s6_addr16[2]) == 0) &&          \
 127          (((a)->s6_addr16[3]) == 0) &&          \
 128          (((a)->s6_addr16[4]) == 0) &&          \
 129          (((a)->s6_addr[10]) == 0))
 130
 131 /* 32 bits, FFXX::00XX:XXXX */
 132 #define lowpan_is_mcast_addr_compressable32(a)  \
 133         ((((a)->s6_addr16[1]) == 0) &&          \
 134          (((a)->s6_addr16[2]) == 0) &&          \
 135          (((a)->s6_addr16[3]) == 0) &&          \
 136          (((a)->s6_addr16[4]) == 0) &&          \
 137          (((a)->s6_addr16[5]) == 0) &&          \
 138          (((a)->s6_addr[12]) == 0))
 139
 140 /* 8 bits, FF02::00XX */
 141 #define lowpan_is_mcast_addr_compressable8(a)   \
 142         ((((a)->s6_addr[1])  == 2) &&           \
 143          (((a)->s6_addr16[1]) == 0) &&          \
 144          (((a)->s6_addr16[2]) == 0) &&          \
 145          (((a)->s6_addr16[3]) == 0) &&          \
 146          (((a)->s6_addr16[4]) == 0) &&          \
 147          (((a)->s6_addr16[5]) == 0) &&          \
 148          (((a)->s6_addr16[6]) == 0) &&          \
 149          (((a)->s6_addr[14]) == 0))
 150
 151 #define lowpan_is_linklocal_zero_padded(a)      \
 152         (!(hdr->saddr.s6_addr[1] & 0x3f) &&     \
 153          !hdr->saddr.s6_addr16[1] &&            \
 154          !hdr->saddr.s6_addr32[1])
 155
 156 #define LOWPAN_IPHC_CID_DCI(cid)        (cid & 0x0f)
 157 #define LOWPAN_IPHC_CID_SCI(cid)        ((cid & 0xf0) >> 4)
 158
 159 static inline void iphc_uncompress_eui64_lladdr(struct in6_addr *ipaddr,
 160                                                 const void *lladdr)
 161 {
 162         /* fe:80::XXXX:XXXX:XXXX:XXXX
 163          *        \_________________/
 164          *              hwaddr
 165          */
 166         ipaddr->s6_addr[0] = 0xFE;
 167         ipaddr->s6_addr[1] = 0x80;
 168         memcpy(&ipaddr->s6_addr[8], lladdr, EUI64_ADDR_LEN);
 169         /* second bit-flip (Universe/Local)
 170          * is done according RFC2464
 171          */
 172         ipaddr->s6_addr[8] ^= 0x02;
 173 }
 174
 175 static inline void iphc_uncompress_802154_lladdr(struct in6_addr *ipaddr,
 176                                                  const void *lladdr)
 177 {
 178         const struct ieee802154_addr *addr = lladdr;
 179         u8 eui64[EUI64_ADDR_LEN] = { };
 180
 181         switch (addr->mode) {
 182         case IEEE802154_ADDR_LONG:
 183                 ieee802154_le64_to_be64(eui64, &addr->extended_addr);
 184                 iphc_uncompress_eui64_lladdr(ipaddr, eui64);
 185                 break;
 186         case IEEE802154_ADDR_SHORT:
 187                 /* fe:80::ff:fe00:XXXX
 188                  *                \__/
 189                  *             short_addr
 190                  *
 191                  * Universe/Local bit is zero.
 192                  */
 193                 ipaddr->s6_addr[0] = 0xFE;
 194                 ipaddr->s6_addr[1] = 0x80;
 195                 ipaddr->s6_addr[11] = 0xFF;
 196                 ipaddr->s6_addr[12] = 0xFE;
 197                 ieee802154_le16_to_be16(&ipaddr->s6_addr16[7],
 198                                         &addr->short_addr);
 199                 break;
 200         default:
 201                 /* should never handled and filtered by 802154 6lowpan */
 202                 WARN_ON_ONCE(1);
 203                 break;
 204         }
 205 }
 206
 207 static struct lowpan_iphc_ctx *
 208 lowpan_iphc_ctx_get_by_id(const struct net_device *dev, u8 id)
 209 {
 210         struct lowpan_iphc_ctx *ret = &lowpan_priv(dev)->ctx.table[id];
 211
 212         if (!lowpan_iphc_ctx_is_active(ret))
 213                 return NULL;
 214
 215         return ret;
 216 }
 217
 218 static struct lowpan_iphc_ctx *
 219 lowpan_iphc_ctx_get_by_addr(const struct net_device *dev,
 220                             const struct in6_addr *addr)
 221 {
 222         struct lowpan_iphc_ctx *table = lowpan_priv(dev)->ctx.table;
 223         struct lowpan_iphc_ctx *ret = NULL;
 224         struct in6_addr addr_pfx;
 225         u8 addr_plen;
 226         int i;
 227
 228         for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) {
 229                 /* Check if context is valid. A context that is not valid
 230                  * MUST NOT be used for compression.
 231                  */
 232                 if (!lowpan_iphc_ctx_is_active(&table[i]) ||
 233                     !lowpan_iphc_ctx_is_compression(&table[i]))
 234                         continue;
 235
 236                 ipv6_addr_prefix(&addr_pfx, addr, table[i].plen);
 237
 238                 /* if prefix len < 64, the remaining bits until 64th bit is
 239                  * zero. Otherwise we use table[i]->plen.
 240                  */
 241                 if (table[i].plen < 64)
 242                         addr_plen = 64;
 243                 else
 244                         addr_plen = table[i].plen;
 245
 246                 if (ipv6_prefix_equal(&addr_pfx, &table[i].pfx, addr_plen)) {
 247                         /* remember first match */
 248                         if (!ret) {
 249                                 ret = &table[i];
 250                                 continue;
 251                         }
 252
 253                         /* get the context with longest prefix len */
 254                         if (table[i].plen > ret->plen)
 255                                 ret = &table[i];
 256                 }
 257         }
 258
 259         return ret;
 260 }
 261
 262 static struct lowpan_iphc_ctx *
 263 lowpan_iphc_ctx_get_by_mcast_addr(const struct net_device *dev,
 264                                   const struct in6_addr *addr)
 265 {
 266         struct lowpan_iphc_ctx *table = lowpan_priv(dev)->ctx.table;
 267         struct lowpan_iphc_ctx *ret = NULL;
 268         struct in6_addr addr_mcast, network_pfx = {};
 269         int i;
 270
 271         /* init mcast address with  */
 272         memcpy(&addr_mcast, addr, sizeof(*addr));
 273
 274         for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) {
 275                 /* Check if context is valid. A context that is not valid
 276                  * MUST NOT be used for compression.
 277                  */
 278                 if (!lowpan_iphc_ctx_is_active(&table[i]) ||
 279                     !lowpan_iphc_ctx_is_compression(&table[i]))
 280                         continue;
 281
 282                 /* setting plen */
 283                 addr_mcast.s6_addr[3] = table[i].plen;
 284                 /* get network prefix to copy into multicast address */
 285                 ipv6_addr_prefix(&network_pfx, &table[i].pfx,
 286                                  table[i].plen);
 287                 /* setting network prefix */
 288                 memcpy(&addr_mcast.s6_addr[4], &network_pfx, 8);
 289
 290                 if (ipv6_addr_equal(addr, &addr_mcast)) {
 291                         ret = &table[i];
 292                         break;
 293                 }
 294         }
 295
 296         return ret;
 297 }
 298
 299 /* Uncompress address function for source and
 300  * destination address(non-multicast).
 301  *
 302  * address_mode is the masked value for sam or dam value
 303  */
 304 static int uncompress_addr(struct sk_buff *skb, const struct net_device *dev,
 305                            struct in6_addr *ipaddr, u8 address_mode,
 306                            const void *lladdr)
 307 {
 308         bool fail;
 309
 310         switch (address_mode) {
 311         /* SAM and DAM are the same here */
 312         case LOWPAN_IPHC_DAM_00:
 313                 /* for global link addresses */
 314                 fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
 315                 break;
 316         case LOWPAN_IPHC_SAM_01:
 317         case LOWPAN_IPHC_DAM_01:
 318                 /* fe:80::XXXX:XXXX:XXXX:XXXX */
 319                 ipaddr->s6_addr[0] = 0xFE;
 320                 ipaddr->s6_addr[1] = 0x80;
 321                 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
 322                 break;
 323         case LOWPAN_IPHC_SAM_10:
 324         case LOWPAN_IPHC_DAM_10:
 325                 /* fe:80::ff:fe00:XXXX */
 326                 ipaddr->s6_addr[0] = 0xFE;
 327                 ipaddr->s6_addr[1] = 0x80;
 328                 ipaddr->s6_addr[11] = 0xFF;
 329                 ipaddr->s6_addr[12] = 0xFE;
 330                 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
 331                 break;
 332         case LOWPAN_IPHC_SAM_11:
 333         case LOWPAN_IPHC_DAM_11:
 334                 fail = false;
 335                 switch (lowpan_priv(dev)->lltype) {
 336                 case LOWPAN_LLTYPE_IEEE802154:
 337                         iphc_uncompress_802154_lladdr(ipaddr, lladdr);
 338                         break;
 339                 default:
 340                         iphc_uncompress_eui64_lladdr(ipaddr, lladdr);
 341                         break;
 342                 }
 343                 break;
 344         default:
 345                 pr_debug("Invalid address mode value: 0x%x\n", address_mode);
 346                 return -EINVAL;
 347         }
 348
 349         if (fail) {
 350                 pr_debug("Failed to fetch skb data\n");
 351                 return -EIO;
 352         }
 353
 354         raw_dump_inline(NULL, "Reconstructed ipv6 addr is",
 355                         ipaddr->s6_addr, 16);
 356
 357         return 0;
 358 }
 359
 360 /* Uncompress address function for source context
 361  * based address(non-multicast).
 362  */
 363 static int uncompress_ctx_addr(struct sk_buff *skb,
 364                                const struct net_device *dev,
 365                                const struct lowpan_iphc_ctx *ctx,
 366                                struct in6_addr *ipaddr, u8 address_mode,
 367                                const void *lladdr)
 368 {
 369         bool fail;
 370
 371         switch (address_mode) {
 372         /* SAM and DAM are the same here */
 373         case LOWPAN_IPHC_DAM_00:
 374                 fail = false;
 375                 /* SAM_00 -> unspec address ::
 376                  * Do nothing, address is already ::
 377                  *
 378                  * DAM 00 -> reserved should never occur.
 379                  */
 380                 break;
 381         case LOWPAN_IPHC_SAM_01:
 382         case LOWPAN_IPHC_DAM_01:
 383                 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
 384                 ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen);
 385                 break;
 386         case LOWPAN_IPHC_SAM_10:
 387         case LOWPAN_IPHC_DAM_10:
 388                 ipaddr->s6_addr[11] = 0xFF;
 389                 ipaddr->s6_addr[12] = 0xFE;
 390                 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
 391                 ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen);
 392                 break;
 393         case LOWPAN_IPHC_SAM_11:
 394         case LOWPAN_IPHC_DAM_11:
 395                 fail = false;
 396                 switch (lowpan_priv(dev)->lltype) {
 397                 case LOWPAN_LLTYPE_IEEE802154:
 398                         iphc_uncompress_802154_lladdr(ipaddr, lladdr);
 399                         break;
 400                 default:
 401                         iphc_uncompress_eui64_lladdr(ipaddr, lladdr);
 402                         break;
 403                 }
 404                 ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen);
 405                 break;
 406         default:
 407                 pr_debug("Invalid sam value: 0x%x\n", address_mode);
 408                 return -EINVAL;
 409         }
 410
 411         if (fail) {
 412                 pr_debug("Failed to fetch skb data\n");
 413                 return -EIO;
 414         }
 415
 416         raw_dump_inline(NULL,
 417                         "Reconstructed context based ipv6 src addr is",
 418                         ipaddr->s6_addr, 16);
 419
 420         return 0;
 421 }
 422
 423 /* Uncompress function for multicast destination address,
 424  * when M bit is set.
 425  */
 426 static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb,
 427                                              struct in6_addr *ipaddr,
 428                                              u8 address_mode)
 429 {
 430         bool fail;
 431
 432         switch (address_mode) {
 433         case LOWPAN_IPHC_DAM_00:
 434                 /* 00:  128 bits.  The full address
 435                  * is carried in-line.
 436                  */
 437                 fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
 438                 break;
 439         case LOWPAN_IPHC_DAM_01:
 440                 /* 01:  48 bits.  The address takes
 441                  * the form ffXX::00XX:XXXX:XXXX.
 442                  */
 443                 ipaddr->s6_addr[0] = 0xFF;
 444                 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
 445                 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5);
 446                 break;
 447         case LOWPAN_IPHC_DAM_10:
 448                 /* 10:  32 bits.  The address takes
 449                  * the form ffXX::00XX:XXXX.
 450                  */
 451                 ipaddr->s6_addr[0] = 0xFF;
 452                 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
 453                 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3);
 454                 break;
 455         case LOWPAN_IPHC_DAM_11:
 456                 /* 11:  8 bits.  The address takes
 457                  * the form ff02::00XX.
 458                  */
 459                 ipaddr->s6_addr[0] = 0xFF;
 460                 ipaddr->s6_addr[1] = 0x02;
 461                 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1);
 462                 break;
 463         default:
 464                 pr_debug("DAM value has a wrong value: 0x%x\n", address_mode);
 465                 return -EINVAL;
 466         }
 467
 468         if (fail) {
 469                 pr_debug("Failed to fetch skb data\n");
 470                 return -EIO;
 471         }
 472
 473         raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is",
 474                         ipaddr->s6_addr, 16);
 475
 476         return 0;
 477 }
 478
 479 static int lowpan_uncompress_multicast_ctx_daddr(struct sk_buff *skb,
 480                                                  struct lowpan_iphc_ctx *ctx,
 481                                                  struct in6_addr *ipaddr,
 482                                                  u8 address_mode)
 483 {
 484         struct in6_addr network_pfx = {};
 485         bool fail;
 486
 487         ipaddr->s6_addr[0] = 0xFF;
 488         fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 2);
 489         fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[12], 4);
 490         if (fail)
 491                 return -EIO;
 492
 493         /* take prefix_len and network prefix from the context */
 494         ipaddr->s6_addr[3] = ctx->plen;
 495         /* get network prefix to copy into multicast address */
 496         ipv6_addr_prefix(&network_pfx, &ctx->pfx, ctx->plen);
 497         /* setting network prefix */
 498         memcpy(&ipaddr->s6_addr[4], &network_pfx, 8);
 499
 500         return 0;
 501 }
 502
 503 /* get the ecn values from iphc tf format and set it to ipv6hdr */
 504 static inline void lowpan_iphc_tf_set_ecn(struct ipv6hdr *hdr, const u8 *tf)
 505 {
 506         /* get the two higher bits which is ecn */
 507         u8 ecn = tf[0] & 0xc0;
 508
 509         /* ECN takes 0x30 in hdr->flow_lbl[0] */
 510         hdr->flow_lbl[0] |= (ecn >> 2);
 511 }
 512
 513 /* get the dscp values from iphc tf format and set it to ipv6hdr */
 514 static inline void lowpan_iphc_tf_set_dscp(struct ipv6hdr *hdr, const u8 *tf)
 515 {
 516         /* DSCP is at place after ECN */
 517         u8 dscp = tf[0] & 0x3f;
 518
 519         /* The four highest bits need to be set at hdr->priority */
 520         hdr->priority |= ((dscp & 0x3c) >> 2);
 521         /* The two lower bits is part of hdr->flow_lbl[0] */
 522         hdr->flow_lbl[0] |= ((dscp & 0x03) << 6);
 523 }
 524
 525 /* get the flow label values from iphc tf format and set it to ipv6hdr */
 526 static inline void lowpan_iphc_tf_set_lbl(struct ipv6hdr *hdr, const u8 *lbl)
 527 {
 528         /* flow label is always some array started with lower nibble of
 529          * flow_lbl[0] and followed with two bytes afterwards. Inside inline
 530          * data the flow_lbl position can be different, which will be handled
 531          * by lbl pointer. E.g. case "01" vs "00" the traffic class is 8 bit
 532          * shifted, the different lbl pointer will handle that.
 533          *
 534          * The flow label will started at lower nibble of flow_lbl[0], the
 535          * higher nibbles are part of DSCP + ECN.
 536          */
 537         hdr->flow_lbl[0] |= lbl[0] & 0x0f;
 538         memcpy(&hdr->flow_lbl[1], &lbl[1], 2);
 539 }
 540
 541 /* lowpan_iphc_tf_decompress - decompress the traffic class.
 542  *      This function will return zero on success, a value lower than zero if
 543  *      failed.
 544  */
 545 static int lowpan_iphc_tf_decompress(struct sk_buff *skb, struct ipv6hdr *hdr,
 546                                      u8 val)
 547 {
 548         u8 tf[4];
 549
 550         /* Traffic Class and Flow Label */
 551         switch (val) {
 552         case LOWPAN_IPHC_TF_00:
 553                 /* ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) */
 554                 if (lowpan_fetch_skb(skb, tf, 4))
 555                         return -EINVAL;
 556
 557                 /*                      1                   2                   3
 558                  *  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
 559                  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 560                  * |ECN|   DSCP    |  rsv  |             Flow Label                |
 561                  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 562                  */
 563                 lowpan_iphc_tf_set_ecn(hdr, tf);
 564                 lowpan_iphc_tf_set_dscp(hdr, tf);
 565                 lowpan_iphc_tf_set_lbl(hdr, &tf[1]);
 566                 break;
 567         case LOWPAN_IPHC_TF_01:
 568                 /* ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided. */
 569                 if (lowpan_fetch_skb(skb, tf, 3))
 570                         return -EINVAL;
 571
 572                 /*                     1                   2
 573                  * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
 574                  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 575                  * |ECN|rsv|             Flow Label                |
 576                  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 577                  */
 578                 lowpan_iphc_tf_set_ecn(hdr, tf);
 579                 lowpan_iphc_tf_set_lbl(hdr, &tf[0]);
 580                 break;
 581         case LOWPAN_IPHC_TF_10:
 582                 /* ECN + DSCP (1 byte), Flow Label is elided. */
 583                 if (lowpan_fetch_skb(skb, tf, 1))
 584                         return -EINVAL;
 585
 586                 /*  0 1 2 3 4 5 6 7
 587                  * +-+-+-+-+-+-+-+-+
 588                  * |ECN|   DSCP    |
 589                  * +-+-+-+-+-+-+-+-+
 590                  */
 591                 lowpan_iphc_tf_set_ecn(hdr, tf);
 592                 lowpan_iphc_tf_set_dscp(hdr, tf);
 593                 break;
 594         case LOWPAN_IPHC_TF_11:
 595                 /* Traffic Class and Flow Label are elided */
 596                 break;
 597         default:
 598                 WARN_ON_ONCE(1);
 599                 return -EINVAL;
 600         }
 601
 602         return 0;
 603 }
 604
 605 /* TTL uncompression values */
 606 static const u8 lowpan_ttl_values[] = {
 607         [LOWPAN_IPHC_HLIM_01] = 1,
 608         [LOWPAN_IPHC_HLIM_10] = 64,
 609         [LOWPAN_IPHC_HLIM_11] = 255,
 610 };
 611
 612 int lowpan_header_decompress(struct sk_buff *skb, const struct net_device *dev,
 613                              const void *daddr, const void *saddr)
 614 {
 615         struct ipv6hdr hdr = {};
 616         struct lowpan_iphc_ctx *ci;
 617         u8 iphc0, iphc1, cid = 0;
 618         int err;
 619
 620         raw_dump_table(__func__, "raw skb data dump uncompressed",
 621                        skb->data, skb->len);
 622
 623         if (lowpan_fetch_skb(skb, &iphc0, sizeof(iphc0)) ||
 624             lowpan_fetch_skb(skb, &iphc1, sizeof(iphc1)))
 625                 return -EINVAL;
 626
 627         hdr.version = 6;
 628
 629         /* default CID = 0, another if the CID flag is set */
 630         if (iphc1 & LOWPAN_IPHC_CID) {
 631                 if (lowpan_fetch_skb(skb, &cid, sizeof(cid)))
 632                         return -EINVAL;
 633         }
 634
 635         err = lowpan_iphc_tf_decompress(skb, &hdr,
 636                                         iphc0 & LOWPAN_IPHC_TF_MASK);
 637         if (err < 0)
 638                 return err;
 639
 640         /* Next Header */
 641         if (!(iphc0 & LOWPAN_IPHC_NH)) {
 642                 /* Next header is carried inline */
 643                 if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr)))
 644                         return -EINVAL;
 645
 646                 pr_debug("NH flag is set, next header carried inline: %02x\n",
 647                          hdr.nexthdr);
 648         }
 649
 650         /* Hop Limit */
 651         if ((iphc0 & LOWPAN_IPHC_HLIM_MASK) != LOWPAN_IPHC_HLIM_00) {
 652                 hdr.hop_limit = lowpan_ttl_values[iphc0 & LOWPAN_IPHC_HLIM_MASK];
 653         } else {
 654                 if (lowpan_fetch_skb(skb, &hdr.hop_limit,
 655                                      sizeof(hdr.hop_limit)))
 656                         return -EINVAL;
 657         }
 658
 659         if (iphc1 & LOWPAN_IPHC_SAC) {
 660                 spin_lock_bh(&lowpan_priv(dev)->ctx.lock);
 661                 ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_SCI(cid));
 662                 if (!ci) {
 663                         spin_unlock_bh(&lowpan_priv(dev)->ctx.lock);
 664                         return -EINVAL;
 665                 }
 666
 667                 pr_debug("SAC bit is set. Handle context based source address.\n");
 668                 err = uncompress_ctx_addr(skb, dev, ci, &hdr.saddr,
 669                                           iphc1 & LOWPAN_IPHC_SAM_MASK, saddr);
 670                 spin_unlock_bh(&lowpan_priv(dev)->ctx.lock);
 671         } else {
 672                 /* Source address uncompression */
 673                 pr_debug("source address stateless compression\n");
 674                 err = uncompress_addr(skb, dev, &hdr.saddr,
 675                                       iphc1 & LOWPAN_IPHC_SAM_MASK, saddr);
 676         }
 677
 678         /* Check on error of previous branch */
 679         if (err)
 680                 return -EINVAL;
 681
 682         switch (iphc1 & (LOWPAN_IPHC_M | LOWPAN_IPHC_DAC)) {
 683         case LOWPAN_IPHC_M | LOWPAN_IPHC_DAC:
 684                 spin_lock_bh(&lowpan_priv(dev)->ctx.lock);
 685                 ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid));
 686                 if (!ci) {
 687                         spin_unlock_bh(&lowpan_priv(dev)->ctx.lock);
 688                         return -EINVAL;
 689                 }
 690
 691                 /* multicast with context */
 692                 pr_debug("dest: context-based mcast compression\n");
 693                 err = lowpan_uncompress_multicast_ctx_daddr(skb, ci,
 694                                                             &hdr.daddr,
 695                                                             iphc1 & LOWPAN_IPHC_DAM_MASK);
 696                 spin_unlock_bh(&lowpan_priv(dev)->ctx.lock);
 697                 break;
 698         case LOWPAN_IPHC_M:
 699                 /* multicast */
 700                 err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr,
 701                                                         iphc1 & LOWPAN_IPHC_DAM_MASK);
 702                 break;
 703         case LOWPAN_IPHC_DAC:
 704                 spin_lock_bh(&lowpan_priv(dev)->ctx.lock);
 705                 ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid));
 706                 if (!ci) {
 707                         spin_unlock_bh(&lowpan_priv(dev)->ctx.lock);
 708                         return -EINVAL;
 709                 }
 710
 711                 /* Destination address context based uncompression */
 712                 pr_debug("DAC bit is set. Handle context based destination address.\n");
 713                 err = uncompress_ctx_addr(skb, dev, ci, &hdr.daddr,
 714                                           iphc1 & LOWPAN_IPHC_DAM_MASK, daddr);
 715                 spin_unlock_bh(&lowpan_priv(dev)->ctx.lock);
 716                 break;
 717         default:
 718                 err = uncompress_addr(skb, dev, &hdr.daddr,
 719                                       iphc1 & LOWPAN_IPHC_DAM_MASK, daddr);
 720                 pr_debug("dest: stateless compression mode %d dest %pI6c\n",
 721                          iphc1 & LOWPAN_IPHC_DAM_MASK, &hdr.daddr);
 722                 break;
 723         }
 724
 725         if (err)
 726                 return -EINVAL;
 727
 728         /* Next header data uncompression */
 729         if (iphc0 & LOWPAN_IPHC_NH) {
 730                 err = lowpan_nhc_do_uncompression(skb, dev, &hdr);
 731                 if (err < 0)
 732                         return err;
 733         } else {
 734                 err = skb_cow(skb, sizeof(hdr));
 735                 if (unlikely(err))
 736                         return err;
 737         }
 738
 739         switch (lowpan_priv(dev)->lltype) {
 740         case LOWPAN_LLTYPE_IEEE802154:
 741                 if (lowpan_802154_cb(skb)->d_size)
 742                         hdr.payload_len = htons(lowpan_802154_cb(skb)->d_size -
 743                                                 sizeof(struct ipv6hdr));
 744                 else
 745                         hdr.payload_len = htons(skb->len);
 746                 break;
 747         default:
 748                 hdr.payload_len = htons(skb->len);
 749                 break;
 750         }
 751
 752         pr_debug("skb headroom size = %d, data length = %d\n",
 753                  skb_headroom(skb), skb->len);
 754
 755         pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n\t"
 756                  "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest    = %pI6c\n",
 757                 hdr.version, ntohs(hdr.payload_len), hdr.nexthdr,
 758                 hdr.hop_limit, &hdr.daddr);
 759
 760         skb_push(skb, sizeof(hdr));
 761         skb_reset_network_header(skb);
 762         skb_copy_to_linear_data(skb, &hdr, sizeof(hdr));
 763
 764         raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, sizeof(hdr));
 765
 766         return 0;
 767 }
 768 EXPORT_SYMBOL_GPL(lowpan_header_decompress);
 769
 770 static const u8 lowpan_iphc_dam_to_sam_value[] = {
 771         [LOWPAN_IPHC_DAM_00] = LOWPAN_IPHC_SAM_00,
 772         [LOWPAN_IPHC_DAM_01] = LOWPAN_IPHC_SAM_01,
 773         [LOWPAN_IPHC_DAM_10] = LOWPAN_IPHC_SAM_10,
 774         [LOWPAN_IPHC_DAM_11] = LOWPAN_IPHC_SAM_11,
 775 };
 776
 777 static u8 lowpan_compress_ctx_addr(u8 **hc_ptr, const struct in6_addr *ipaddr,
 778                                    const struct lowpan_iphc_ctx *ctx,
 779                                    const unsigned char *lladdr, bool sam)
 780 {
 781         struct in6_addr tmp = {};
 782         u8 dam;
 783
 784         /* check for SAM/DAM = 11 */
 785         memcpy(&tmp.s6_addr[8], lladdr, 8);
 786         /* second bit-flip (Universe/Local) is done according RFC2464 */
 787         tmp.s6_addr[8] ^= 0x02;
 788         /* context information are always used */
 789         ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
 790         if (ipv6_addr_equal(&tmp, ipaddr)) {
 791                 dam = LOWPAN_IPHC_DAM_11;
 792                 goto out;
 793         }
 794
 795         memset(&tmp, 0, sizeof(tmp));
 796         /* check for SAM/DAM = 10 */
 797         tmp.s6_addr[11] = 0xFF;
 798         tmp.s6_addr[12] = 0xFE;
 799         memcpy(&tmp.s6_addr[14], &ipaddr->s6_addr[14], 2);
 800         /* context information are always used */
 801         ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
 802         if (ipv6_addr_equal(&tmp, ipaddr)) {
 803                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[14], 2);
 804                 dam = LOWPAN_IPHC_DAM_10;
 805                 goto out;
 806         }
 807
 808         memset(&tmp, 0, sizeof(tmp));
 809         /* check for SAM/DAM = 01, should always match */
 810         memcpy(&tmp.s6_addr[8], &ipaddr->s6_addr[8], 8);
 811         /* context information are always used */
 812         ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
 813         if (ipv6_addr_equal(&tmp, ipaddr)) {
 814                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[8], 8);
 815                 dam = LOWPAN_IPHC_DAM_01;
 816                 goto out;
 817         }
 818
 819         WARN_ONCE(1, "context found but no address mode matched\n");
 820         return LOWPAN_IPHC_DAM_00;
 821 out:
 822
 823         if (sam)
 824                 return lowpan_iphc_dam_to_sam_value[dam];
 825         else
 826                 return dam;
 827 }
 828
 829 static u8 lowpan_compress_addr_64(u8 **hc_ptr, const struct in6_addr *ipaddr,
 830                                   const unsigned char *lladdr, bool sam)
 831 {
 832         u8 dam = LOWPAN_IPHC_DAM_00;
 833
 834         if (is_addr_mac_addr_based(ipaddr, lladdr)) {
 835                 dam = LOWPAN_IPHC_DAM_11; /* 0-bits */
 836                 pr_debug("address compression 0 bits\n");
 837         } else if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
 838                 /* compress IID to 16 bits xxxx::XXXX */
 839                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2);
 840                 dam = LOWPAN_IPHC_DAM_10; /* 16-bits */
 841                 raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)",
 842                                 *hc_ptr - 2, 2);
 843         } else {
 844                 /* do not compress IID => xxxx::IID */
 845                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8);
 846                 dam = LOWPAN_IPHC_DAM_01; /* 64-bits */
 847                 raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)",
 848                                 *hc_ptr - 8, 8);
 849         }
 850
 851         if (sam)
 852                 return lowpan_iphc_dam_to_sam_value[dam];
 853         else
 854                 return dam;
 855 }
 856
 857 /* lowpan_iphc_get_tc - get the ECN + DCSP fields in hc format */
 858 static inline u8 lowpan_iphc_get_tc(const struct ipv6hdr *hdr)
 859 {
 860         u8 dscp, ecn;
 861
 862         /* hdr->priority contains the higher bits of dscp, lower are part of
 863          * flow_lbl[0]. Note ECN, DCSP is swapped in ipv6 hdr.
 864          */
 865         dscp = (hdr->priority << 2) | ((hdr->flow_lbl[0] & 0xc0) >> 6);
 866         /* ECN is at the two lower bits from first nibble of flow_lbl[0] */
 867         ecn = (hdr->flow_lbl[0] & 0x30);
 868         /* for pretty debug output, also shift ecn to get the ecn value */
 869         pr_debug("ecn 0x%02x dscp 0x%02x\n", ecn >> 4, dscp);
 870         /* ECN is at 0x30 now, shift it to have ECN + DCSP */
 871         return (ecn << 2) | dscp;
 872 }
 873
 874 /* lowpan_iphc_is_flow_lbl_zero - check if flow label is zero */
 875 static inline bool lowpan_iphc_is_flow_lbl_zero(const struct ipv6hdr *hdr)
 876 {
 877         return ((!(hdr->flow_lbl[0] & 0x0f)) &&
 878                 !hdr->flow_lbl[1] && !hdr->flow_lbl[2]);
 879 }
 880
 881 /* lowpan_iphc_tf_compress - compress the traffic class which is set by
 882  *      ipv6hdr. Return the corresponding format identifier which is used.
 883  */
 884 static u8 lowpan_iphc_tf_compress(u8 **hc_ptr, const struct ipv6hdr *hdr)
 885 {
 886         /* get ecn dscp data in a byteformat as: ECN(hi) + DSCP(lo) */
 887         u8 tc = lowpan_iphc_get_tc(hdr), tf[4], val;
 888
 889         /* printout the traffic class in hc format */
 890         pr_debug("tc 0x%02x\n", tc);
 891
 892         if (lowpan_iphc_is_flow_lbl_zero(hdr)) {
 893                 if (!tc) {
 894                         /* 11:  Traffic Class and Flow Label are elided. */
 895                         val = LOWPAN_IPHC_TF_11;
 896                 } else {
 897                         /* 10:  ECN + DSCP (1 byte), Flow Label is elided.
 898                          *
 899                          *  0 1 2 3 4 5 6 7
 900                          * +-+-+-+-+-+-+-+-+
 901                          * |ECN|   DSCP    |
 902                          * +-+-+-+-+-+-+-+-+
 903                          */
 904                         lowpan_push_hc_data(hc_ptr, &tc, sizeof(tc));
 905                         val = LOWPAN_IPHC_TF_10;
 906                 }
 907         } else {
 908                 /* check if dscp is zero, it's after the first two bit */
 909                 if (!(tc & 0x3f)) {
 910                         /* 01:  ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
 911                          *
 912                          *                     1                   2
 913                          * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
 914                          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 915                          * |ECN|rsv|             Flow Label                |
 916                          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 917                          */
 918                         memcpy(&tf[0], &hdr->flow_lbl[0], 3);
 919                         /* zero the highest 4-bits, contains DCSP + ECN */
 920                         tf[0] &= ~0xf0;
 921                         /* set ECN */
 922                         tf[0] |= (tc & 0xc0);
 923
 924                         lowpan_push_hc_data(hc_ptr, tf, 3);
 925                         val = LOWPAN_IPHC_TF_01;
 926                 } else {
 927                         /* 00:  ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
 928                          *
 929                          *                      1                   2                   3
 930                          *  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
 931                          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 932                          * |ECN|   DSCP    |  rsv  |             Flow Label                |
 933                          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 934                          */
 935                         memcpy(&tf[0], &tc, sizeof(tc));
 936                         /* highest nibble of flow_lbl[0] is part of DSCP + ECN
 937                          * which will be the 4-bit pad and will be filled with
 938                          * zeros afterwards.
 939                          */
 940                         memcpy(&tf[1], &hdr->flow_lbl[0], 3);
 941                         /* zero the 4-bit pad, which is reserved */
 942                         tf[1] &= ~0xf0;
 943
 944                         lowpan_push_hc_data(hc_ptr, tf, 4);
 945                         val = LOWPAN_IPHC_TF_00;
 946                 }
 947         }
 948
 949         return val;
 950 }
 951
 952 static u8 lowpan_iphc_mcast_ctx_addr_compress(u8 **hc_ptr,
 953                                               const struct lowpan_iphc_ctx *ctx,
 954                                               const struct in6_addr *ipaddr)
 955 {
 956         u8 data[6];
 957
 958         /* flags/scope, reserved (RIID) */
 959         memcpy(data, &ipaddr->s6_addr[1], 2);
 960         /* group ID */
 961         memcpy(&data[1], &ipaddr->s6_addr[11], 4);
 962         lowpan_push_hc_data(hc_ptr, data, 6);
 963
 964         return LOWPAN_IPHC_DAM_00;
 965 }
 966
 967 static u8 lowpan_iphc_mcast_addr_compress(u8 **hc_ptr,
 968                                           const struct in6_addr *ipaddr)
 969 {
 970         u8 val;
 971
 972         if (lowpan_is_mcast_addr_compressable8(ipaddr)) {
 973                 pr_debug("compressed to 1 octet\n");
 974                 /* use last byte */
 975                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[15], 1);
 976                 val = LOWPAN_IPHC_DAM_11;
 977         } else if (lowpan_is_mcast_addr_compressable32(ipaddr)) {
 978                 pr_debug("compressed to 4 octets\n");
 979                 /* second byte + the last three */
 980                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1);
 981                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[13], 3);
 982                 val = LOWPAN_IPHC_DAM_10;
 983         } else if (lowpan_is_mcast_addr_compressable48(ipaddr)) {
 984                 pr_debug("compressed to 6 octets\n");
 985                 /* second byte + the last five */
 986                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1);
 987                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[11], 5);
 988                 val = LOWPAN_IPHC_DAM_01;
 989         } else {
 990                 pr_debug("using full address\n");
 991                 lowpan_push_hc_data(hc_ptr, ipaddr->s6_addr, 16);
 992                 val = LOWPAN_IPHC_DAM_00;
 993         }
 994
 995         return val;
 996 }
 997
 998 int lowpan_header_compress(struct sk_buff *skb, const struct net_device *dev,
 999                            const void *daddr, const void *saddr)
1000 {
1001         u8 iphc0, iphc1, *hc_ptr, cid = 0;
1002         struct ipv6hdr *hdr;
1003         u8 head[LOWPAN_IPHC_MAX_HC_BUF_LEN] = {};
1004         struct lowpan_iphc_ctx *dci, *sci, dci_entry, sci_entry;
1005         int ret, ipv6_daddr_type, ipv6_saddr_type;
1006
1007         if (skb->protocol != htons(ETH_P_IPV6))
1008                 return -EINVAL;
1009
1010         hdr = ipv6_hdr(skb);
1011         hc_ptr = head + 2;
1012
1013         pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n"
1014                  "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest    = %pI6c\n",
1015                  hdr->version, ntohs(hdr->payload_len), hdr->nexthdr,
1016                  hdr->hop_limit, &hdr->daddr);
1017
1018         raw_dump_table(__func__, "raw skb network header dump",
1019                        skb_network_header(skb), sizeof(struct ipv6hdr));
1020
1021         /* As we copy some bit-length fields, in the IPHC encoding bytes,
1022          * we sometimes use |=
1023          * If the field is 0, and the current bit value in memory is 1,
1024          * this does not work. We therefore reset the IPHC encoding here
1025          */
1026         iphc0 = LOWPAN_DISPATCH_IPHC;
1027         iphc1 = 0;
1028
1029         raw_dump_inline(__func__, "saddr", saddr, EUI64_ADDR_LEN);
1030         raw_dump_inline(__func__, "daddr", daddr, EUI64_ADDR_LEN);
1031
1032         raw_dump_table(__func__, "sending raw skb network uncompressed packet",
1033                        skb->data, skb->len);
1034
1035         ipv6_daddr_type = ipv6_addr_type(&hdr->daddr);
1036         spin_lock_bh(&lowpan_priv(dev)->ctx.lock);
1037         if (ipv6_daddr_type & IPV6_ADDR_MULTICAST)
1038                 dci = lowpan_iphc_ctx_get_by_mcast_addr(dev, &hdr->daddr);
1039         else
1040                 dci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->daddr);
1041         if (dci) {
1042                 memcpy(&dci_entry, dci, sizeof(*dci));
1043                 cid |= dci->id;
1044         }
1045         spin_unlock_bh(&lowpan_priv(dev)->ctx.lock);
1046
1047         spin_lock_bh(&lowpan_priv(dev)->ctx.lock);
1048         sci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->saddr);
1049         if (sci) {
1050                 memcpy(&sci_entry, sci, sizeof(*sci));
1051                 cid |= (sci->id << 4);
1052         }
1053         spin_unlock_bh(&lowpan_priv(dev)->ctx.lock);
1054
1055         /* if cid is zero it will be compressed */
1056         if (cid) {
1057                 iphc1 |= LOWPAN_IPHC_CID;
1058                 lowpan_push_hc_data(&hc_ptr, &cid, sizeof(cid));
1059         }
1060
1061         /* Traffic Class, Flow Label compression */
1062         iphc0 |= lowpan_iphc_tf_compress(&hc_ptr, hdr);
1063
1064         /* NOTE: payload length is always compressed */
1065
1066         /* Check if we provide the nhc format for nexthdr and compression
1067          * functionality. If not nexthdr is handled inline and not compressed.
1068          */
1069         ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr);
1070         if (ret == -ENOENT)
1071                 lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr,
1072                                     sizeof(hdr->nexthdr));
1073         else
1074                 iphc0 |= LOWPAN_IPHC_NH;
1075
1076         /* Hop limit
1077          * if 1:   compress, encoding is 01
1078          * if 64:  compress, encoding is 10
1079          * if 255: compress, encoding is 11
1080          * else do not compress
1081          */
1082         switch (hdr->hop_limit) {
1083         case 1:
1084                 iphc0 |= LOWPAN_IPHC_HLIM_01;
1085                 break;
1086         case 64:
1087                 iphc0 |= LOWPAN_IPHC_HLIM_10;
1088                 break;
1089         case 255:
1090                 iphc0 |= LOWPAN_IPHC_HLIM_11;
1091                 break;
1092         default:
1093                 lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit,
1094                                     sizeof(hdr->hop_limit));
1095         }
1096
1097         ipv6_saddr_type = ipv6_addr_type(&hdr->saddr);
1098         /* source address compression */
1099         if (ipv6_saddr_type == IPV6_ADDR_ANY) {
1100                 pr_debug("source address is unspecified, setting SAC\n");
1101                 iphc1 |= LOWPAN_IPHC_SAC;
1102         } else {
1103                 if (sci) {
1104                         iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, &hdr->saddr,
1105                                                           &sci_entry, saddr,
1106                                                           true);
1107                         iphc1 |= LOWPAN_IPHC_SAC;
1108                 } else {
1109                         if (ipv6_saddr_type & IPV6_ADDR_LINKLOCAL &&
1110                             lowpan_is_linklocal_zero_padded(hdr->saddr)) {
1111                                 iphc1 |= lowpan_compress_addr_64(&hc_ptr,
1112                                                                  &hdr->saddr,
1113                                                                  saddr, true);
1114                                 pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n",
1115                                          &hdr->saddr, iphc1);
1116                         } else {
1117                                 pr_debug("send the full source address\n");
1118                                 lowpan_push_hc_data(&hc_ptr,
1119                                                     hdr->saddr.s6_addr, 16);
1120                         }
1121                 }
1122         }
1123
1124         /* destination address compression */
1125         if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) {
1126                 pr_debug("destination address is multicast: ");
1127                 iphc1 |= LOWPAN_IPHC_M;
1128                 if (dci) {
1129                         iphc1 |= lowpan_iphc_mcast_ctx_addr_compress(&hc_ptr,
1130                                                                      &dci_entry,
1131                                                                      &hdr->daddr);
1132                         iphc1 |= LOWPAN_IPHC_DAC;
1133                 } else {
1134                         iphc1 |= lowpan_iphc_mcast_addr_compress(&hc_ptr,
1135                                                                  &hdr->daddr);
1136                 }
1137         } else {
1138                 if (dci) {
1139                         iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, &hdr->daddr,
1140                                                           &dci_entry, daddr,
1141                                                           false);
1142                         iphc1 |= LOWPAN_IPHC_DAC;
1143                 } else {
1144                         if (ipv6_daddr_type & IPV6_ADDR_LINKLOCAL &&
1145                             lowpan_is_linklocal_zero_padded(hdr->daddr)) {
1146                                 iphc1 |= lowpan_compress_addr_64(&hc_ptr,
1147                                                                  &hdr->daddr,
1148                                                                  daddr, false);
1149                                 pr_debug("dest address unicast link-local %pI6c iphc1 0x%02x\n",
1150                                          &hdr->daddr, iphc1);
1151                         } else {
1152                                 pr_debug("dest address unicast %pI6c\n",
1153                                          &hdr->daddr);
1154                                 lowpan_push_hc_data(&hc_ptr,
1155                                                     hdr->daddr.s6_addr, 16);
1156                         }
1157                 }
1158         }
1159
1160         /* next header compression */
1161         if (iphc0 & LOWPAN_IPHC_NH) {
1162                 ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr);
1163                 if (ret < 0)
1164                         return ret;
1165         }
1166
1167         head[0] = iphc0;
1168         head[1] = iphc1;
1169
1170         skb_pull(skb, sizeof(struct ipv6hdr));
1171         skb_reset_transport_header(skb);
1172         memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head);
1173         skb_reset_network_header(skb);
1174
1175         pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len);
1176
1177         raw_dump_table(__func__, "raw skb data dump compressed",
1178                        skb->data, skb->len);
1179         return 0;
1180 }
1181 EXPORT_SYMBOL_GPL(lowpan_header_compress);