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");