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