[linux-2.6-block.git] / net / ieee802154 / header_ops.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * Copyright (C) 2014 Fraunhofer ITWM
 *
 * Written by:
 * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
 */

#include <linux/ieee802154.h>

#include <net/mac802154.h>
#include <net/ieee802154_netdev.h>

static int
ieee802154_hdr_push_addr(u8 *buf, const struct ieee802154_addr *addr,
			 bool omit_pan)
{
	int pos = 0;

	if (addr->mode == IEEE802154_ADDR_NONE)
		return 0;

	if (!omit_pan) {
		memcpy(buf + pos, &addr->pan_id, 2);
		pos += 2;
	}

	switch (addr->mode) {
	case IEEE802154_ADDR_SHORT:
		memcpy(buf + pos, &addr->short_addr, 2);
		pos += 2;
		break;

	case IEEE802154_ADDR_LONG:
		memcpy(buf + pos, &addr->extended_addr, IEEE802154_ADDR_LEN);
		pos += IEEE802154_ADDR_LEN;
		break;

	default:
		return -EINVAL;
	}

	return pos;
}

static int
ieee802154_hdr_push_sechdr(u8 *buf, const struct ieee802154_sechdr *hdr)
{
	int pos = 5;

	memcpy(buf, hdr, 1);
	memcpy(buf + 1, &hdr->frame_counter, 4);

	switch (hdr->key_id_mode) {
	case IEEE802154_SCF_KEY_IMPLICIT:
		return pos;

	case IEEE802154_SCF_KEY_INDEX:
		break;

	case IEEE802154_SCF_KEY_SHORT_INDEX:
		memcpy(buf + pos, &hdr->short_src, 4);
		pos += 4;
		break;

	case IEEE802154_SCF_KEY_HW_INDEX:
		memcpy(buf + pos, &hdr->extended_src, IEEE802154_ADDR_LEN);
		pos += IEEE802154_ADDR_LEN;
		break;
	}

	buf[pos++] = hdr->key_id;

	return pos;
}

int
ieee802154_hdr_push(struct sk_buff *skb, struct ieee802154_hdr *hdr)
{
	u8 buf[IEEE802154_MAX_HEADER_LEN];
	int pos = 2;
	int rc;
	struct ieee802154_hdr_fc *fc = &hdr->fc;

	buf[pos++] = hdr->seq;

	fc->dest_addr_mode = hdr->dest.mode;

	rc = ieee802154_hdr_push_addr(buf + pos, &hdr->dest, false);
	if (rc < 0)
		return -EINVAL;
	pos += rc;

	fc->source_addr_mode = hdr->source.mode;

	if (hdr->source.pan_id == hdr->dest.pan_id &&
	    hdr->dest.mode != IEEE802154_ADDR_NONE)
		fc->intra_pan = true;

	rc = ieee802154_hdr_push_addr(buf + pos, &hdr->source, fc->intra_pan);
	if (rc < 0)
		return -EINVAL;
	pos += rc;

	if (fc->security_enabled) {
		fc->version = 1;

		rc = ieee802154_hdr_push_sechdr(buf + pos, &hdr->sec);
		if (rc < 0)
			return -EINVAL;

		pos += rc;
	}

	memcpy(buf, fc, 2);

	memcpy(skb_push(skb, pos), buf, pos);

	return pos;
}
EXPORT_SYMBOL_GPL(ieee802154_hdr_push);

static int
ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan,
			struct ieee802154_addr *addr)
{
	int pos = 0;

	addr->mode = mode;

	if (mode == IEEE802154_ADDR_NONE)
		return 0;

	if (!omit_pan) {
		memcpy(&addr->pan_id, buf + pos, 2);
		pos += 2;
	}

	if (mode == IEEE802154_ADDR_SHORT) {
		memcpy(&addr->short_addr, buf + pos, 2);
		return pos + 2;
	} else {
		memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN);
		return pos + IEEE802154_ADDR_LEN;
	}
}

static int ieee802154_hdr_addr_len(int mode, bool omit_pan)
{
	int pan_len = omit_pan ? 0 : 2;

	switch (mode) {
	case IEEE802154_ADDR_NONE: return 0;
	case IEEE802154_ADDR_SHORT: return 2 + pan_len;
	case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len;
	default: return -EINVAL;
	}
}

static int
ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr)
{
	int pos = 5;

	memcpy(hdr, buf, 1);
	memcpy(&hdr->frame_counter, buf + 1, 4);

	switch (hdr->key_id_mode) {
	case IEEE802154_SCF_KEY_IMPLICIT:
		return pos;

	case IEEE802154_SCF_KEY_INDEX:
		break;

	case IEEE802154_SCF_KEY_SHORT_INDEX:
		memcpy(&hdr->short_src, buf + pos, 4);
		pos += 4;
		break;

	case IEEE802154_SCF_KEY_HW_INDEX:
		memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN);
		pos += IEEE802154_ADDR_LEN;
		break;
	}

	hdr->key_id = buf[pos++];

	return pos;
}

static int ieee802154_sechdr_lengths[4] = {
	[IEEE802154_SCF_KEY_IMPLICIT] = 5,
	[IEEE802154_SCF_KEY_INDEX] = 6,
	[IEEE802154_SCF_KEY_SHORT_INDEX] = 10,
	[IEEE802154_SCF_KEY_HW_INDEX] = 14,
};

static int ieee802154_hdr_sechdr_len(u8 sc)
{
	return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
}

static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr)
{
	int dlen, slen;

	dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false);
	slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode,
				       hdr->fc.intra_pan);

	if (slen < 0 || dlen < 0)
		return -EINVAL;

	return 3 + dlen + slen + hdr->fc.security_enabled;
}

static int
ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr)
{
	int pos = 0;

	pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode,
				       false, &hdr->dest);
	pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode,
				       hdr->fc.intra_pan, &hdr->source);

	if (hdr->fc.intra_pan)
		hdr->source.pan_id = hdr->dest.pan_id;

	return pos;
}

int
ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr)
{
	int pos = 3, rc;

	if (!pskb_may_pull(skb, 3))
		return -EINVAL;

	memcpy(hdr, skb->data, 3);

	rc = ieee802154_hdr_minlen(hdr);
	if (rc < 0 || !pskb_may_pull(skb, rc))
		return -EINVAL;

	pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr);

	if (hdr->fc.security_enabled) {
		int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]);

		if (!pskb_may_pull(skb, want))
			return -EINVAL;

		pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec);
	}

	skb_pull(skb, pos);
	return pos;
}
EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);

int
ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
{
	const u8 *buf = skb_mac_header(skb);
	int pos = 3, rc;

	if (buf + 3 > skb_tail_pointer(skb))
		return -EINVAL;

	memcpy(hdr, buf, 3);

	rc = ieee802154_hdr_minlen(hdr);
	if (rc < 0 || buf + rc > skb_tail_pointer(skb))
		return -EINVAL;

	pos += ieee802154_hdr_get_addrs(buf + pos, hdr);
	return pos;
}
EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);

int
ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
{
	const u8 *buf = skb_mac_header(skb);
	int pos;

	pos = ieee802154_hdr_peek_addrs(skb, hdr);
	if (pos < 0)
		return -EINVAL;

	if (hdr->fc.security_enabled) {
		u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos));
		int want = pos + ieee802154_sechdr_lengths[key_id_mode];

		if (buf + want > skb_tail_pointer(skb))
			return -EINVAL;

		pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec);
	}

	return pos;
}
EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);

int ieee802154_max_payload(const struct ieee802154_hdr *hdr)
{
	int hlen = ieee802154_hdr_minlen(hdr);

	if (hdr->fc.security_enabled) {
		hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1;
		hlen += ieee802154_sechdr_authtag_len(&hdr->sec);
	}

	return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE;
}
EXPORT_SYMBOL_GPL(ieee802154_max_payload);
Commit	Line	Data
1802d0be	1	// SPDX-License-Identifier: GPL-2.0-only
94b4f6c2 PB	2	/*
	3	* Copyright (C) 2014 Fraunhofer ITWM
	4	*
94b4f6c2 PB	5	* Written by:
	6	* Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
	7	*/
	8
4ca24aca AA	9	#include <linux/ieee802154.h>
4ca24aca AA	10
94b4f6c2	11	#include <net/mac802154.h>
94b4f6c2 PB	12	#include <net/ieee802154_netdev.h>
	13
	14	static int
	15	ieee802154_hdr_push_addr(u8 buf, const struct ieee802154_addr addr,
	16	bool omit_pan)
	17	{
	18	int pos = 0;
	19
	20	if (addr->mode == IEEE802154_ADDR_NONE)
	21	return 0;
	22
	23	if (!omit_pan) {
	24	memcpy(buf + pos, &addr->pan_id, 2);
	25	pos += 2;
	26	}
	27
	28	switch (addr->mode) {
	29	case IEEE802154_ADDR_SHORT:
	30	memcpy(buf + pos, &addr->short_addr, 2);
	31	pos += 2;
	32	break;
	33
	34	case IEEE802154_ADDR_LONG:
	35	memcpy(buf + pos, &addr->extended_addr, IEEE802154_ADDR_LEN);
	36	pos += IEEE802154_ADDR_LEN;
	37	break;
	38
	39	default:
	40	return -EINVAL;
	41	}
	42
	43	return pos;
	44	}
	45
	46	static int
	47	ieee802154_hdr_push_sechdr(u8 buf, const struct ieee802154_sechdr hdr)
	48	{
	49	int pos = 5;
	50
	51	memcpy(buf, hdr, 1);
	52	memcpy(buf + 1, &hdr->frame_counter, 4);
	53
	54	switch (hdr->key_id_mode) {
	55	case IEEE802154_SCF_KEY_IMPLICIT:
	56	return pos;
	57
	58	case IEEE802154_SCF_KEY_INDEX:
	59	break;
	60
	61	case IEEE802154_SCF_KEY_SHORT_INDEX:
	62	memcpy(buf + pos, &hdr->short_src, 4);
	63	pos += 4;
	64	break;
	65
	66	case IEEE802154_SCF_KEY_HW_INDEX:
	67	memcpy(buf + pos, &hdr->extended_src, IEEE802154_ADDR_LEN);
	68	pos += IEEE802154_ADDR_LEN;
	69	break;
	70	}
	71
	72	buf[pos++] = hdr->key_id;
	73
	74	return pos;
	75	}
76
77	int
a1da67b8	78	ieee802154_hdr_push(struct sk_buff skb, struct ieee802154_hdr hdr)
94b4f6c2	79	{
87a93e4e	80	u8 buf[IEEE802154_MAX_HEADER_LEN];
94b4f6c2 PB	81	int pos = 2;
94b4f6c2 PB	82	int rc;
a1da67b8	83	struct ieee802154_hdr_fc *fc = &hdr->fc;
94b4f6c2 PB	84
	85	buf[pos++] = hdr->seq;
	86
a1da67b8	87	fc->dest_addr_mode = hdr->dest.mode;
94b4f6c2 PB	88
	89	rc = ieee802154_hdr_push_addr(buf + pos, &hdr->dest, false);
	90	if (rc < 0)
	91	return -EINVAL;
	92	pos += rc;
	93
a1da67b8	94	fc->source_addr_mode = hdr->source.mode;
94b4f6c2 PB	95
	96	if (hdr->source.pan_id == hdr->dest.pan_id &&
	97	hdr->dest.mode != IEEE802154_ADDR_NONE)
a1da67b8	98	fc->intra_pan = true;
94b4f6c2	99
a1da67b8	100	rc = ieee802154_hdr_push_addr(buf + pos, &hdr->source, fc->intra_pan);
94b4f6c2 PB	101	if (rc < 0)
	102	return -EINVAL;
	103	pos += rc;
	104
a1da67b8 AA	105	if (fc->security_enabled) {
a1da67b8 AA	106	fc->version = 1;
94b4f6c2 PB	107
	108	rc = ieee802154_hdr_push_sechdr(buf + pos, &hdr->sec);
	109	if (rc < 0)
	110	return -EINVAL;
	111
	112	pos += rc;
	113	}
	114
a1da67b8	115	memcpy(buf, fc, 2);
94b4f6c2 PB	116
	117	memcpy(skb_push(skb, pos), buf, pos);
	118
	119	return pos;
	120	}
	121	EXPORT_SYMBOL_GPL(ieee802154_hdr_push);
	122
	123	static int
	124	ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan,
	125	struct ieee802154_addr *addr)
	126	{
	127	int pos = 0;
	128
	129	addr->mode = mode;
	130
	131	if (mode == IEEE802154_ADDR_NONE)
	132	return 0;
	133
	134	if (!omit_pan) {
	135	memcpy(&addr->pan_id, buf + pos, 2);
	136	pos += 2;
	137	}
	138
	139	if (mode == IEEE802154_ADDR_SHORT) {
	140	memcpy(&addr->short_addr, buf + pos, 2);
	141	return pos + 2;
	142	} else {
	143	memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN);
	144	return pos + IEEE802154_ADDR_LEN;
	145	}
	146	}
	147
	148	static int ieee802154_hdr_addr_len(int mode, bool omit_pan)
	149	{
	150	int pan_len = omit_pan ? 0 : 2;
	151
	152	switch (mode) {
	153	case IEEE802154_ADDR_NONE: return 0;
	154	case IEEE802154_ADDR_SHORT: return 2 + pan_len;
	155	case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len;
	156	default: return -EINVAL;
	157	}
	158	}
	159
	160	static int
	161	ieee802154_hdr_get_sechdr(const u8 buf, struct ieee802154_sechdr hdr)
	162	{
	163	int pos = 5;
	164
	165	memcpy(hdr, buf, 1);
	166	memcpy(&hdr->frame_counter, buf + 1, 4);
	167
	168	switch (hdr->key_id_mode) {
	169	case IEEE802154_SCF_KEY_IMPLICIT:
	170	return pos;
	171
	172	case IEEE802154_SCF_KEY_INDEX:
	173	break;
	174
	175	case IEEE802154_SCF_KEY_SHORT_INDEX:
	176	memcpy(&hdr->short_src, buf + pos, 4);
	177	pos += 4;
	178	break;
	179
180	case IEEE802154_SCF_KEY_HW_INDEX:
181	memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN);
182	pos += IEEE802154_ADDR_LEN;
183	break;
184	}
185
186	hdr->key_id = buf[pos++];
187
188	return pos;
189	}
190
c3a6114f PB	191	static int ieee802154_sechdr_lengths[4] = {
	192	[IEEE802154_SCF_KEY_IMPLICIT] = 5,
	193	[IEEE802154_SCF_KEY_INDEX] = 6,
	194	[IEEE802154_SCF_KEY_SHORT_INDEX] = 10,
	195	[IEEE802154_SCF_KEY_HW_INDEX] = 14,
	196	};
	197
94b4f6c2 PB	198	static int ieee802154_hdr_sechdr_len(u8 sc)
94b4f6c2 PB	199	{
c3a6114f	200	return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
94b4f6c2 PB	201	}
	202
	203	static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr)
	204	{
	205	int dlen, slen;
	206
	207	dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false);
	208	slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode,
	209	hdr->fc.intra_pan);
	210
	211	if (slen < 0 \|\| dlen < 0)
	212	return -EINVAL;
	213
	214	return 3 + dlen + slen + hdr->fc.security_enabled;
	215	}
	216
	217	static int
	218	ieee802154_hdr_get_addrs(const u8 buf, struct ieee802154_hdr hdr)
	219	{
	220	int pos = 0;
	221
	222	pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode,
	223	false, &hdr->dest);
	224	pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode,
	225	hdr->fc.intra_pan, &hdr->source);
	226
	227	if (hdr->fc.intra_pan)
	228	hdr->source.pan_id = hdr->dest.pan_id;
	229
	230	return pos;
	231	}
	232
	233	int
	234	ieee802154_hdr_pull(struct sk_buff skb, struct ieee802154_hdr hdr)
	235	{
	236	int pos = 3, rc;
	237
	238	if (!pskb_may_pull(skb, 3))
	239	return -EINVAL;
	240
	241	memcpy(hdr, skb->data, 3);
	242
	243	rc = ieee802154_hdr_minlen(hdr);
	244	if (rc < 0 \|\| !pskb_may_pull(skb, rc))
	245	return -EINVAL;
	246
	247	pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr);
	248
	249	if (hdr->fc.security_enabled) {
	250	int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]);
	251
	252	if (!pskb_may_pull(skb, want))
	253	return -EINVAL;
	254
	255	pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec);
	256	}
	257
	258	skb_pull(skb, pos);
	259	return pos;
	260	}
	261	EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
	262
	263	int
	264	ieee802154_hdr_peek_addrs(const struct sk_buff skb, struct ieee802154_hdr hdr)
265	{
266	const u8 *buf = skb_mac_header(skb);
267	int pos = 3, rc;
268
269	if (buf + 3 > skb_tail_pointer(skb))
270	return -EINVAL;
271
272	memcpy(hdr, buf, 3);
273
274	rc = ieee802154_hdr_minlen(hdr);
275	if (rc < 0 \|\| buf + rc > skb_tail_pointer(skb))
276	return -EINVAL;
277
278	pos += ieee802154_hdr_get_addrs(buf + pos, hdr);
279	return pos;
280	}
281	EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
c3a6114f PB	282
	283	int
	284	ieee802154_hdr_peek(const struct sk_buff skb, struct ieee802154_hdr hdr)
	285	{
	286	const u8 *buf = skb_mac_header(skb);
	287	int pos;
	288
	289	pos = ieee802154_hdr_peek_addrs(skb, hdr);
	290	if (pos < 0)
	291	return -EINVAL;
	292
	293	if (hdr->fc.security_enabled) {
	294	u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos));
	295	int want = pos + ieee802154_sechdr_lengths[key_id_mode];
	296
	297	if (buf + want > skb_tail_pointer(skb))
	298	return -EINVAL;
	299
	300	pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec);
	301	}
	302
	303	return pos;
	304	}
	305	EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
	306
	307	int ieee802154_max_payload(const struct ieee802154_hdr *hdr)
	308	{
	309	int hlen = ieee802154_hdr_minlen(hdr);
	310
	311	if (hdr->fc.security_enabled) {
	312	hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1;
	313	hlen += ieee802154_sechdr_authtag_len(&hdr->sec);
	314	}
	315
	316	return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE;
	317	}
	318	EXPORT_SYMBOL_GPL(ieee802154_max_payload);