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