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