1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * 6LoWPAN IPv6 UDP compression according to RFC6282 4 * 5 * Authors: 6 * Alexander Aring <aar@pengutronix.de> 7 * 8 * Original written by: 9 * Alexander Smirnov <alex.bluesman.smirnov@gmail.com> 10 * Jon Smirl <jonsmirl@gmail.com> 11 */ 12 13 #include "nhc.h" 14 15 #define LOWPAN_NHC_UDP_MASK 0xF8 16 #define LOWPAN_NHC_UDP_ID 0xF0 17 18 #define LOWPAN_NHC_UDP_4BIT_PORT 0xF0B0 19 #define LOWPAN_NHC_UDP_4BIT_MASK 0xFFF0 20 #define LOWPAN_NHC_UDP_8BIT_PORT 0xF000 21 #define LOWPAN_NHC_UDP_8BIT_MASK 0xFF00 22 23 /* values for port compression, _with checksum_ ie bit 5 set to 0 */ 24 25 /* all inline */ 26 #define LOWPAN_NHC_UDP_CS_P_00 0xF0 27 /* source 16bit inline, dest = 0xF0 + 8 bit inline */ 28 #define LOWPAN_NHC_UDP_CS_P_01 0xF1 29 /* source = 0xF0 + 8bit inline, dest = 16 bit inline */ 30 #define LOWPAN_NHC_UDP_CS_P_10 0xF2 31 /* source & dest = 0xF0B + 4bit inline */ 32 #define LOWPAN_NHC_UDP_CS_P_11 0xF3 33 /* checksum elided */ 34 #define LOWPAN_NHC_UDP_CS_C 0x04 35 36 static int udp_uncompress(struct sk_buff *skb, size_t needed) 37 { 38 u8 tmp = 0, val = 0; 39 struct udphdr uh; 40 bool fail; 41 int err; 42 43 fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp)); 44 45 pr_debug("UDP header uncompression\n"); 46 switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { 47 case LOWPAN_NHC_UDP_CS_P_00: 48 fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); 49 fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); 50 break; 51 case LOWPAN_NHC_UDP_CS_P_01: 52 fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); 53 fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); 54 uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); 55 break; 56 case LOWPAN_NHC_UDP_CS_P_10: 57 fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); 58 uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); 59 fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); 60 break; 61 case LOWPAN_NHC_UDP_CS_P_11: 62 fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); 63 uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4)); 64 uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f)); 65 break; 66 default: 67 BUG(); 68 } 69 70 pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", 71 ntohs(uh.source), ntohs(uh.dest)); 72 73 /* checksum */ 74 if (tmp & LOWPAN_NHC_UDP_CS_C) { 75 pr_debug_ratelimited("checksum elided currently not supported\n"); 76 fail = true; 77 } else { 78 fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check)); 79 } 80 81 if (fail) 82 return -EINVAL; 83 84 /* UDP length needs to be inferred from the lower layers 85 * here, we obtain the hint from the remaining size of the 86 * frame 87 */ 88 switch (lowpan_dev(skb->dev)->lltype) { 89 case LOWPAN_LLTYPE_IEEE802154: 90 if (lowpan_802154_cb(skb)->d_size) 91 uh.len = htons(lowpan_802154_cb(skb)->d_size - 92 sizeof(struct ipv6hdr)); 93 else 94 uh.len = htons(skb->len + sizeof(struct udphdr)); 95 break; 96 default: 97 uh.len = htons(skb->len + sizeof(struct udphdr)); 98 break; 99 } 100 pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len)); 101 102 /* replace the compressed UDP head by the uncompressed UDP 103 * header 104 */ 105 err = skb_cow(skb, needed); 106 if (unlikely(err)) 107 return err; 108 109 skb_push(skb, sizeof(struct udphdr)); 110 skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); 111 112 return 0; 113 } 114 115 static int udp_compress(struct sk_buff *skb, u8 **hc_ptr) 116 { 117 const struct udphdr *uh = udp_hdr(skb); 118 u8 tmp; 119 120 if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) == 121 LOWPAN_NHC_UDP_4BIT_PORT) && 122 ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) == 123 LOWPAN_NHC_UDP_4BIT_PORT)) { 124 pr_debug("UDP header: both ports compression to 4 bits\n"); 125 /* compression value */ 126 tmp = LOWPAN_NHC_UDP_CS_P_11; 127 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 128 /* source and destination port */ 129 tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT + 130 ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4); 131 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 132 } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) == 133 LOWPAN_NHC_UDP_8BIT_PORT) { 134 pr_debug("UDP header: remove 8 bits of dest\n"); 135 /* compression value */ 136 tmp = LOWPAN_NHC_UDP_CS_P_01; 137 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 138 /* source port */ 139 lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); 140 /* destination port */ 141 tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT; 142 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 143 } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) == 144 LOWPAN_NHC_UDP_8BIT_PORT) { 145 pr_debug("UDP header: remove 8 bits of source\n"); 146 /* compression value */ 147 tmp = LOWPAN_NHC_UDP_CS_P_10; 148 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 149 /* source port */ 150 tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT; 151 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 152 /* destination port */ 153 lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); 154 } else { 155 pr_debug("UDP header: can't compress\n"); 156 /* compression value */ 157 tmp = LOWPAN_NHC_UDP_CS_P_00; 158 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); 159 /* source port */ 160 lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); 161 /* destination port */ 162 lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); 163 } 164 165 /* checksum is always inline */ 166 lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check)); 167 168 return 0; 169 } 170 171 LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr), 172 LOWPAN_NHC_UDP_ID, LOWPAN_NHC_UDP_MASK, udp_uncompress, udp_compress); 173 174 module_lowpan_nhc(nhc_udp); 175 MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression"); 176 MODULE_LICENSE("GPL"); 177