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