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, data: &tmp, len: 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, data: &uh.source, len: sizeof(uh.source)); |
49 | fail |= lowpan_fetch_skb(skb, data: &uh.dest, len: sizeof(uh.dest)); |
50 | break; |
51 | case LOWPAN_NHC_UDP_CS_P_01: |
52 | fail |= lowpan_fetch_skb(skb, data: &uh.source, len: sizeof(uh.source)); |
53 | fail |= lowpan_fetch_skb(skb, data: &val, len: 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, data: &val, len: sizeof(val)); |
58 | uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); |
59 | fail |= lowpan_fetch_skb(skb, data: &uh.dest, len: sizeof(uh.dest)); |
60 | break; |
61 | case LOWPAN_NHC_UDP_CS_P_11: |
62 | fail |= lowpan_fetch_skb(skb, data: &val, len: 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, data: &uh.check, len: 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(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, headroom: needed); |
106 | if (unlikely(err)) |
107 | return err; |
108 | |
109 | skb_push(skb, len: sizeof(struct udphdr)); |
110 | skb_copy_to_linear_data(skb, from: &uh, len: 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, data: &tmp, len: 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, data: &tmp, len: 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, data: &tmp, len: sizeof(tmp)); |
138 | /* source port */ |
139 | lowpan_push_hc_data(hc_ptr, data: &uh->source, len: sizeof(uh->source)); |
140 | /* destination port */ |
141 | tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT; |
142 | lowpan_push_hc_data(hc_ptr, data: &tmp, len: 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, data: &tmp, len: sizeof(tmp)); |
149 | /* source port */ |
150 | tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT; |
151 | lowpan_push_hc_data(hc_ptr, data: &tmp, len: sizeof(tmp)); |
152 | /* destination port */ |
153 | lowpan_push_hc_data(hc_ptr, data: &uh->dest, len: 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, data: &tmp, len: sizeof(tmp)); |
159 | /* source port */ |
160 | lowpan_push_hc_data(hc_ptr, data: &uh->source, len: sizeof(uh->source)); |
161 | /* destination port */ |
162 | lowpan_push_hc_data(hc_ptr, data: &uh->dest, len: sizeof(uh->dest)); |
163 | } |
164 | |
165 | /* checksum is always inline */ |
166 | lowpan_push_hc_data(hc_ptr, data: &uh->check, len: 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 | |