Loading...
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");
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
36static 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
115static 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
171LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr),
172 LOWPAN_NHC_UDP_ID, LOWPAN_NHC_UDP_MASK, udp_uncompress, udp_compress);
173
174module_lowpan_nhc(nhc_udp);
175MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression");
176MODULE_LICENSE("GPL");