xref: /linux/net/ieee802154/header_ops.c (revision f9bff0e31881d03badf191d3b0005839391f5f2b)
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 
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
78 ieee802154_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 }
121 EXPORT_SYMBOL_GPL(ieee802154_hdr_push);
122 
123 int 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 }
144 EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_push);
145 
146 int 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 }
168 EXPORT_SYMBOL_GPL(ieee802154_beacon_push);
169 
170 static int
171 ieee802154_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 
195 static 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 
207 static int
208 ieee802154_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 
238 static 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 
245 static int ieee802154_hdr_sechdr_len(u8 sc)
246 {
247 	return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
248 }
249 
250 static 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 
264 static int
265 ieee802154_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 
280 int
281 ieee802154_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 }
308 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
309 
310 int 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 }
321 EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_pl_pull);
322 
323 int
324 ieee802154_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 }
341 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
342 
343 int
344 ieee802154_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 }
365 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
366 
367 int 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 }
378 EXPORT_SYMBOL_GPL(ieee802154_max_payload);
379