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