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

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