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 inline bool
 765 lowpan_iphc_compress_ctx_802154_lladdr(const struct in6_addr *ipaddr,
 766                                        const struct lowpan_iphc_ctx *ctx,
 767                                        const void *lladdr)
 768 {
 769         const struct ieee802154_addr *addr = lladdr;
 770         unsigned char extended_addr[EUI64_ADDR_LEN];
 771         bool lladdr_compress = false;
 772         struct in6_addr tmp = {};
 773
 774         switch (addr->mode) {
 775         case IEEE802154_ADDR_LONG:
 776                 ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr);
 777                 /* check for SAM/DAM = 11 */
 778                 memcpy(&tmp.s6_addr[8], &extended_addr, EUI64_ADDR_LEN);
 779                 /* second bit-flip (Universe/Local) is done according RFC2464 */
 780                 tmp.s6_addr[8] ^= 0x02;
 781                 /* context information are always used */
 782                 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
 783                 if (ipv6_addr_equal(&tmp, ipaddr))
 784                         lladdr_compress = true;
 785                 break;
 786         case IEEE802154_ADDR_SHORT:
 787                 tmp.s6_addr[11] = 0xFF;
 788                 tmp.s6_addr[12] = 0xFE;
 789                 ieee802154_le16_to_be16(&tmp.s6_addr16[7],
 790                                         &addr->short_addr);
 791                 /* context information are always used */
 792                 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
 793                 if (ipv6_addr_equal(&tmp, ipaddr))
 794                         lladdr_compress = true;
 795                 break;
 796         default:
 797                 /* should never handled and filtered by 802154 6lowpan */
 798                 WARN_ON_ONCE(1);
 799                 break;
 800         }
 801
 802         return lladdr_compress;
 803 }
 804
 805 static u8 lowpan_compress_ctx_addr(u8 **hc_ptr, const struct net_device *dev,
 806                                    const struct in6_addr *ipaddr,
 807                                    const struct lowpan_iphc_ctx *ctx,
 808                                    const unsigned char *lladdr, bool sam)
 809 {
 810         struct in6_addr tmp = {};
 811         u8 dam;
 812
 813         switch (lowpan_dev(dev)->lltype) {
 814         case LOWPAN_LLTYPE_IEEE802154:
 815                 if (lowpan_iphc_compress_ctx_802154_lladdr(ipaddr, ctx,
 816                                                            lladdr)) {
 817                         dam = LOWPAN_IPHC_DAM_11;
 818                         goto out;
 819                 }
 820                 break;
 821         default:
 822                 /* check for SAM/DAM = 11 */
 823                 memcpy(&tmp.s6_addr[8], lladdr, EUI64_ADDR_LEN);
 824                 /* second bit-flip (Universe/Local) is done according RFC2464 */
 825                 tmp.s6_addr[8] ^= 0x02;
 826                 /* context information are always used */
 827                 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
 828                 if (ipv6_addr_equal(&tmp, ipaddr)) {
 829                         dam = LOWPAN_IPHC_DAM_11;
 830                         goto out;
 831                 }
 832                 break;
 833         }
 834
 835         memset(&tmp, 0, sizeof(tmp));
 836         /* check for SAM/DAM = 10 */
 837         tmp.s6_addr[11] = 0xFF;
 838         tmp.s6_addr[12] = 0xFE;
 839         memcpy(&tmp.s6_addr[14], &ipaddr->s6_addr[14], 2);
 840         /* context information are always used */
 841         ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
 842         if (ipv6_addr_equal(&tmp, ipaddr)) {
 843                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[14], 2);
 844                 dam = LOWPAN_IPHC_DAM_10;
 845                 goto out;
 846         }
 847
 848         memset(&tmp, 0, sizeof(tmp));
 849         /* check for SAM/DAM = 01, should always match */
 850         memcpy(&tmp.s6_addr[8], &ipaddr->s6_addr[8], 8);
 851         /* context information are always used */
 852         ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
 853         if (ipv6_addr_equal(&tmp, ipaddr)) {
 854                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[8], 8);
 855                 dam = LOWPAN_IPHC_DAM_01;
 856                 goto out;
 857         }
 858
 859         WARN_ONCE(1, "context found but no address mode matched\n");
 860         return LOWPAN_IPHC_DAM_00;
 861 out:
 862
 863         if (sam)
 864                 return lowpan_iphc_dam_to_sam_value[dam];
 865         else
 866                 return dam;
 867 }
 868
 869 static inline bool
 870 lowpan_iphc_compress_802154_lladdr(const struct in6_addr *ipaddr,
 871                                    const void *lladdr)
 872 {
 873         const struct ieee802154_addr *addr = lladdr;
 874         unsigned char extended_addr[EUI64_ADDR_LEN];
 875         bool lladdr_compress = false;
 876         struct in6_addr tmp = {};
 877
 878         switch (addr->mode) {
 879         case IEEE802154_ADDR_LONG:
 880                 ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr);
 881                 if (is_addr_mac_addr_based(ipaddr, extended_addr))
 882                         lladdr_compress = true;
 883                 break;
 884         case IEEE802154_ADDR_SHORT:
 885                 /* fe:80::ff:fe00:XXXX
 886                  *                \__/
 887                  *             short_addr
 888                  *
 889                  * Universe/Local bit is zero.
 890                  */
 891                 tmp.s6_addr[0] = 0xFE;
 892                 tmp.s6_addr[1] = 0x80;
 893                 tmp.s6_addr[11] = 0xFF;
 894                 tmp.s6_addr[12] = 0xFE;
 895                 ieee802154_le16_to_be16(&tmp.s6_addr16[7],
 896                                         &addr->short_addr);
 897                 if (ipv6_addr_equal(&tmp, ipaddr))
 898                         lladdr_compress = true;
 899                 break;
 900         default:
 901                 /* should never handled and filtered by 802154 6lowpan */
 902                 WARN_ON_ONCE(1);
 903                 break;
 904         }
 905
 906         return lladdr_compress;
 907 }
 908
 909 static u8 lowpan_compress_addr_64(u8 **hc_ptr, const struct net_device *dev,
 910                                   const struct in6_addr *ipaddr,
 911                                   const unsigned char *lladdr, bool sam)
 912 {
 913         u8 dam = LOWPAN_IPHC_DAM_01;
 914
 915         switch (lowpan_dev(dev)->lltype) {
 916         case LOWPAN_LLTYPE_IEEE802154:
 917                 if (lowpan_iphc_compress_802154_lladdr(ipaddr, lladdr)) {
 918                         dam = LOWPAN_IPHC_DAM_11; /* 0-bits */
 919                         pr_debug("address compression 0 bits\n");
 920                         goto out;
 921                 }
 922                 break;
 923         default:
 924                 if (is_addr_mac_addr_based(ipaddr, lladdr)) {
 925                         dam = LOWPAN_IPHC_DAM_11; /* 0-bits */
 926                         pr_debug("address compression 0 bits\n");
 927                         goto out;
 928                 }
 929                 break;
 930         }
 931
 932         if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
 933                 /* compress IID to 16 bits xxxx::XXXX */
 934                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2);
 935                 dam = LOWPAN_IPHC_DAM_10; /* 16-bits */
 936                 raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)",
 937                                 *hc_ptr - 2, 2);
 938                 goto out;
 939         }
 940
 941         /* do not compress IID => xxxx::IID */
 942         lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8);
 943         raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)",
 944                         *hc_ptr - 8, 8);
 945
 946 out:
 947
 948         if (sam)
 949                 return lowpan_iphc_dam_to_sam_value[dam];
 950         else
 951                 return dam;
 952 }
 953
 954 /* lowpan_iphc_get_tc - get the ECN + DCSP fields in hc format */
 955 static inline u8 lowpan_iphc_get_tc(const struct ipv6hdr *hdr)
 956 {
 957         u8 dscp, ecn;
 958
 959         /* hdr->priority contains the higher bits of dscp, lower are part of
 960          * flow_lbl[0]. Note ECN, DCSP is swapped in ipv6 hdr.
 961          */
 962         dscp = (hdr->priority << 2) | ((hdr->flow_lbl[0] & 0xc0) >> 6);
 963         /* ECN is at the two lower bits from first nibble of flow_lbl[0] */
 964         ecn = (hdr->flow_lbl[0] & 0x30);
 965         /* for pretty debug output, also shift ecn to get the ecn value */
 966         pr_debug("ecn 0x%02x dscp 0x%02x\n", ecn >> 4, dscp);
 967         /* ECN is at 0x30 now, shift it to have ECN + DCSP */
 968         return (ecn << 2) | dscp;
 969 }
 970
 971 /* lowpan_iphc_is_flow_lbl_zero - check if flow label is zero */
 972 static inline bool lowpan_iphc_is_flow_lbl_zero(const struct ipv6hdr *hdr)
 973 {
 974         return ((!(hdr->flow_lbl[0] & 0x0f)) &&
 975                 !hdr->flow_lbl[1] && !hdr->flow_lbl[2]);
 976 }
 977
 978 /* lowpan_iphc_tf_compress - compress the traffic class which is set by
 979  *      ipv6hdr. Return the corresponding format identifier which is used.
 980  */
 981 static u8 lowpan_iphc_tf_compress(u8 **hc_ptr, const struct ipv6hdr *hdr)
 982 {
 983         /* get ecn dscp data in a byteformat as: ECN(hi) + DSCP(lo) */
 984         u8 tc = lowpan_iphc_get_tc(hdr), tf[4], val;
 985
 986         /* printout the traffic class in hc format */
 987         pr_debug("tc 0x%02x\n", tc);
 988
 989         if (lowpan_iphc_is_flow_lbl_zero(hdr)) {
 990                 if (!tc) {
 991                         /* 11:  Traffic Class and Flow Label are elided. */
 992                         val = LOWPAN_IPHC_TF_11;
 993                 } else {
 994                         /* 10:  ECN + DSCP (1 byte), Flow Label is elided.
 995                          *
 996                          *  0 1 2 3 4 5 6 7
 997                          * +-+-+-+-+-+-+-+-+
 998                          * |ECN|   DSCP    |
 999                          * +-+-+-+-+-+-+-+-+
1000                          */
1001                         lowpan_push_hc_data(hc_ptr, &tc, sizeof(tc));
1002                         val = LOWPAN_IPHC_TF_10;
1003                 }
1004         } else {
1005                 /* check if dscp is zero, it's after the first two bit */
1006                 if (!(tc & 0x3f)) {
1007                         /* 01:  ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
1008                          *
1009                          *                     1                   2
1010                          * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
1011                          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1012                          * |ECN|rsv|             Flow Label                |
1013                          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1014                          */
1015                         memcpy(&tf[0], &hdr->flow_lbl[0], 3);
1016                         /* zero the highest 4-bits, contains DCSP + ECN */
1017                         tf[0] &= ~0xf0;
1018                         /* set ECN */
1019                         tf[0] |= (tc & 0xc0);
1020
1021                         lowpan_push_hc_data(hc_ptr, tf, 3);
1022                         val = LOWPAN_IPHC_TF_01;
1023                 } else {
1024                         /* 00:  ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
1025                          *
1026                          *                      1                   2                   3
1027                          *  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
1028                          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1029                          * |ECN|   DSCP    |  rsv  |             Flow Label                |
1030                          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1031                          */
1032                         memcpy(&tf[0], &tc, sizeof(tc));
1033                         /* highest nibble of flow_lbl[0] is part of DSCP + ECN
1034                          * which will be the 4-bit pad and will be filled with
1035                          * zeros afterwards.
1036                          */
1037                         memcpy(&tf[1], &hdr->flow_lbl[0], 3);
1038                         /* zero the 4-bit pad, which is reserved */
1039                         tf[1] &= ~0xf0;
1040
1041                         lowpan_push_hc_data(hc_ptr, tf, 4);
1042                         val = LOWPAN_IPHC_TF_00;
1043                 }
1044         }
1045
1046         return val;
1047 }
1048
1049 static u8 lowpan_iphc_mcast_ctx_addr_compress(u8 **hc_ptr,
1050                                               const struct lowpan_iphc_ctx *ctx,
1051                                               const struct in6_addr *ipaddr)
1052 {
1053         u8 data[6];
1054
1055         /* flags/scope, reserved (RIID) */
1056         memcpy(data, &ipaddr->s6_addr[1], 2);
1057         /* group ID */
1058         memcpy(&data[1], &ipaddr->s6_addr[11], 4);
1059         lowpan_push_hc_data(hc_ptr, data, 6);
1060
1061         return LOWPAN_IPHC_DAM_00;
1062 }
1063
1064 static u8 lowpan_iphc_mcast_addr_compress(u8 **hc_ptr,
1065                                           const struct in6_addr *ipaddr)
1066 {
1067         u8 val;
1068
1069         if (lowpan_is_mcast_addr_compressable8(ipaddr)) {
1070                 pr_debug("compressed to 1 octet\n");
1071                 /* use last byte */
1072                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[15], 1);
1073                 val = LOWPAN_IPHC_DAM_11;
1074         } else if (lowpan_is_mcast_addr_compressable32(ipaddr)) {
1075                 pr_debug("compressed to 4 octets\n");
1076                 /* second byte + the last three */
1077                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1);
1078                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[13], 3);
1079                 val = LOWPAN_IPHC_DAM_10;
1080         } else if (lowpan_is_mcast_addr_compressable48(ipaddr)) {
1081                 pr_debug("compressed to 6 octets\n");
1082                 /* second byte + the last five */
1083                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1);
1084                 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[11], 5);
1085                 val = LOWPAN_IPHC_DAM_01;
1086         } else {
1087                 pr_debug("using full address\n");
1088                 lowpan_push_hc_data(hc_ptr, ipaddr->s6_addr, 16);
1089                 val = LOWPAN_IPHC_DAM_00;
1090         }
1091
1092         return val;
1093 }
1094
1095 int lowpan_header_compress(struct sk_buff *skb, const struct net_device *dev,
1096                            const void *daddr, const void *saddr)
1097 {
1098         u8 iphc0, iphc1, *hc_ptr, cid = 0;
1099         struct ipv6hdr *hdr;
1100         u8 head[LOWPAN_IPHC_MAX_HC_BUF_LEN] = {};
1101         struct lowpan_iphc_ctx *dci, *sci, dci_entry, sci_entry;
1102         int ret, ipv6_daddr_type, ipv6_saddr_type;
1103
1104         if (skb->protocol != htons(ETH_P_IPV6))
1105                 return -EINVAL;
1106
1107         hdr = ipv6_hdr(skb);
1108         hc_ptr = head + 2;
1109
1110         pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n"
1111                  "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest    = %pI6c\n",
1112                  hdr->version, ntohs(hdr->payload_len), hdr->nexthdr,
1113                  hdr->hop_limit, &hdr->daddr);
1114
1115         raw_dump_table(__func__, "raw skb network header dump",
1116                        skb_network_header(skb), sizeof(struct ipv6hdr));
1117
1118         /* As we copy some bit-length fields, in the IPHC encoding bytes,
1119          * we sometimes use |=
1120          * If the field is 0, and the current bit value in memory is 1,
1121          * this does not work. We therefore reset the IPHC encoding here
1122          */
1123         iphc0 = LOWPAN_DISPATCH_IPHC;
1124         iphc1 = 0;
1125
1126         raw_dump_table(__func__, "sending raw skb network uncompressed packet",
1127                        skb->data, skb->len);
1128
1129         ipv6_daddr_type = ipv6_addr_type(&hdr->daddr);
1130         spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
1131         if (ipv6_daddr_type & IPV6_ADDR_MULTICAST)
1132                 dci = lowpan_iphc_ctx_get_by_mcast_addr(dev, &hdr->daddr);
1133         else
1134                 dci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->daddr);
1135         if (dci) {
1136                 memcpy(&dci_entry, dci, sizeof(*dci));
1137                 cid |= dci->id;
1138         }
1139         spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
1140
1141         spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
1142         sci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->saddr);
1143         if (sci) {
1144                 memcpy(&sci_entry, sci, sizeof(*sci));
1145                 cid |= (sci->id << 4);
1146         }
1147         spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
1148
1149         /* if cid is zero it will be compressed */
1150         if (cid) {
1151                 iphc1 |= LOWPAN_IPHC_CID;
1152                 lowpan_push_hc_data(&hc_ptr, &cid, sizeof(cid));
1153         }
1154
1155         /* Traffic Class, Flow Label compression */
1156         iphc0 |= lowpan_iphc_tf_compress(&hc_ptr, hdr);
1157
1158         /* NOTE: payload length is always compressed */
1159
1160         /* Check if we provide the nhc format for nexthdr and compression
1161          * functionality. If not nexthdr is handled inline and not compressed.
1162          */
1163         ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr);
1164         if (ret == -ENOENT)
1165                 lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr,
1166                                     sizeof(hdr->nexthdr));
1167         else
1168                 iphc0 |= LOWPAN_IPHC_NH;
1169
1170         /* Hop limit
1171          * if 1:   compress, encoding is 01
1172          * if 64:  compress, encoding is 10
1173          * if 255: compress, encoding is 11
1174          * else do not compress
1175          */
1176         switch (hdr->hop_limit) {
1177         case 1:
1178                 iphc0 |= LOWPAN_IPHC_HLIM_01;
1179                 break;
1180         case 64:
1181                 iphc0 |= LOWPAN_IPHC_HLIM_10;
1182                 break;
1183         case 255:
1184                 iphc0 |= LOWPAN_IPHC_HLIM_11;
1185                 break;
1186         default:
1187                 lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit,
1188                                     sizeof(hdr->hop_limit));
1189         }
1190
1191         ipv6_saddr_type = ipv6_addr_type(&hdr->saddr);
1192         /* source address compression */
1193         if (ipv6_saddr_type == IPV6_ADDR_ANY) {
1194                 pr_debug("source address is unspecified, setting SAC\n");
1195                 iphc1 |= LOWPAN_IPHC_SAC;
1196         } else {
1197                 if (sci) {
1198                         iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev,
1199                                                           &hdr->saddr,
1200                                                           &sci_entry, saddr,
1201                                                           true);
1202                         iphc1 |= LOWPAN_IPHC_SAC;
1203                 } else {
1204                         if (ipv6_saddr_type & IPV6_ADDR_LINKLOCAL &&
1205                             lowpan_is_linklocal_zero_padded(hdr->saddr)) {
1206                                 iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev,
1207                                                                  &hdr->saddr,
1208                                                                  saddr, true);
1209                                 pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n",
1210                                          &hdr->saddr, iphc1);
1211                         } else {
1212                                 pr_debug("send the full source address\n");
1213                                 lowpan_push_hc_data(&hc_ptr,
1214                                                     hdr->saddr.s6_addr, 16);
1215                         }
1216                 }
1217         }
1218
1219         /* destination address compression */
1220         if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) {
1221                 pr_debug("destination address is multicast: ");
1222                 iphc1 |= LOWPAN_IPHC_M;
1223                 if (dci) {
1224                         iphc1 |= lowpan_iphc_mcast_ctx_addr_compress(&hc_ptr,
1225                                                                      &dci_entry,
1226                                                                      &hdr->daddr);
1227                         iphc1 |= LOWPAN_IPHC_DAC;
1228                 } else {
1229                         iphc1 |= lowpan_iphc_mcast_addr_compress(&hc_ptr,
1230                                                                  &hdr->daddr);
1231                 }
1232         } else {
1233                 if (dci) {
1234                         iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev,
1235                                                           &hdr->daddr,
1236                                                           &dci_entry, daddr,
1237                                                           false);
1238                         iphc1 |= LOWPAN_IPHC_DAC;
1239                 } else {
1240                         if (ipv6_daddr_type & IPV6_ADDR_LINKLOCAL &&
1241                             lowpan_is_linklocal_zero_padded(hdr->daddr)) {
1242                                 iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev,
1243                                                                  &hdr->daddr,
1244                                                                  daddr, false);
1245                                 pr_debug("dest address unicast link-local %pI6c iphc1 0x%02x\n",
1246                                          &hdr->daddr, iphc1);
1247                         } else {
1248                                 pr_debug("dest address unicast %pI6c\n",
1249                                          &hdr->daddr);
1250                                 lowpan_push_hc_data(&hc_ptr,
1251                                                     hdr->daddr.s6_addr, 16);
1252                         }
1253                 }
1254         }
1255
1256         /* next header compression */
1257         if (iphc0 & LOWPAN_IPHC_NH) {
1258                 ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr);
1259                 if (ret < 0)
1260                         return ret;
1261         }
1262
1263         head[0] = iphc0;
1264         head[1] = iphc1;
1265
1266         skb_pull(skb, sizeof(struct ipv6hdr));
1267         skb_reset_transport_header(skb);
1268         memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head);
1269         skb_reset_network_header(skb);
1270
1271         pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len);
1272
1273         raw_dump_table(__func__, "raw skb data dump compressed",
1274                        skb->data, skb->len);
1275         return 0;
1276 }
1277 EXPORT_SYMBOL_GPL(lowpan_header_compress);