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