]>
Commit | Line | Data |
---|---|---|
2874c5fd | 1 | // SPDX-License-Identifier: GPL-2.0-or-later |
cc6ed268 AA |
2 | /* |
3 | * 6LoWPAN IPv6 UDP compression according to RFC6282 | |
4 | * | |
cc6ed268 AA |
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> | |
cc6ed268 AA |
11 | */ |
12 | ||
13 | #include "nhc.h" | |
14 | ||
6350047e AA |
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 | |
cc6ed268 AA |
36 | |
37 | static 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 infered from the lower layers | |
86 | * here, we obtain the hint from the remaining size of the | |
87 | * frame | |
88 | */ | |
2e4d60cb | 89 | switch (lowpan_dev(skb->dev)->lltype) { |
72a5e6bb AA |
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 | } | |
cc6ed268 AA |
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 | ||
116 | static 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 | ||
172 | static 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 | ||
178 | LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr), | |
179 | udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress); | |
180 | ||
181 | module_lowpan_nhc(nhc_udp); | |
182 | MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression"); | |
183 | MODULE_LICENSE("GPL"); |