1// SPDX-License-Identifier: GPL-2.0-only
2/*
3 * Copyright (C) 2014 Fraunhofer ITWM
4 *
5 * Written by:
6 * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
7 */
8
9#include <linux/ieee802154.h>
10
11#include <net/mac802154.h>
12#include <net/ieee802154_netdev.h>
13
14static int
15ieee802154_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
46static int
47ieee802154_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
77int
78ieee802154_hdr_push(struct sk_buff *skb, struct ieee802154_hdr *hdr)
79{
80	u8 buf[IEEE802154_MAX_HEADER_LEN];
81	int pos = 2;
82	int rc;
83	struct ieee802154_hdr_fc *fc = &hdr->fc;
84
85	buf[pos++] = hdr->seq;
86
87	fc->dest_addr_mode = hdr->dest.mode;
88
89	rc = ieee802154_hdr_push_addr(buf + pos, &hdr->dest, false);
90	if (rc < 0)
91		return -EINVAL;
92	pos += rc;
93
94	fc->source_addr_mode = hdr->source.mode;
95
96	if (hdr->source.pan_id == hdr->dest.pan_id &&
97	    hdr->dest.mode != IEEE802154_ADDR_NONE)
98		fc->intra_pan = true;
99
100	rc = ieee802154_hdr_push_addr(buf + pos, &hdr->source, fc->intra_pan);
101	if (rc < 0)
102		return -EINVAL;
103	pos += rc;
104
105	if (fc->security_enabled) {
106		fc->version = 1;
107
108		rc = ieee802154_hdr_push_sechdr(buf + pos, &hdr->sec);
109		if (rc < 0)
110			return -EINVAL;
111
112		pos += rc;
113	}
114
115	memcpy(buf, fc, 2);
116
117	memcpy(skb_push(skb, pos), buf, pos);
118
119	return pos;
120}
121EXPORT_SYMBOL_GPL(ieee802154_hdr_push);
122
123int ieee802154_mac_cmd_push(struct sk_buff *skb, void *f,
124			    const void *pl, unsigned int pl_len)
125{
126	struct ieee802154_mac_cmd_frame *frame = f;
127	struct ieee802154_mac_cmd_pl *mac_pl = &frame->mac_pl;
128	struct ieee802154_hdr *mhr = &frame->mhr;
129	int ret;
130
131	skb_reserve(skb, sizeof(*mhr));
132	ret = ieee802154_hdr_push(skb, mhr);
133	if (ret < 0)
134		return ret;
135
136	skb_reset_mac_header(skb);
137	skb->mac_len = ret;
138
139	skb_put_data(skb, mac_pl, sizeof(*mac_pl));
140	skb_put_data(skb, pl, pl_len);
141
142	return 0;
143}
144EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_push);
145
146int ieee802154_beacon_push(struct sk_buff *skb,
147			   struct ieee802154_beacon_frame *beacon)
148{
149	struct ieee802154_beacon_hdr *mac_pl = &beacon->mac_pl;
150	struct ieee802154_hdr *mhr = &beacon->mhr;
151	int ret;
152
153	skb_reserve(skb, sizeof(*mhr));
154	ret = ieee802154_hdr_push(skb, mhr);
155	if (ret < 0)
156		return ret;
157
158	skb_reset_mac_header(skb);
159	skb->mac_len = ret;
160
161	skb_put_data(skb, mac_pl, sizeof(*mac_pl));
162
163	if (mac_pl->pend_short_addr_count || mac_pl->pend_ext_addr_count)
164		return -EOPNOTSUPP;
165
166	return 0;
167}
168EXPORT_SYMBOL_GPL(ieee802154_beacon_push);
169
170static int
171ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan,
172			struct ieee802154_addr *addr)
173{
174	int pos = 0;
175
176	addr->mode = mode;
177
178	if (mode == IEEE802154_ADDR_NONE)
179		return 0;
180
181	if (!omit_pan) {
182		memcpy(&addr->pan_id, buf + pos, 2);
183		pos += 2;
184	}
185
186	if (mode == IEEE802154_ADDR_SHORT) {
187		memcpy(&addr->short_addr, buf + pos, 2);
188		return pos + 2;
189	} else {
190		memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN);
191		return pos + IEEE802154_ADDR_LEN;
192	}
193}
194
195static int ieee802154_hdr_addr_len(int mode, bool omit_pan)
196{
197	int pan_len = omit_pan ? 0 : 2;
198
199	switch (mode) {
200	case IEEE802154_ADDR_NONE: return 0;
201	case IEEE802154_ADDR_SHORT: return 2 + pan_len;
202	case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len;
203	default: return -EINVAL;
204	}
205}
206
207static int
208ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr)
209{
210	int pos = 5;
211
212	memcpy(hdr, buf, 1);
213	memcpy(&hdr->frame_counter, buf + 1, 4);
214
215	switch (hdr->key_id_mode) {
216	case IEEE802154_SCF_KEY_IMPLICIT:
217		return pos;
218
219	case IEEE802154_SCF_KEY_INDEX:
220		break;
221
222	case IEEE802154_SCF_KEY_SHORT_INDEX:
223		memcpy(&hdr->short_src, buf + pos, 4);
224		pos += 4;
225		break;
226
227	case IEEE802154_SCF_KEY_HW_INDEX:
228		memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN);
229		pos += IEEE802154_ADDR_LEN;
230		break;
231	}
232
233	hdr->key_id = buf[pos++];
234
235	return pos;
236}
237
238static int ieee802154_sechdr_lengths[4] = {
239	[IEEE802154_SCF_KEY_IMPLICIT] = 5,
240	[IEEE802154_SCF_KEY_INDEX] = 6,
241	[IEEE802154_SCF_KEY_SHORT_INDEX] = 10,
242	[IEEE802154_SCF_KEY_HW_INDEX] = 14,
243};
244
245static int ieee802154_hdr_sechdr_len(u8 sc)
246{
247	return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
248}
249
250static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr)
251{
252	int dlen, slen;
253
254	dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false);
255	slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode,
256				       hdr->fc.intra_pan);
257
258	if (slen < 0 || dlen < 0)
259		return -EINVAL;
260
261	return 3 + dlen + slen + hdr->fc.security_enabled;
262}
263
264static int
265ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr)
266{
267	int pos = 0;
268
269	pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode,
270				       false, &hdr->dest);
271	pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode,
272				       hdr->fc.intra_pan, &hdr->source);
273
274	if (hdr->fc.intra_pan)
275		hdr->source.pan_id = hdr->dest.pan_id;
276
277	return pos;
278}
279
280int
281ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr)
282{
283	int pos = 3, rc;
284
285	if (!pskb_may_pull(skb, 3))
286		return -EINVAL;
287
288	memcpy(hdr, skb->data, 3);
289
290	rc = ieee802154_hdr_minlen(hdr);
291	if (rc < 0 || !pskb_may_pull(skb, rc))
292		return -EINVAL;
293
294	pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr);
295
296	if (hdr->fc.security_enabled) {
297		int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]);
298
299		if (!pskb_may_pull(skb, want))
300			return -EINVAL;
301
302		pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec);
303	}
304
305	skb_pull(skb, pos);
306	return pos;
307}
308EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
309
310int ieee802154_mac_cmd_pl_pull(struct sk_buff *skb,
311			       struct ieee802154_mac_cmd_pl *mac_pl)
312{
313	if (!pskb_may_pull(skb, sizeof(*mac_pl)))
314		return -EINVAL;
315
316	memcpy(mac_pl, skb->data, sizeof(*mac_pl));
317	skb_pull(skb, sizeof(*mac_pl));
318
319	return 0;
320}
321EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_pl_pull);
322
323int
324ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
325{
326	const u8 *buf = skb_mac_header(skb);
327	int pos = 3, rc;
328
329	if (buf + 3 > skb_tail_pointer(skb))
330		return -EINVAL;
331
332	memcpy(hdr, buf, 3);
333
334	rc = ieee802154_hdr_minlen(hdr);
335	if (rc < 0 || buf + rc > skb_tail_pointer(skb))
336		return -EINVAL;
337
338	pos += ieee802154_hdr_get_addrs(buf + pos, hdr);
339	return pos;
340}
341EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
342
343int
344ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
345{
346	const u8 *buf = skb_mac_header(skb);
347	int pos;
348
349	pos = ieee802154_hdr_peek_addrs(skb, hdr);
350	if (pos < 0)
351		return -EINVAL;
352
353	if (hdr->fc.security_enabled) {
354		u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos));
355		int want = pos + ieee802154_sechdr_lengths[key_id_mode];
356
357		if (buf + want > skb_tail_pointer(skb))
358			return -EINVAL;
359
360		pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec);
361	}
362
363	return pos;
364}
365EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
366
367int ieee802154_max_payload(const struct ieee802154_hdr *hdr)
368{
369	int hlen = ieee802154_hdr_minlen(hdr);
370
371	if (hdr->fc.security_enabled) {
372		hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1;
373		hlen += ieee802154_sechdr_authtag_len(&hdr->sec);
374	}
375
376	return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE;
377}
378EXPORT_SYMBOL_GPL(ieee802154_max_payload);
379