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