Back to home page

OSCL-LXR
 
 

     

0001 /*
0002  * Copyright 2011, Siemens AG
0003  * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
0004  */
0005 
0006 /* Based on patches from Jon Smirl <jonsmirl@gmail.com>
0007  * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
0008  *
0009  * This program is free software; you can redistribute it and/or modify
0010  * it under the terms of the GNU General Public License version 2
0011  * as published by the Free Software Foundation.
0012  *
0013  * This program is distributed in the hope that it will be useful,
0014  * but WITHOUT ANY WARRANTY; without even the implied warranty of
0015  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0016  * GNU General Public License for more details.
0017  *
0018  */
0019 
0020 /* Jon's code is based on 6lowpan implementation for Contiki which is:
0021  * Copyright (c) 2008, Swedish Institute of Computer Science.
0022  * All rights reserved.
0023  *
0024  * Redistribution and use in source and binary forms, with or without
0025  * modification, are permitted provided that the following conditions
0026  * are met:
0027  * 1. Redistributions of source code must retain the above copyright
0028  *    notice, this list of conditions and the following disclaimer.
0029  * 2. Redistributions in binary form must reproduce the above copyright
0030  *    notice, this list of conditions and the following disclaimer in the
0031  *    documentation and/or other materials provided with the distribution.
0032  * 3. Neither the name of the Institute nor the names of its contributors
0033  *    may be used to endorse or promote products derived from this software
0034  *    without specific prior written permission.
0035  *
0036  * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
0037  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
0038  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
0039  * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
0040  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
0041  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
0042  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
0043  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
0044  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
0045  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
0046  * SUCH DAMAGE.
0047  */
0048 
0049 #include <linux/bitops.h>
0050 #include <linux/if_arp.h>
0051 #include <linux/netdevice.h>
0052 
0053 #include <net/6lowpan.h>
0054 #include <net/ipv6.h>
0055 
0056 #include "6lowpan_i.h"
0057 #include "nhc.h"
0058 
0059 /* Values of fields within the IPHC encoding first byte */
0060 #define LOWPAN_IPHC_TF_MASK 0x18
0061 #define LOWPAN_IPHC_TF_00   0x00
0062 #define LOWPAN_IPHC_TF_01   0x08
0063 #define LOWPAN_IPHC_TF_10   0x10
0064 #define LOWPAN_IPHC_TF_11   0x18
0065 
0066 #define LOWPAN_IPHC_NH      0x04
0067 
0068 #define LOWPAN_IPHC_HLIM_MASK   0x03
0069 #define LOWPAN_IPHC_HLIM_00 0x00
0070 #define LOWPAN_IPHC_HLIM_01 0x01
0071 #define LOWPAN_IPHC_HLIM_10 0x02
0072 #define LOWPAN_IPHC_HLIM_11 0x03
0073 
0074 /* Values of fields within the IPHC encoding second byte */
0075 #define LOWPAN_IPHC_CID     0x80
0076 
0077 #define LOWPAN_IPHC_SAC     0x40
0078 
0079 #define LOWPAN_IPHC_SAM_MASK    0x30
0080 #define LOWPAN_IPHC_SAM_00  0x00
0081 #define LOWPAN_IPHC_SAM_01  0x10
0082 #define LOWPAN_IPHC_SAM_10  0x20
0083 #define LOWPAN_IPHC_SAM_11  0x30
0084 
0085 #define LOWPAN_IPHC_M       0x08
0086 
0087 #define LOWPAN_IPHC_DAC     0x04
0088 
0089 #define LOWPAN_IPHC_DAM_MASK    0x03
0090 #define LOWPAN_IPHC_DAM_00  0x00
0091 #define LOWPAN_IPHC_DAM_01  0x01
0092 #define LOWPAN_IPHC_DAM_10  0x02
0093 #define LOWPAN_IPHC_DAM_11  0x03
0094 
0095 /* ipv6 address based on mac
0096  * second bit-flip (Universe/Local) is done according RFC2464
0097  */
0098 #define is_addr_mac_addr_based(a, m) \
0099     ((((a)->s6_addr[8])  == (((m)[0]) ^ 0x02)) &&   \
0100      (((a)->s6_addr[9])  == (m)[1]) &&      \
0101      (((a)->s6_addr[10]) == (m)[2]) &&      \
0102      (((a)->s6_addr[11]) == (m)[3]) &&      \
0103      (((a)->s6_addr[12]) == (m)[4]) &&      \
0104      (((a)->s6_addr[13]) == (m)[5]) &&      \
0105      (((a)->s6_addr[14]) == (m)[6]) &&      \
0106      (((a)->s6_addr[15]) == (m)[7]))
0107 
0108 /* check whether we can compress the IID to 16 bits,
0109  * it's possible for unicast addresses with first 49 bits are zero only.
0110  */
0111 #define lowpan_is_iid_16_bit_compressable(a)    \
0112     ((((a)->s6_addr16[4]) == 0) &&      \
0113      (((a)->s6_addr[10]) == 0) &&       \
0114      (((a)->s6_addr[11]) == 0xff) &&    \
0115      (((a)->s6_addr[12]) == 0xfe) &&    \
0116      (((a)->s6_addr[13]) == 0))
0117 
0118 /* check whether the 112-bit gid of the multicast address is mappable to: */
0119 
0120 /* 48 bits, FFXX::00XX:XXXX:XXXX */
0121 #define lowpan_is_mcast_addr_compressable48(a)  \
0122     ((((a)->s6_addr16[1]) == 0) &&      \
0123      (((a)->s6_addr16[2]) == 0) &&      \
0124      (((a)->s6_addr16[3]) == 0) &&      \
0125      (((a)->s6_addr16[4]) == 0) &&      \
0126      (((a)->s6_addr[10]) == 0))
0127 
0128 /* 32 bits, FFXX::00XX:XXXX */
0129 #define lowpan_is_mcast_addr_compressable32(a)  \
0130     ((((a)->s6_addr16[1]) == 0) &&      \
0131      (((a)->s6_addr16[2]) == 0) &&      \
0132      (((a)->s6_addr16[3]) == 0) &&      \
0133      (((a)->s6_addr16[4]) == 0) &&      \
0134      (((a)->s6_addr16[5]) == 0) &&      \
0135      (((a)->s6_addr[12]) == 0))
0136 
0137 /* 8 bits, FF02::00XX */
0138 #define lowpan_is_mcast_addr_compressable8(a)   \
0139     ((((a)->s6_addr[1])  == 2) &&       \
0140      (((a)->s6_addr16[1]) == 0) &&      \
0141      (((a)->s6_addr16[2]) == 0) &&      \
0142      (((a)->s6_addr16[3]) == 0) &&      \
0143      (((a)->s6_addr16[4]) == 0) &&      \
0144      (((a)->s6_addr16[5]) == 0) &&      \
0145      (((a)->s6_addr16[6]) == 0) &&      \
0146      (((a)->s6_addr[14]) == 0))
0147 
0148 #define lowpan_is_linklocal_zero_padded(a)  \
0149     (!(hdr->saddr.s6_addr[1] & 0x3f) && \
0150      !hdr->saddr.s6_addr16[1] &&        \
0151      !hdr->saddr.s6_addr32[1])
0152 
0153 #define LOWPAN_IPHC_CID_DCI(cid)    (cid & 0x0f)
0154 #define LOWPAN_IPHC_CID_SCI(cid)    ((cid & 0xf0) >> 4)
0155 
0156 static inline void
0157 lowpan_iphc_uncompress_802154_lladdr(struct in6_addr *ipaddr,
0158                      const void *lladdr)
0159 {
0160     const struct ieee802154_addr *addr = lladdr;
0161     u8 eui64[EUI64_ADDR_LEN];
0162 
0163     switch (addr->mode) {
0164     case IEEE802154_ADDR_LONG:
0165         ieee802154_le64_to_be64(eui64, &addr->extended_addr);
0166         lowpan_iphc_uncompress_eui64_lladdr(ipaddr, eui64);
0167         break;
0168     case IEEE802154_ADDR_SHORT:
0169         /* fe:80::ff:fe00:XXXX
0170          *                \__/
0171          *             short_addr
0172          *
0173          * Universe/Local bit is zero.
0174          */
0175         ipaddr->s6_addr[0] = 0xFE;
0176         ipaddr->s6_addr[1] = 0x80;
0177         ipaddr->s6_addr[11] = 0xFF;
0178         ipaddr->s6_addr[12] = 0xFE;
0179         ieee802154_le16_to_be16(&ipaddr->s6_addr16[7],
0180                     &addr->short_addr);
0181         break;
0182     default:
0183         /* should never handled and filtered by 802154 6lowpan */
0184         WARN_ON_ONCE(1);
0185         break;
0186     }
0187 }
0188 
0189 static struct lowpan_iphc_ctx *
0190 lowpan_iphc_ctx_get_by_id(const struct net_device *dev, u8 id)
0191 {
0192     struct lowpan_iphc_ctx *ret = &lowpan_dev(dev)->ctx.table[id];
0193 
0194     if (!lowpan_iphc_ctx_is_active(ret))
0195         return NULL;
0196 
0197     return ret;
0198 }
0199 
0200 static struct lowpan_iphc_ctx *
0201 lowpan_iphc_ctx_get_by_addr(const struct net_device *dev,
0202                 const struct in6_addr *addr)
0203 {
0204     struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table;
0205     struct lowpan_iphc_ctx *ret = NULL;
0206     struct in6_addr addr_pfx;
0207     u8 addr_plen;
0208     int i;
0209 
0210     for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) {
0211         /* Check if context is valid. A context that is not valid
0212          * MUST NOT be used for compression.
0213          */
0214         if (!lowpan_iphc_ctx_is_active(&table[i]) ||
0215             !lowpan_iphc_ctx_is_compression(&table[i]))
0216             continue;
0217 
0218         ipv6_addr_prefix(&addr_pfx, addr, table[i].plen);
0219 
0220         /* if prefix len < 64, the remaining bits until 64th bit is
0221          * zero. Otherwise we use table[i]->plen.
0222          */
0223         if (table[i].plen < 64)
0224             addr_plen = 64;
0225         else
0226             addr_plen = table[i].plen;
0227 
0228         if (ipv6_prefix_equal(&addr_pfx, &table[i].pfx, addr_plen)) {
0229             /* remember first match */
0230             if (!ret) {
0231                 ret = &table[i];
0232                 continue;
0233             }
0234 
0235             /* get the context with longest prefix len */
0236             if (table[i].plen > ret->plen)
0237                 ret = &table[i];
0238         }
0239     }
0240 
0241     return ret;
0242 }
0243 
0244 static struct lowpan_iphc_ctx *
0245 lowpan_iphc_ctx_get_by_mcast_addr(const struct net_device *dev,
0246                   const struct in6_addr *addr)
0247 {
0248     struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table;
0249     struct lowpan_iphc_ctx *ret = NULL;
0250     struct in6_addr addr_mcast, network_pfx = {};
0251     int i;
0252 
0253     /* init mcast address with  */
0254     memcpy(&addr_mcast, addr, sizeof(*addr));
0255 
0256     for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) {
0257         /* Check if context is valid. A context that is not valid
0258          * MUST NOT be used for compression.
0259          */
0260         if (!lowpan_iphc_ctx_is_active(&table[i]) ||
0261             !lowpan_iphc_ctx_is_compression(&table[i]))
0262             continue;
0263 
0264         /* setting plen */
0265         addr_mcast.s6_addr[3] = table[i].plen;
0266         /* get network prefix to copy into multicast address */
0267         ipv6_addr_prefix(&network_pfx, &table[i].pfx,
0268                  table[i].plen);
0269         /* setting network prefix */
0270         memcpy(&addr_mcast.s6_addr[4], &network_pfx, 8);
0271 
0272         if (ipv6_addr_equal(addr, &addr_mcast)) {
0273             ret = &table[i];
0274             break;
0275         }
0276     }
0277 
0278     return ret;
0279 }
0280 
0281 static void lowpan_iphc_uncompress_lladdr(const struct net_device *dev,
0282                       struct in6_addr *ipaddr,
0283                       const void *lladdr)
0284 {
0285     switch (dev->addr_len) {
0286     case ETH_ALEN:
0287         lowpan_iphc_uncompress_eui48_lladdr(ipaddr, lladdr);
0288         break;
0289     case EUI64_ADDR_LEN:
0290         lowpan_iphc_uncompress_eui64_lladdr(ipaddr, lladdr);
0291         break;
0292     default:
0293         WARN_ON_ONCE(1);
0294         break;
0295     }
0296 }
0297 
0298 /* Uncompress address function for source and
0299  * destination address(non-multicast).
0300  *
0301  * address_mode is the masked value for sam or dam value
0302  */
0303 static int lowpan_iphc_uncompress_addr(struct sk_buff *skb,
0304                        const struct net_device *dev,
0305                        struct in6_addr *ipaddr,
0306                        u8 address_mode, const void *lladdr)
0307 {
0308     bool fail;
0309 
0310     switch (address_mode) {
0311     /* SAM and DAM are the same here */
0312     case LOWPAN_IPHC_DAM_00:
0313         /* for global link addresses */
0314         fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
0315         break;
0316     case LOWPAN_IPHC_SAM_01:
0317     case LOWPAN_IPHC_DAM_01:
0318         /* fe:80::XXXX:XXXX:XXXX:XXXX */
0319         ipaddr->s6_addr[0] = 0xFE;
0320         ipaddr->s6_addr[1] = 0x80;
0321         fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
0322         break;
0323     case LOWPAN_IPHC_SAM_10:
0324     case LOWPAN_IPHC_DAM_10:
0325         /* fe:80::ff:fe00:XXXX */
0326         ipaddr->s6_addr[0] = 0xFE;
0327         ipaddr->s6_addr[1] = 0x80;
0328         ipaddr->s6_addr[11] = 0xFF;
0329         ipaddr->s6_addr[12] = 0xFE;
0330         fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
0331         break;
0332     case LOWPAN_IPHC_SAM_11:
0333     case LOWPAN_IPHC_DAM_11:
0334         fail = false;
0335         switch (lowpan_dev(dev)->lltype) {
0336         case LOWPAN_LLTYPE_IEEE802154:
0337             lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr);
0338             break;
0339         default:
0340             lowpan_iphc_uncompress_lladdr(dev, ipaddr, lladdr);
0341             break;
0342         }
0343         break;
0344     default:
0345         pr_debug("Invalid address mode value: 0x%x\n", address_mode);
0346         return -EINVAL;
0347     }
0348 
0349     if (fail) {
0350         pr_debug("Failed to fetch skb data\n");
0351         return -EIO;
0352     }
0353 
0354     raw_dump_inline(NULL, "Reconstructed ipv6 addr is",
0355             ipaddr->s6_addr, 16);
0356 
0357     return 0;
0358 }
0359 
0360 /* Uncompress address function for source context
0361  * based address(non-multicast).
0362  */
0363 static int lowpan_iphc_uncompress_ctx_addr(struct sk_buff *skb,
0364                        const struct net_device *dev,
0365                        const struct lowpan_iphc_ctx *ctx,
0366                        struct in6_addr *ipaddr,
0367                        u8 address_mode, const void *lladdr)
0368 {
0369     bool fail;
0370 
0371     switch (address_mode) {
0372     /* SAM and DAM are the same here */
0373     case LOWPAN_IPHC_DAM_00:
0374         fail = false;
0375         /* SAM_00 -> unspec address ::
0376          * Do nothing, address is already ::
0377          *
0378          * DAM 00 -> reserved should never occur.
0379          */
0380         break;
0381     case LOWPAN_IPHC_SAM_01:
0382     case LOWPAN_IPHC_DAM_01:
0383         fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
0384         ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen);
0385         break;
0386     case LOWPAN_IPHC_SAM_10:
0387     case LOWPAN_IPHC_DAM_10:
0388         ipaddr->s6_addr[11] = 0xFF;
0389         ipaddr->s6_addr[12] = 0xFE;
0390         fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
0391         ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen);
0392         break;
0393     case LOWPAN_IPHC_SAM_11:
0394     case LOWPAN_IPHC_DAM_11:
0395         fail = false;
0396         switch (lowpan_dev(dev)->lltype) {
0397         case LOWPAN_LLTYPE_IEEE802154:
0398             lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr);
0399             break;
0400         default:
0401             lowpan_iphc_uncompress_lladdr(dev, ipaddr, lladdr);
0402             break;
0403         }
0404         ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen);
0405         break;
0406     default:
0407         pr_debug("Invalid sam value: 0x%x\n", address_mode);
0408         return -EINVAL;
0409     }
0410 
0411     if (fail) {
0412         pr_debug("Failed to fetch skb data\n");
0413         return -EIO;
0414     }
0415 
0416     raw_dump_inline(NULL,
0417             "Reconstructed context based ipv6 src addr is",
0418             ipaddr->s6_addr, 16);
0419 
0420     return 0;
0421 }
0422 
0423 /* Uncompress function for multicast destination address,
0424  * when M bit is set.
0425  */
0426 static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb,
0427                          struct in6_addr *ipaddr,
0428                          u8 address_mode)
0429 {
0430     bool fail;
0431 
0432     switch (address_mode) {
0433     case LOWPAN_IPHC_DAM_00:
0434         /* 00:  128 bits.  The full address
0435          * is carried in-line.
0436          */
0437         fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
0438         break;
0439     case LOWPAN_IPHC_DAM_01:
0440         /* 01:  48 bits.  The address takes
0441          * the form ffXX::00XX:XXXX:XXXX.
0442          */
0443         ipaddr->s6_addr[0] = 0xFF;
0444         fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
0445         fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5);
0446         break;
0447     case LOWPAN_IPHC_DAM_10:
0448         /* 10:  32 bits.  The address takes
0449          * the form ffXX::00XX:XXXX.
0450          */
0451         ipaddr->s6_addr[0] = 0xFF;
0452         fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
0453         fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3);
0454         break;
0455     case LOWPAN_IPHC_DAM_11:
0456         /* 11:  8 bits.  The address takes
0457          * the form ff02::00XX.
0458          */
0459         ipaddr->s6_addr[0] = 0xFF;
0460         ipaddr->s6_addr[1] = 0x02;
0461         fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1);
0462         break;
0463     default:
0464         pr_debug("DAM value has a wrong value: 0x%x\n", address_mode);
0465         return -EINVAL;
0466     }
0467 
0468     if (fail) {
0469         pr_debug("Failed to fetch skb data\n");
0470         return -EIO;
0471     }
0472 
0473     raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is",
0474             ipaddr->s6_addr, 16);
0475 
0476     return 0;
0477 }
0478 
0479 static int lowpan_uncompress_multicast_ctx_daddr(struct sk_buff *skb,
0480                          struct lowpan_iphc_ctx *ctx,
0481                          struct in6_addr *ipaddr,
0482                          u8 address_mode)
0483 {
0484     struct in6_addr network_pfx = {};
0485     bool fail;
0486 
0487     ipaddr->s6_addr[0] = 0xFF;
0488     fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 2);
0489     fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[12], 4);
0490     if (fail)
0491         return -EIO;
0492 
0493     /* take prefix_len and network prefix from the context */
0494     ipaddr->s6_addr[3] = ctx->plen;
0495     /* get network prefix to copy into multicast address */
0496     ipv6_addr_prefix(&network_pfx, &ctx->pfx, ctx->plen);
0497     /* setting network prefix */
0498     memcpy(&ipaddr->s6_addr[4], &network_pfx, 8);
0499 
0500     return 0;
0501 }
0502 
0503 /* get the ecn values from iphc tf format and set it to ipv6hdr */
0504 static inline void lowpan_iphc_tf_set_ecn(struct ipv6hdr *hdr, const u8 *tf)
0505 {
0506     /* get the two higher bits which is ecn */
0507     u8 ecn = tf[0] & 0xc0;
0508 
0509     /* ECN takes 0x30 in hdr->flow_lbl[0] */
0510     hdr->flow_lbl[0] |= (ecn >> 2);
0511 }
0512 
0513 /* get the dscp values from iphc tf format and set it to ipv6hdr */
0514 static inline void lowpan_iphc_tf_set_dscp(struct ipv6hdr *hdr, const u8 *tf)
0515 {
0516     /* DSCP is at place after ECN */
0517     u8 dscp = tf[0] & 0x3f;
0518 
0519     /* The four highest bits need to be set at hdr->priority */
0520     hdr->priority |= ((dscp & 0x3c) >> 2);
0521     /* The two lower bits is part of hdr->flow_lbl[0] */
0522     hdr->flow_lbl[0] |= ((dscp & 0x03) << 6);
0523 }
0524 
0525 /* get the flow label values from iphc tf format and set it to ipv6hdr */
0526 static inline void lowpan_iphc_tf_set_lbl(struct ipv6hdr *hdr, const u8 *lbl)
0527 {
0528     /* flow label is always some array started with lower nibble of
0529      * flow_lbl[0] and followed with two bytes afterwards. Inside inline
0530      * data the flow_lbl position can be different, which will be handled
0531      * by lbl pointer. E.g. case "01" vs "00" the traffic class is 8 bit
0532      * shifted, the different lbl pointer will handle that.
0533      *
0534      * The flow label will started at lower nibble of flow_lbl[0], the
0535      * higher nibbles are part of DSCP + ECN.
0536      */
0537     hdr->flow_lbl[0] |= lbl[0] & 0x0f;
0538     memcpy(&hdr->flow_lbl[1], &lbl[1], 2);
0539 }
0540 
0541 /* lowpan_iphc_tf_decompress - decompress the traffic class.
0542  *  This function will return zero on success, a value lower than zero if
0543  *  failed.
0544  */
0545 static int lowpan_iphc_tf_decompress(struct sk_buff *skb, struct ipv6hdr *hdr,
0546                      u8 val)
0547 {
0548     u8 tf[4];
0549 
0550     /* Traffic Class and Flow Label */
0551     switch (val) {
0552     case LOWPAN_IPHC_TF_00:
0553         /* ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) */
0554         if (lowpan_fetch_skb(skb, tf, 4))
0555             return -EINVAL;
0556 
0557         /*                      1                   2                   3
0558          *  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
0559          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
0560          * |ECN|   DSCP    |  rsv  |             Flow Label                |
0561          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
0562          */
0563         lowpan_iphc_tf_set_ecn(hdr, tf);
0564         lowpan_iphc_tf_set_dscp(hdr, tf);
0565         lowpan_iphc_tf_set_lbl(hdr, &tf[1]);
0566         break;
0567     case LOWPAN_IPHC_TF_01:
0568         /* ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided. */
0569         if (lowpan_fetch_skb(skb, tf, 3))
0570             return -EINVAL;
0571 
0572         /*                     1                   2
0573          * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
0574          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
0575          * |ECN|rsv|             Flow Label                |
0576          * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
0577          */
0578         lowpan_iphc_tf_set_ecn(hdr, tf);
0579         lowpan_iphc_tf_set_lbl(hdr, &tf[0]);
0580         break;
0581     case LOWPAN_IPHC_TF_10:
0582         /* ECN + DSCP (1 byte), Flow Label is elided. */
0583         if (lowpan_fetch_skb(skb, tf, 1))
0584             return -EINVAL;
0585 
0586         /*  0 1 2 3 4 5 6 7
0587          * +-+-+-+-+-+-+-+-+
0588          * |ECN|   DSCP    |
0589          * +-+-+-+-+-+-+-+-+
0590          */
0591         lowpan_iphc_tf_set_ecn(hdr, tf);
0592         lowpan_iphc_tf_set_dscp(hdr, tf);
0593         break;
0594     case LOWPAN_IPHC_TF_11:
0595         /* Traffic Class and Flow Label are elided */
0596         break;
0597     default:
0598         WARN_ON_ONCE(1);
0599         return -EINVAL;
0600     }
0601 
0602     return 0;
0603 }
0604 
0605 /* TTL uncompression values */
0606 static const u8 lowpan_ttl_values[] = {
0607     [LOWPAN_IPHC_HLIM_01] = 1,
0608     [LOWPAN_IPHC_HLIM_10] = 64,
0609     [LOWPAN_IPHC_HLIM_11] = 255,
0610 };
0611 
0612 int lowpan_header_decompress(struct sk_buff *skb, const struct net_device *dev,
0613                  const void *daddr, const void *saddr)
0614 {
0615     struct ipv6hdr hdr = {};
0616     struct lowpan_iphc_ctx *ci;
0617     u8 iphc0, iphc1, cid = 0;
0618     int err;
0619 
0620     raw_dump_table(__func__, "raw skb data dump uncompressed",
0621                skb->data, skb->len);
0622 
0623     if (lowpan_fetch_skb(skb, &iphc0, sizeof(iphc0)) ||
0624         lowpan_fetch_skb(skb, &iphc1, sizeof(iphc1)))
0625         return -EINVAL;
0626 
0627     hdr.version = 6;
0628 
0629     /* default CID = 0, another if the CID flag is set */
0630     if (iphc1 & LOWPAN_IPHC_CID) {
0631         if (lowpan_fetch_skb(skb, &cid, sizeof(cid)))
0632             return -EINVAL;
0633     }
0634 
0635     err = lowpan_iphc_tf_decompress(skb, &hdr,
0636                     iphc0 & LOWPAN_IPHC_TF_MASK);
0637     if (err < 0)
0638         return err;
0639 
0640     /* Next Header */
0641     if (!(iphc0 & LOWPAN_IPHC_NH)) {
0642         /* Next header is carried inline */
0643         if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr)))
0644             return -EINVAL;
0645 
0646         pr_debug("NH flag is set, next header carried inline: %02x\n",
0647              hdr.nexthdr);
0648     }
0649 
0650     /* Hop Limit */
0651     if ((iphc0 & LOWPAN_IPHC_HLIM_MASK) != LOWPAN_IPHC_HLIM_00) {
0652         hdr.hop_limit = lowpan_ttl_values[iphc0 & LOWPAN_IPHC_HLIM_MASK];
0653     } else {
0654         if (lowpan_fetch_skb(skb, &hdr.hop_limit,
0655                      sizeof(hdr.hop_limit)))
0656             return -EINVAL;
0657     }
0658 
0659     if (iphc1 & LOWPAN_IPHC_SAC) {
0660         spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
0661         ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_SCI(cid));
0662         if (!ci) {
0663             spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
0664             return -EINVAL;
0665         }
0666 
0667         pr_debug("SAC bit is set. Handle context based source address.\n");
0668         err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.saddr,
0669                               iphc1 & LOWPAN_IPHC_SAM_MASK,
0670                               saddr);
0671         spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
0672     } else {
0673         /* Source address uncompression */
0674         pr_debug("source address stateless compression\n");
0675         err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.saddr,
0676                           iphc1 & LOWPAN_IPHC_SAM_MASK,
0677                           saddr);
0678     }
0679 
0680     /* Check on error of previous branch */
0681     if (err)
0682         return -EINVAL;
0683 
0684     switch (iphc1 & (LOWPAN_IPHC_M | LOWPAN_IPHC_DAC)) {
0685     case LOWPAN_IPHC_M | LOWPAN_IPHC_DAC:
0686         skb->pkt_type = PACKET_BROADCAST;
0687 
0688         spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
0689         ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid));
0690         if (!ci) {
0691             spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
0692             return -EINVAL;
0693         }
0694 
0695         /* multicast with context */
0696         pr_debug("dest: context-based mcast compression\n");
0697         err = lowpan_uncompress_multicast_ctx_daddr(skb, ci,
0698                                 &hdr.daddr,
0699                                 iphc1 & LOWPAN_IPHC_DAM_MASK);
0700         spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
0701         break;
0702     case LOWPAN_IPHC_M:
0703         skb->pkt_type = PACKET_BROADCAST;
0704 
0705         /* multicast */
0706         err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr,
0707                             iphc1 & LOWPAN_IPHC_DAM_MASK);
0708         break;
0709     case LOWPAN_IPHC_DAC:
0710         skb->pkt_type = PACKET_HOST;
0711 
0712         spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
0713         ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid));
0714         if (!ci) {
0715             spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
0716             return -EINVAL;
0717         }
0718 
0719         /* Destination address context based uncompression */
0720         pr_debug("DAC bit is set. Handle context based destination address.\n");
0721         err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.daddr,
0722                               iphc1 & LOWPAN_IPHC_DAM_MASK,
0723                               daddr);
0724         spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
0725         break;
0726     default:
0727         skb->pkt_type = PACKET_HOST;
0728 
0729         err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.daddr,
0730                           iphc1 & LOWPAN_IPHC_DAM_MASK,
0731                           daddr);
0732         pr_debug("dest: stateless compression mode %d dest %pI6c\n",
0733              iphc1 & LOWPAN_IPHC_DAM_MASK, &hdr.daddr);
0734         break;
0735     }
0736 
0737     if (err)
0738         return -EINVAL;
0739 
0740     /* Next header data uncompression */
0741     if (iphc0 & LOWPAN_IPHC_NH) {
0742         err = lowpan_nhc_do_uncompression(skb, dev, &hdr);
0743         if (err < 0)
0744             return err;
0745     } else {
0746         err = skb_cow(skb, sizeof(hdr));
0747         if (unlikely(err))
0748             return err;
0749     }
0750 
0751     switch (lowpan_dev(dev)->lltype) {
0752     case LOWPAN_LLTYPE_IEEE802154:
0753         if (lowpan_802154_cb(skb)->d_size)
0754             hdr.payload_len = htons(lowpan_802154_cb(skb)->d_size -
0755                         sizeof(struct ipv6hdr));
0756         else
0757             hdr.payload_len = htons(skb->len);
0758         break;
0759     default:
0760         hdr.payload_len = htons(skb->len);
0761         break;
0762     }
0763 
0764     pr_debug("skb headroom size = %d, data length = %d\n",
0765          skb_headroom(skb), skb->len);
0766 
0767     pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n\t"
0768          "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest    = %pI6c\n",
0769         hdr.version, ntohs(hdr.payload_len), hdr.nexthdr,
0770         hdr.hop_limit, &hdr.daddr);
0771 
0772     skb_push(skb, sizeof(hdr));
0773     skb_reset_mac_header(skb);
0774     skb_reset_network_header(skb);
0775     skb_copy_to_linear_data(skb, &hdr, sizeof(hdr));
0776 
0777     raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, sizeof(hdr));
0778 
0779     return 0;
0780 }
0781 EXPORT_SYMBOL_GPL(lowpan_header_decompress);
0782 
0783 static const u8 lowpan_iphc_dam_to_sam_value[] = {
0784     [LOWPAN_IPHC_DAM_00] = LOWPAN_IPHC_SAM_00,
0785     [LOWPAN_IPHC_DAM_01] = LOWPAN_IPHC_SAM_01,
0786     [LOWPAN_IPHC_DAM_10] = LOWPAN_IPHC_SAM_10,
0787     [LOWPAN_IPHC_DAM_11] = LOWPAN_IPHC_SAM_11,
0788 };
0789 
0790 static inline bool
0791 lowpan_iphc_compress_ctx_802154_lladdr(const struct in6_addr *ipaddr,
0792                        const struct lowpan_iphc_ctx *ctx,
0793                        const void *lladdr)
0794 {
0795     const struct ieee802154_addr *addr = lladdr;
0796     unsigned char extended_addr[EUI64_ADDR_LEN];
0797     bool lladdr_compress = false;
0798     struct in6_addr tmp = {};
0799 
0800     switch (addr->mode) {
0801     case IEEE802154_ADDR_LONG:
0802         ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr);
0803         /* check for SAM/DAM = 11 */
0804         memcpy(&tmp.s6_addr[8], &extended_addr, EUI64_ADDR_LEN);
0805         /* second bit-flip (Universe/Local) is done according RFC2464 */
0806         tmp.s6_addr[8] ^= 0x02;
0807         /* context information are always used */
0808         ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
0809         if (ipv6_addr_equal(&tmp, ipaddr))
0810             lladdr_compress = true;
0811         break;
0812     case IEEE802154_ADDR_SHORT:
0813         tmp.s6_addr[11] = 0xFF;
0814         tmp.s6_addr[12] = 0xFE;
0815         ieee802154_le16_to_be16(&tmp.s6_addr16[7],
0816                     &addr->short_addr);
0817         /* context information are always used */
0818         ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
0819         if (ipv6_addr_equal(&tmp, ipaddr))
0820             lladdr_compress = true;
0821         break;
0822     default:
0823         /* should never handled and filtered by 802154 6lowpan */
0824         WARN_ON_ONCE(1);
0825         break;
0826     }
0827 
0828     return lladdr_compress;
0829 }
0830 
0831 static bool lowpan_iphc_addr_equal(const struct net_device *dev,
0832                    const struct lowpan_iphc_ctx *ctx,
0833                    const struct in6_addr *ipaddr,
0834                    const void *lladdr)
0835 {
0836     struct in6_addr tmp = {};
0837 
0838     lowpan_iphc_uncompress_lladdr(dev, &tmp, lladdr);
0839 
0840     if (ctx)
0841         ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
0842 
0843     return ipv6_addr_equal(&tmp, ipaddr);
0844 }
0845 
0846 static u8 lowpan_compress_ctx_addr(u8 **hc_ptr, const struct net_device *dev,
0847                    const struct in6_addr *ipaddr,
0848                    const struct lowpan_iphc_ctx *ctx,
0849                    const unsigned char *lladdr, bool sam)
0850 {
0851     struct in6_addr tmp = {};
0852     u8 dam;
0853 
0854     switch (lowpan_dev(dev)->lltype) {
0855     case LOWPAN_LLTYPE_IEEE802154:
0856         if (lowpan_iphc_compress_ctx_802154_lladdr(ipaddr, ctx,
0857                                lladdr)) {
0858             dam = LOWPAN_IPHC_DAM_11;
0859             goto out;
0860         }
0861         break;
0862     default:
0863         if (lowpan_iphc_addr_equal(dev, ctx, ipaddr, lladdr)) {
0864             dam = LOWPAN_IPHC_DAM_11;
0865             goto out;
0866         }
0867         break;
0868     }
0869 
0870     memset(&tmp, 0, sizeof(tmp));
0871     /* check for SAM/DAM = 10 */
0872     tmp.s6_addr[11] = 0xFF;
0873     tmp.s6_addr[12] = 0xFE;
0874     memcpy(&tmp.s6_addr[14], &ipaddr->s6_addr[14], 2);
0875     /* context information are always used */
0876     ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
0877     if (ipv6_addr_equal(&tmp, ipaddr)) {
0878         lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[14], 2);
0879         dam = LOWPAN_IPHC_DAM_10;
0880         goto out;
0881     }
0882 
0883     memset(&tmp, 0, sizeof(tmp));
0884     /* check for SAM/DAM = 01, should always match */
0885     memcpy(&tmp.s6_addr[8], &ipaddr->s6_addr[8], 8);
0886     /* context information are always used */
0887     ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
0888     if (ipv6_addr_equal(&tmp, ipaddr)) {
0889         lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[8], 8);
0890         dam = LOWPAN_IPHC_DAM_01;
0891         goto out;
0892     }
0893 
0894     WARN_ONCE(1, "context found but no address mode matched\n");
0895     return LOWPAN_IPHC_DAM_00;
0896 out:
0897 
0898     if (sam)
0899         return lowpan_iphc_dam_to_sam_value[dam];
0900     else
0901         return dam;
0902 }
0903 
0904 static inline bool
0905 lowpan_iphc_compress_802154_lladdr(const struct in6_addr *ipaddr,
0906                    const void *lladdr)
0907 {
0908     const struct ieee802154_addr *addr = lladdr;
0909     unsigned char extended_addr[EUI64_ADDR_LEN];
0910     bool lladdr_compress = false;
0911     struct in6_addr tmp = {};
0912 
0913     switch (addr->mode) {
0914     case IEEE802154_ADDR_LONG:
0915         ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr);
0916         if (is_addr_mac_addr_based(ipaddr, extended_addr))
0917             lladdr_compress = true;
0918         break;
0919     case IEEE802154_ADDR_SHORT:
0920         /* fe:80::ff:fe00:XXXX
0921          *                \__/
0922          *             short_addr
0923          *
0924          * Universe/Local bit is zero.
0925          */
0926         tmp.s6_addr[0] = 0xFE;
0927         tmp.s6_addr[1] = 0x80;
0928         tmp.s6_addr[11] = 0xFF;
0929         tmp.s6_addr[12] = 0xFE;
0930         ieee802154_le16_to_be16(&tmp.s6_addr16[7],
0931                     &addr->short_addr);
0932         if (ipv6_addr_equal(&tmp, ipaddr))
0933             lladdr_compress = true;
0934         break;
0935     default:
0936         /* should never handled and filtered by 802154 6lowpan */
0937         WARN_ON_ONCE(1);
0938         break;
0939     }
0940 
0941     return lladdr_compress;
0942 }
0943 
0944 static u8 lowpan_compress_addr_64(u8 **hc_ptr, const struct net_device *dev,
0945                   const struct in6_addr *ipaddr,
0946                   const unsigned char *lladdr, bool sam)
0947 {
0948     u8 dam = LOWPAN_IPHC_DAM_01;
0949 
0950     switch (lowpan_dev(dev)->lltype) {
0951     case LOWPAN_LLTYPE_IEEE802154:
0952         if (lowpan_iphc_compress_802154_lladdr(ipaddr, lladdr)) {
0953             dam = LOWPAN_IPHC_DAM_11; /* 0-bits */
0954             pr_debug("address compression 0 bits\n");
0955             goto out;
0956         }
0957         break;
0958     default:
0959         if (lowpan_iphc_addr_equal(dev, NULL, ipaddr, lladdr)) {
0960             dam = LOWPAN_IPHC_DAM_11;
0961             pr_debug("address compression 0 bits\n");
0962             goto out;
0963         }
0964 
0965         break;
0966     }
0967 
0968     if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
0969         /* compress IID to 16 bits xxxx::XXXX */
0970         lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2);
0971         dam = LOWPAN_IPHC_DAM_10; /* 16-bits */
0972         raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)",
0973                 *hc_ptr - 2, 2);
0974         goto out;
0975     }
0976 
0977     /* do not compress IID => xxxx::IID */
0978     lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8);
0979     raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)",
0980             *hc_ptr - 8, 8);
0981 
0982 out:
0983 
0984     if (sam)
0985         return lowpan_iphc_dam_to_sam_value[dam];
0986     else
0987         return dam;
0988 }
0989 
0990 /* lowpan_iphc_get_tc - get the ECN + DCSP fields in hc format */
0991 static inline u8 lowpan_iphc_get_tc(const struct ipv6hdr *hdr)
0992 {
0993     u8 dscp, ecn;
0994 
0995     /* hdr->priority contains the higher bits of dscp, lower are part of
0996      * flow_lbl[0]. Note ECN, DCSP is swapped in ipv6 hdr.
0997      */
0998     dscp = (hdr->priority << 2) | ((hdr->flow_lbl[0] & 0xc0) >> 6);
0999     /* 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 */
1008 static 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  */
1017 static 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 
1085 static 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 
1100 static 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 
1131 int 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 }
1313 EXPORT_SYMBOL_GPL(lowpan_header_compress);
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!