[linux-2.6-block.git] / net / 6lowpan / nhc_udp.c

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 *	6LoWPAN IPv6 UDP compression according to RFC6282
 *
 *	Authors:
 *	Alexander Aring	<aar@pengutronix.de>
 *
 *	Orignal written by:
 *	Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
 *	Jon Smirl <jonsmirl@gmail.com>
 */

#include "nhc.h"

#define LOWPAN_NHC_UDP_MASK		0xF8
#define LOWPAN_NHC_UDP_ID		0xF0
#define LOWPAN_NHC_UDP_IDLEN		1

#define LOWPAN_NHC_UDP_4BIT_PORT	0xF0B0
#define LOWPAN_NHC_UDP_4BIT_MASK	0xFFF0
#define LOWPAN_NHC_UDP_8BIT_PORT	0xF000
#define LOWPAN_NHC_UDP_8BIT_MASK	0xFF00

/* values for port compression, _with checksum_ ie bit 5 set to 0 */

/* all inline */
#define LOWPAN_NHC_UDP_CS_P_00	0xF0
/* source 16bit inline, dest = 0xF0 + 8 bit inline */
#define LOWPAN_NHC_UDP_CS_P_01	0xF1
/* source = 0xF0 + 8bit inline, dest = 16 bit inline */
#define LOWPAN_NHC_UDP_CS_P_10	0xF2
/* source & dest = 0xF0B + 4bit inline */
#define LOWPAN_NHC_UDP_CS_P_11	0xF3
/* checksum elided */
#define LOWPAN_NHC_UDP_CS_C	0x04

static int udp_uncompress(struct sk_buff *skb, size_t needed)
{
	u8 tmp = 0, val = 0;
	struct udphdr uh;
	bool fail;
	int err;

	fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp));

	pr_debug("UDP header uncompression\n");
	switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
	case LOWPAN_NHC_UDP_CS_P_00:
		fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
		fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
		break;
	case LOWPAN_NHC_UDP_CS_P_01:
		fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
		fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
		uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
		break;
	case LOWPAN_NHC_UDP_CS_P_10:
		fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
		uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
		fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
		break;
	case LOWPAN_NHC_UDP_CS_P_11:
		fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
		uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4));
		uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f));
		break;
	default:
		BUG();
	}

	pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
		 ntohs(uh.source), ntohs(uh.dest));

	/* checksum */
	if (tmp & LOWPAN_NHC_UDP_CS_C) {
		pr_debug_ratelimited("checksum elided currently not supported\n");
		fail = true;
	} else {
		fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check));
	}

	if (fail)
		return -EINVAL;

	/* UDP length needs to be infered from the lower layers
	 * here, we obtain the hint from the remaining size of the
	 * frame
	 */
	switch (lowpan_dev(skb->dev)->lltype) {
	case LOWPAN_LLTYPE_IEEE802154:
		if (lowpan_802154_cb(skb)->d_size)
			uh.len = htons(lowpan_802154_cb(skb)->d_size -
				       sizeof(struct ipv6hdr));
		else
			uh.len = htons(skb->len + sizeof(struct udphdr));
		break;
	default:
		uh.len = htons(skb->len + sizeof(struct udphdr));
		break;
	}
	pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len));

	/* replace the compressed UDP head by the uncompressed UDP
	 * header
	 */
	err = skb_cow(skb, needed);
	if (unlikely(err))
		return err;

	skb_push(skb, sizeof(struct udphdr));
	skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));

	return 0;
}

static int udp_compress(struct sk_buff *skb, u8 **hc_ptr)
{
	const struct udphdr *uh = udp_hdr(skb);
	u8 tmp;

	if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) ==
	     LOWPAN_NHC_UDP_4BIT_PORT) &&
	    ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) ==
	     LOWPAN_NHC_UDP_4BIT_PORT)) {
		pr_debug("UDP header: both ports compression to 4 bits\n");
		/* compression value */
		tmp = LOWPAN_NHC_UDP_CS_P_11;
		lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
		/* source and destination port */
		tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT +
		      ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4);
		lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
	} else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) ==
			LOWPAN_NHC_UDP_8BIT_PORT) {
		pr_debug("UDP header: remove 8 bits of dest\n");
		/* compression value */
		tmp = LOWPAN_NHC_UDP_CS_P_01;
		lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
		/* source port */
		lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
		/* destination port */
		tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT;
		lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
	} else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) ==
			LOWPAN_NHC_UDP_8BIT_PORT) {
		pr_debug("UDP header: remove 8 bits of source\n");
		/* compression value */
		tmp = LOWPAN_NHC_UDP_CS_P_10;
		lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
		/* source port */
		tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT;
		lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
		/* destination port */
		lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
	} else {
		pr_debug("UDP header: can't compress\n");
		/* compression value */
		tmp = LOWPAN_NHC_UDP_CS_P_00;
		lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
		/* source port */
		lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
		/* destination port */
		lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
	}

	/* checksum is always inline */
	lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check));

	return 0;
}

static void udp_nhid_setup(struct lowpan_nhc *nhc)
{
	nhc->id[0] = LOWPAN_NHC_UDP_ID;
	nhc->idmask[0] = LOWPAN_NHC_UDP_MASK;
}

LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr),
	   udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress);

module_lowpan_nhc(nhc_udp);
MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression");
MODULE_LICENSE("GPL");
Commit	Line	Data
2874c5fd	1	// SPDX-License-Identifier: GPL-2.0-or-later
cc6ed268 AA	2	/*
	3	* 6LoWPAN IPv6 UDP compression according to RFC6282
	4	*
cc6ed268 AA	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>
cc6ed268 AA	11	*/
	12
	13	#include "nhc.h"
	14
6350047e AA	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
cc6ed268 AA	36
	37	static 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 infered from the lower layers
	86	* here, we obtain the hint from the remaining size of the
	87	* frame
	88	*/
2e4d60cb	89	switch (lowpan_dev(skb->dev)->lltype) {
72a5e6bb AA	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	}
cc6ed268 AA	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
	116	static 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
172	static 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
178	LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr),
179	udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress);
180
181	module_lowpan_nhc(nhc_udp);
182	MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression");
183	MODULE_LICENSE("GPL");