LXR linux/net/6lowpan/nhc

   1// SPDX-License-Identifier: GPL-2.0-or-later
   2/*
   3 *      6LoWPAN IPv6 UDP compression according to RFC6282
   4 *
   5 *      Authors:
   6 *      Alexander Aring <aar@pengutronix.de>
   7 *
   8 *      Original written by:
   9 *      Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
  10 *      Jon Smirl <jonsmirl@gmail.com>
  11 */
  12
  13#include "nhc.h"
  14
  15#define LOWPAN_NHC_UDP_MASK             0xF8
  16#define LOWPAN_NHC_UDP_ID               0xF0
  17#define LOWPAN_NHC_UDP_IDLEN            1
  18
  19#define LOWPAN_NHC_UDP_4BIT_PORT        0xF0B0
  20#define LOWPAN_NHC_UDP_4BIT_MASK        0xFFF0
  21#define LOWPAN_NHC_UDP_8BIT_PORT        0xF000
  22#define LOWPAN_NHC_UDP_8BIT_MASK        0xFF00
  23
  24/* values for port compression, _with checksum_ ie bit 5 set to 0 */
  25
  26/* all inline */
  27#define LOWPAN_NHC_UDP_CS_P_00  0xF0
  28/* source 16bit inline, dest = 0xF0 + 8 bit inline */
  29#define LOWPAN_NHC_UDP_CS_P_01  0xF1
  30/* source = 0xF0 + 8bit inline, dest = 16 bit inline */
  31#define LOWPAN_NHC_UDP_CS_P_10  0xF2
  32/* source & dest = 0xF0B + 4bit inline */
  33#define LOWPAN_NHC_UDP_CS_P_11  0xF3
  34/* checksum elided */
  35#define LOWPAN_NHC_UDP_CS_C     0x04
  36
  37static int udp_uncompress(struct sk_buff *skb, size_t needed)
  38{
  39        u8 tmp = 0, val = 0;
  40        struct udphdr uh;
  41        bool fail;
  42        int err;
  43
  44        fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp));
  45
  46        pr_debug("UDP header uncompression\n");
  47        switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
  48        case LOWPAN_NHC_UDP_CS_P_00:
  49                fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
  50                fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
  51                break;
  52        case LOWPAN_NHC_UDP_CS_P_01:
  53                fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
  54                fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
  55                uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
  56                break;
  57        case LOWPAN_NHC_UDP_CS_P_10:
  58                fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
  59                uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
  60                fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
  61                break;
  62        case LOWPAN_NHC_UDP_CS_P_11:
  63                fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
  64                uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4));
  65                uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f));
  66                break;
  67        default:
  68                BUG();
  69        }
  70
  71        pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
  72                 ntohs(uh.source), ntohs(uh.dest));
  73
  74        /* checksum */
  75        if (tmp & LOWPAN_NHC_UDP_CS_C) {
  76                pr_debug_ratelimited("checksum elided currently not supported\n");
  77                fail = true;
  78        } else {
  79                fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check));
  80        }
  81
  82        if (fail)
  83                return -EINVAL;
  84
  85        /* UDP length needs to be inferred from the lower layers
  86         * here, we obtain the hint from the remaining size of the
  87         * frame
  88         */
  89        switch (lowpan_dev(skb->dev)->lltype) {
  90        case LOWPAN_LLTYPE_IEEE802154:
  91                if (lowpan_802154_cb(skb)->d_size)
  92                        uh.len = htons(lowpan_802154_cb(skb)->d_size -
  93                                       sizeof(struct ipv6hdr));
  94                else
  95                        uh.len = htons(skb->len + sizeof(struct udphdr));
  96                break;
  97        default:
  98                uh.len = htons(skb->len + sizeof(struct udphdr));
  99                break;
 100        }
 101        pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len));
 102
 103        /* replace the compressed UDP head by the uncompressed UDP
 104         * header
 105         */
 106        err = skb_cow(skb, needed);
 107        if (unlikely(err))
 108                return err;
 109
 110        skb_push(skb, sizeof(struct udphdr));
 111        skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
 112
 113        return 0;
 114}
 115
 116static int udp_compress(struct sk_buff *skb, u8 **hc_ptr)
 117{
 118        const struct udphdr *uh = udp_hdr(skb);
 119        u8 tmp;
 120
 121        if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) ==
 122             LOWPAN_NHC_UDP_4BIT_PORT) &&
 123            ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) ==
 124             LOWPAN_NHC_UDP_4BIT_PORT)) {
 125                pr_debug("UDP header: both ports compression to 4 bits\n");
 126                /* compression value */
 127                tmp = LOWPAN_NHC_UDP_CS_P_11;
 128                lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 129                /* source and destination port */
 130                tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT +
 131                      ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4);
 132                lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 133        } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) ==
 134                        LOWPAN_NHC_UDP_8BIT_PORT) {
 135                pr_debug("UDP header: remove 8 bits of dest\n");
 136                /* compression value */
 137                tmp = LOWPAN_NHC_UDP_CS_P_01;
 138                lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 139                /* source port */
 140                lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
 141                /* destination port */
 142                tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT;
 143                lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 144        } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) ==
 145                        LOWPAN_NHC_UDP_8BIT_PORT) {
 146                pr_debug("UDP header: remove 8 bits of source\n");
 147                /* compression value */
 148                tmp = LOWPAN_NHC_UDP_CS_P_10;
 149                lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 150                /* source port */
 151                tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT;
 152                lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 153                /* destination port */
 154                lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
 155        } else {
 156                pr_debug("UDP header: can't compress\n");
 157                /* compression value */
 158                tmp = LOWPAN_NHC_UDP_CS_P_00;
 159                lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 160                /* source port */
 161                lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
 162                /* destination port */
 163                lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
 164        }
 165
 166        /* checksum is always inline */
 167        lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check));
 168
 169        return 0;
 170}
 171
 172static void udp_nhid_setup(struct lowpan_nhc *nhc)
 173{
 174        nhc->id[0] = LOWPAN_NHC_UDP_ID;
 175        nhc->idmask[0] = LOWPAN_NHC_UDP_MASK;
 176}
 177
 178LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr),
 179           udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress);
 180
 181module_lowpan_nhc(nhc_udp);
 182MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression");
 183MODULE_LICENSE("GPL");
 184