net/6lowpan/nhc_udp.c

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