net/6lowpan/nhc_udp.c

   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  *      Orignal 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
  37 static 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 infered 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
 116 static 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
 172 static 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
 178 LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr),
 179            udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress);
 180
 181 module_lowpan_nhc(nhc_udp);
 182 MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression");
 183 MODULE_LICENSE("GPL");