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
17#include <linux/ieee802154.h>
18
19#include <net/mac802154.h>
20#include <net/ieee802154_netdev.h>
21
22static int
23ieee802154_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
54static int
55ieee802154_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
85int
86ieee802154_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}
129EXPORT_SYMBOL_GPL(ieee802154_hdr_push);
130
131static int
132ieee802154_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
156static 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
168static int
169ieee802154_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
199static 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
206static int ieee802154_hdr_sechdr_len(u8 sc)
207{
208	return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
209}
210
211static 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
225static int
226ieee802154_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
241int
242ieee802154_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}
269EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
270
271int
272ieee802154_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}
289EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
290
291int
292ieee802154_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}
313EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
314
315int 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}
326EXPORT_SYMBOL_GPL(ieee802154_max_payload);
327