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