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         uh.len = htons(skb->len + sizeof(struct udphdr));
  75         pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len));
  76
  77         /* replace the compressed UDP head by the uncompressed UDP
  78          * header
  79          */
  80         err = skb_cow(skb, needed);
  81         if (unlikely(err))
  82                 return err;
  83
  84         skb_push(skb, sizeof(struct udphdr));
  85         skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
  86
  87         return 0;
  88 }
  89
  90 static int udp_compress(struct sk_buff *skb, u8 **hc_ptr)
  91 {
  92         const struct udphdr *uh = udp_hdr(skb);
  93         u8 tmp;
  94
  95         if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) ==
  96              LOWPAN_NHC_UDP_4BIT_PORT) &&
  97             ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) ==
  98              LOWPAN_NHC_UDP_4BIT_PORT)) {
  99                 pr_debug("UDP header: both ports compression to 4 bits\n");
 100                 /* compression value */
 101                 tmp = LOWPAN_NHC_UDP_CS_P_11;
 102                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 103                 /* source and destination port */
 104                 tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT +
 105                       ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4);
 106                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 107         } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) ==
 108                         LOWPAN_NHC_UDP_8BIT_PORT) {
 109                 pr_debug("UDP header: remove 8 bits of dest\n");
 110                 /* compression value */
 111                 tmp = LOWPAN_NHC_UDP_CS_P_01;
 112                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 113                 /* source port */
 114                 lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
 115                 /* destination port */
 116                 tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT;
 117                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 118         } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) ==
 119                         LOWPAN_NHC_UDP_8BIT_PORT) {
 120                 pr_debug("UDP header: remove 8 bits of source\n");
 121                 /* compression value */
 122                 tmp = LOWPAN_NHC_UDP_CS_P_10;
 123                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 124                 /* source port */
 125                 tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT;
 126                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 127                 /* destination port */
 128                 lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
 129         } else {
 130                 pr_debug("UDP header: can't compress\n");
 131                 /* compression value */
 132                 tmp = LOWPAN_NHC_UDP_CS_P_00;
 133                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
 134                 /* source port */
 135                 lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
 136                 /* destination port */
 137                 lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
 138         }
 139
 140         /* checksum is always inline */
 141         lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check));
 142
 143         return 0;
 144 }
 145
 146 static void udp_nhid_setup(struct lowpan_nhc *nhc)
 147 {
 148         nhc->id[0] = LOWPAN_NHC_UDP_ID;
 149         nhc->idmask[0] = LOWPAN_NHC_UDP_MASK;
 150 }
 151
 152 LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr),
 153            udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress);
 154
 155 module_lowpan_nhc(nhc_udp);
 156 MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression");
 157 MODULE_LICENSE("GPL");