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"); 169