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