nhc_udp.c

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