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