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
ieee802154_beacon_push(struct sk_buff * skb,struct ieee802154_beacon_frame * beacon)123 int ieee802154_beacon_push(struct sk_buff *skb,
124 struct ieee802154_beacon_frame *beacon)
125 {
126 struct ieee802154_beacon_hdr *mac_pl = &beacon->mac_pl;
127 struct ieee802154_hdr *mhr = &beacon->mhr;
128 int ret;
129
130 skb_reserve(skb, sizeof(*mhr));
131 ret = ieee802154_hdr_push(skb, mhr);
132 if (ret < 0)
133 return ret;
134
135 skb_reset_mac_header(skb);
136 skb->mac_len = ret;
137
138 skb_put_data(skb, mac_pl, sizeof(*mac_pl));
139
140 if (mac_pl->pend_short_addr_count || mac_pl->pend_ext_addr_count)
141 return -EOPNOTSUPP;
142
143 return 0;
144 }
145 EXPORT_SYMBOL_GPL(ieee802154_beacon_push);
146
147 static int
ieee802154_hdr_get_addr(const u8 * buf,int mode,bool omit_pan,struct ieee802154_addr * addr)148 ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan,
149 struct ieee802154_addr *addr)
150 {
151 int pos = 0;
152
153 addr->mode = mode;
154
155 if (mode == IEEE802154_ADDR_NONE)
156 return 0;
157
158 if (!omit_pan) {
159 memcpy(&addr->pan_id, buf + pos, 2);
160 pos += 2;
161 }
162
163 if (mode == IEEE802154_ADDR_SHORT) {
164 memcpy(&addr->short_addr, buf + pos, 2);
165 return pos + 2;
166 } else {
167 memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN);
168 return pos + IEEE802154_ADDR_LEN;
169 }
170 }
171
ieee802154_hdr_addr_len(int mode,bool omit_pan)172 static int ieee802154_hdr_addr_len(int mode, bool omit_pan)
173 {
174 int pan_len = omit_pan ? 0 : 2;
175
176 switch (mode) {
177 case IEEE802154_ADDR_NONE: return 0;
178 case IEEE802154_ADDR_SHORT: return 2 + pan_len;
179 case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len;
180 default: return -EINVAL;
181 }
182 }
183
184 static int
ieee802154_hdr_get_sechdr(const u8 * buf,struct ieee802154_sechdr * hdr)185 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr)
186 {
187 int pos = 5;
188
189 memcpy(hdr, buf, 1);
190 memcpy(&hdr->frame_counter, buf + 1, 4);
191
192 switch (hdr->key_id_mode) {
193 case IEEE802154_SCF_KEY_IMPLICIT:
194 return pos;
195
196 case IEEE802154_SCF_KEY_INDEX:
197 break;
198
199 case IEEE802154_SCF_KEY_SHORT_INDEX:
200 memcpy(&hdr->short_src, buf + pos, 4);
201 pos += 4;
202 break;
203
204 case IEEE802154_SCF_KEY_HW_INDEX:
205 memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN);
206 pos += IEEE802154_ADDR_LEN;
207 break;
208 }
209
210 hdr->key_id = buf[pos++];
211
212 return pos;
213 }
214
215 static int ieee802154_sechdr_lengths[4] = {
216 [IEEE802154_SCF_KEY_IMPLICIT] = 5,
217 [IEEE802154_SCF_KEY_INDEX] = 6,
218 [IEEE802154_SCF_KEY_SHORT_INDEX] = 10,
219 [IEEE802154_SCF_KEY_HW_INDEX] = 14,
220 };
221
ieee802154_hdr_sechdr_len(u8 sc)222 static int ieee802154_hdr_sechdr_len(u8 sc)
223 {
224 return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
225 }
226
ieee802154_hdr_minlen(const struct ieee802154_hdr * hdr)227 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr)
228 {
229 int dlen, slen;
230
231 dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false);
232 slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode,
233 hdr->fc.intra_pan);
234
235 if (slen < 0 || dlen < 0)
236 return -EINVAL;
237
238 return 3 + dlen + slen + hdr->fc.security_enabled;
239 }
240
241 static int
ieee802154_hdr_get_addrs(const u8 * buf,struct ieee802154_hdr * hdr)242 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr)
243 {
244 int pos = 0;
245
246 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode,
247 false, &hdr->dest);
248 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode,
249 hdr->fc.intra_pan, &hdr->source);
250
251 if (hdr->fc.intra_pan)
252 hdr->source.pan_id = hdr->dest.pan_id;
253
254 return pos;
255 }
256
257 int
ieee802154_hdr_pull(struct sk_buff * skb,struct ieee802154_hdr * hdr)258 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr)
259 {
260 int pos = 3, rc;
261
262 if (!pskb_may_pull(skb, 3))
263 return -EINVAL;
264
265 memcpy(hdr, skb->data, 3);
266
267 rc = ieee802154_hdr_minlen(hdr);
268 if (rc < 0 || !pskb_may_pull(skb, rc))
269 return -EINVAL;
270
271 pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr);
272
273 if (hdr->fc.security_enabled) {
274 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]);
275
276 if (!pskb_may_pull(skb, want))
277 return -EINVAL;
278
279 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec);
280 }
281
282 skb_pull(skb, pos);
283 return pos;
284 }
285 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
286
287 int
ieee802154_hdr_peek_addrs(const struct sk_buff * skb,struct ieee802154_hdr * hdr)288 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
289 {
290 const u8 *buf = skb_mac_header(skb);
291 int pos = 3, rc;
292
293 if (buf + 3 > skb_tail_pointer(skb))
294 return -EINVAL;
295
296 memcpy(hdr, buf, 3);
297
298 rc = ieee802154_hdr_minlen(hdr);
299 if (rc < 0 || buf + rc > skb_tail_pointer(skb))
300 return -EINVAL;
301
302 pos += ieee802154_hdr_get_addrs(buf + pos, hdr);
303 return pos;
304 }
305 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
306
307 int
ieee802154_hdr_peek(const struct sk_buff * skb,struct ieee802154_hdr * hdr)308 ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
309 {
310 const u8 *buf = skb_mac_header(skb);
311 int pos;
312
313 pos = ieee802154_hdr_peek_addrs(skb, hdr);
314 if (pos < 0)
315 return -EINVAL;
316
317 if (hdr->fc.security_enabled) {
318 u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos));
319 int want = pos + ieee802154_sechdr_lengths[key_id_mode];
320
321 if (buf + want > skb_tail_pointer(skb))
322 return -EINVAL;
323
324 pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec);
325 }
326
327 return pos;
328 }
329 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
330
ieee802154_max_payload(const struct ieee802154_hdr * hdr)331 int ieee802154_max_payload(const struct ieee802154_hdr *hdr)
332 {
333 int hlen = ieee802154_hdr_minlen(hdr);
334
335 if (hdr->fc.security_enabled) {
336 hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1;
337 hlen += ieee802154_sechdr_authtag_len(&hdr->sec);
338 }
339
340 return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE;
341 }
342 EXPORT_SYMBOL_GPL(ieee802154_max_payload);
343