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