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
ieee802154_hdr_push_addr(u8 * buf,const struct ieee802154_addr * addr,bool omit_pan)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
ieee802154_hdr_push_sechdr(u8 * buf,const struct ieee802154_sechdr * hdr)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
ieee802154_hdr_push(struct sk_buff * skb,struct ieee802154_hdr * hdr)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 static int
ieee802154_hdr_get_addr(const u8 * buf,int mode,bool omit_pan,struct ieee802154_addr * addr)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 
ieee802154_hdr_addr_len(int mode,bool omit_pan)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
ieee802154_hdr_get_sechdr(const u8 * buf,struct ieee802154_sechdr * hdr)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 
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 
ieee802154_hdr_sechdr_len(u8 sc)198 static int ieee802154_hdr_sechdr_len(u8 sc)
199 {
200 	return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
201 }
202 
ieee802154_hdr_minlen(const struct ieee802154_hdr * hdr)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
ieee802154_hdr_get_addrs(const u8 * buf,struct ieee802154_hdr * hdr)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
ieee802154_hdr_pull(struct sk_buff * skb,struct ieee802154_hdr * hdr)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
ieee802154_hdr_peek_addrs(const struct sk_buff * skb,struct ieee802154_hdr * hdr)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);
282 
283 int
ieee802154_hdr_peek(const struct sk_buff * skb,struct ieee802154_hdr * hdr)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 
ieee802154_max_payload(const struct ieee802154_hdr * hdr)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);
319