1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17#include <linux/err.h>
18#include <linux/bug.h>
19#include <linux/completion.h>
20#include <net/ieee802154.h>
21#include <crypto/algapi.h>
22
23#include "mac802154.h"
24#include "llsec.h"
25
26static void llsec_key_put(struct mac802154_llsec_key *key);
27static bool llsec_key_id_equal(const struct ieee802154_llsec_key_id *a,
28 const struct ieee802154_llsec_key_id *b);
29
30static void llsec_dev_free(struct mac802154_llsec_device *dev);
31
32void mac802154_llsec_init(struct mac802154_llsec *sec)
33{
34 memset(sec, 0, sizeof(*sec));
35
36 memset(&sec->params.default_key_source, 0xFF, IEEE802154_ADDR_LEN);
37
38 INIT_LIST_HEAD(&sec->table.security_levels);
39 INIT_LIST_HEAD(&sec->table.devices);
40 INIT_LIST_HEAD(&sec->table.keys);
41 hash_init(sec->devices_short);
42 hash_init(sec->devices_hw);
43 rwlock_init(&sec->lock);
44}
45
46void mac802154_llsec_destroy(struct mac802154_llsec *sec)
47{
48 struct ieee802154_llsec_seclevel *sl, *sn;
49 struct ieee802154_llsec_device *dev, *dn;
50 struct ieee802154_llsec_key_entry *key, *kn;
51
52 list_for_each_entry_safe(sl, sn, &sec->table.security_levels, list) {
53 struct mac802154_llsec_seclevel *msl;
54
55 msl = container_of(sl, struct mac802154_llsec_seclevel, level);
56 list_del(&sl->list);
57 kfree(msl);
58 }
59
60 list_for_each_entry_safe(dev, dn, &sec->table.devices, list) {
61 struct mac802154_llsec_device *mdev;
62
63 mdev = container_of(dev, struct mac802154_llsec_device, dev);
64 list_del(&dev->list);
65 llsec_dev_free(mdev);
66 }
67
68 list_for_each_entry_safe(key, kn, &sec->table.keys, list) {
69 struct mac802154_llsec_key *mkey;
70
71 mkey = container_of(key->key, struct mac802154_llsec_key, key);
72 list_del(&key->list);
73 llsec_key_put(mkey);
74 kfree(key);
75 }
76}
77
78
79
80int mac802154_llsec_get_params(struct mac802154_llsec *sec,
81 struct ieee802154_llsec_params *params)
82{
83 read_lock_bh(&sec->lock);
84 *params = sec->params;
85 read_unlock_bh(&sec->lock);
86
87 return 0;
88}
89
90int mac802154_llsec_set_params(struct mac802154_llsec *sec,
91 const struct ieee802154_llsec_params *params,
92 int changed)
93{
94 write_lock_bh(&sec->lock);
95
96 if (changed & IEEE802154_LLSEC_PARAM_ENABLED)
97 sec->params.enabled = params->enabled;
98 if (changed & IEEE802154_LLSEC_PARAM_FRAME_COUNTER)
99 sec->params.frame_counter = params->frame_counter;
100 if (changed & IEEE802154_LLSEC_PARAM_OUT_LEVEL)
101 sec->params.out_level = params->out_level;
102 if (changed & IEEE802154_LLSEC_PARAM_OUT_KEY)
103 sec->params.out_key = params->out_key;
104 if (changed & IEEE802154_LLSEC_PARAM_KEY_SOURCE)
105 sec->params.default_key_source = params->default_key_source;
106 if (changed & IEEE802154_LLSEC_PARAM_PAN_ID)
107 sec->params.pan_id = params->pan_id;
108 if (changed & IEEE802154_LLSEC_PARAM_HWADDR)
109 sec->params.hwaddr = params->hwaddr;
110 if (changed & IEEE802154_LLSEC_PARAM_COORD_HWADDR)
111 sec->params.coord_hwaddr = params->coord_hwaddr;
112 if (changed & IEEE802154_LLSEC_PARAM_COORD_SHORTADDR)
113 sec->params.coord_shortaddr = params->coord_shortaddr;
114
115 write_unlock_bh(&sec->lock);
116
117 return 0;
118}
119
120
121
122static struct mac802154_llsec_key*
123llsec_key_alloc(const struct ieee802154_llsec_key *template)
124{
125 const int authsizes[3] = { 4, 8, 16 };
126 struct mac802154_llsec_key *key;
127 int i;
128
129 key = kzalloc(sizeof(*key), GFP_KERNEL);
130 if (!key)
131 return NULL;
132
133 kref_init(&key->ref);
134 key->key = *template;
135
136 BUILD_BUG_ON(ARRAY_SIZE(authsizes) != ARRAY_SIZE(key->tfm));
137
138 for (i = 0; i < ARRAY_SIZE(key->tfm); i++) {
139 key->tfm[i] = crypto_alloc_aead("ccm(aes)", 0,
140 CRYPTO_ALG_ASYNC);
141 if (!key->tfm[i])
142 goto err_tfm;
143 if (crypto_aead_setkey(key->tfm[i], template->key,
144 IEEE802154_LLSEC_KEY_SIZE))
145 goto err_tfm;
146 if (crypto_aead_setauthsize(key->tfm[i], authsizes[i]))
147 goto err_tfm;
148 }
149
150 key->tfm0 = crypto_alloc_blkcipher("ctr(aes)", 0, CRYPTO_ALG_ASYNC);
151 if (!key->tfm0)
152 goto err_tfm;
153
154 if (crypto_blkcipher_setkey(key->tfm0, template->key,
155 IEEE802154_LLSEC_KEY_SIZE))
156 goto err_tfm0;
157
158 return key;
159
160err_tfm0:
161 crypto_free_blkcipher(key->tfm0);
162err_tfm:
163 for (i = 0; i < ARRAY_SIZE(key->tfm); i++)
164 if (key->tfm[i])
165 crypto_free_aead(key->tfm[i]);
166
167 kfree(key);
168 return NULL;
169}
170
171static void llsec_key_release(struct kref *ref)
172{
173 struct mac802154_llsec_key *key;
174 int i;
175
176 key = container_of(ref, struct mac802154_llsec_key, ref);
177
178 for (i = 0; i < ARRAY_SIZE(key->tfm); i++)
179 crypto_free_aead(key->tfm[i]);
180
181 crypto_free_blkcipher(key->tfm0);
182 kfree(key);
183}
184
185static struct mac802154_llsec_key*
186llsec_key_get(struct mac802154_llsec_key *key)
187{
188 kref_get(&key->ref);
189 return key;
190}
191
192static void llsec_key_put(struct mac802154_llsec_key *key)
193{
194 kref_put(&key->ref, llsec_key_release);
195}
196
197static bool llsec_key_id_equal(const struct ieee802154_llsec_key_id *a,
198 const struct ieee802154_llsec_key_id *b)
199{
200 if (a->mode != b->mode)
201 return false;
202
203 if (a->mode == IEEE802154_SCF_KEY_IMPLICIT)
204 return ieee802154_addr_equal(&a->device_addr, &b->device_addr);
205
206 if (a->id != b->id)
207 return false;
208
209 switch (a->mode) {
210 case IEEE802154_SCF_KEY_INDEX:
211 return true;
212 case IEEE802154_SCF_KEY_SHORT_INDEX:
213 return a->short_source == b->short_source;
214 case IEEE802154_SCF_KEY_HW_INDEX:
215 return a->extended_source == b->extended_source;
216 }
217
218 return false;
219}
220
221int mac802154_llsec_key_add(struct mac802154_llsec *sec,
222 const struct ieee802154_llsec_key_id *id,
223 const struct ieee802154_llsec_key *key)
224{
225 struct mac802154_llsec_key *mkey = NULL;
226 struct ieee802154_llsec_key_entry *pos, *new;
227
228 if (!(key->frame_types & (1 << IEEE802154_FC_TYPE_MAC_CMD)) &&
229 key->cmd_frame_ids)
230 return -EINVAL;
231
232 list_for_each_entry(pos, &sec->table.keys, list) {
233 if (llsec_key_id_equal(&pos->id, id))
234 return -EEXIST;
235
236 if (memcmp(pos->key->key, key->key,
237 IEEE802154_LLSEC_KEY_SIZE))
238 continue;
239
240 mkey = container_of(pos->key, struct mac802154_llsec_key, key);
241
242
243
244
245
246 if (pos->key->frame_types != key->frame_types ||
247 pos->key->cmd_frame_ids != key->cmd_frame_ids)
248 return -EEXIST;
249
250 break;
251 }
252
253 new = kzalloc(sizeof(*new), GFP_KERNEL);
254 if (!new)
255 return -ENOMEM;
256
257 if (!mkey)
258 mkey = llsec_key_alloc(key);
259 else
260 mkey = llsec_key_get(mkey);
261
262 if (!mkey)
263 goto fail;
264
265 new->id = *id;
266 new->key = &mkey->key;
267
268 list_add_rcu(&new->list, &sec->table.keys);
269
270 return 0;
271
272fail:
273 kfree(new);
274 return -ENOMEM;
275}
276
277int mac802154_llsec_key_del(struct mac802154_llsec *sec,
278 const struct ieee802154_llsec_key_id *key)
279{
280 struct ieee802154_llsec_key_entry *pos;
281
282 list_for_each_entry(pos, &sec->table.keys, list) {
283 struct mac802154_llsec_key *mkey;
284
285 mkey = container_of(pos->key, struct mac802154_llsec_key, key);
286
287 if (llsec_key_id_equal(&pos->id, key)) {
288 list_del_rcu(&pos->list);
289 llsec_key_put(mkey);
290 return 0;
291 }
292 }
293
294 return -ENOENT;
295}
296
297
298
299static bool llsec_dev_use_shortaddr(__le16 short_addr)
300{
301 return short_addr != cpu_to_le16(IEEE802154_ADDR_UNDEF) &&
302 short_addr != cpu_to_le16(0xffff);
303}
304
305static u32 llsec_dev_hash_short(__le16 short_addr, __le16 pan_id)
306{
307 return ((__force u16) short_addr) << 16 | (__force u16) pan_id;
308}
309
310static u64 llsec_dev_hash_long(__le64 hwaddr)
311{
312 return (__force u64) hwaddr;
313}
314
315static struct mac802154_llsec_device*
316llsec_dev_find_short(struct mac802154_llsec *sec, __le16 short_addr,
317 __le16 pan_id)
318{
319 struct mac802154_llsec_device *dev;
320 u32 key = llsec_dev_hash_short(short_addr, pan_id);
321
322 hash_for_each_possible_rcu(sec->devices_short, dev, bucket_s, key) {
323 if (dev->dev.short_addr == short_addr &&
324 dev->dev.pan_id == pan_id)
325 return dev;
326 }
327
328 return NULL;
329}
330
331static struct mac802154_llsec_device*
332llsec_dev_find_long(struct mac802154_llsec *sec, __le64 hwaddr)
333{
334 struct mac802154_llsec_device *dev;
335 u64 key = llsec_dev_hash_long(hwaddr);
336
337 hash_for_each_possible_rcu(sec->devices_hw, dev, bucket_hw, key) {
338 if (dev->dev.hwaddr == hwaddr)
339 return dev;
340 }
341
342 return NULL;
343}
344
345static void llsec_dev_free(struct mac802154_llsec_device *dev)
346{
347 struct ieee802154_llsec_device_key *pos, *pn;
348 struct mac802154_llsec_device_key *devkey;
349
350 list_for_each_entry_safe(pos, pn, &dev->dev.keys, list) {
351 devkey = container_of(pos, struct mac802154_llsec_device_key,
352 devkey);
353
354 list_del(&pos->list);
355 kfree(devkey);
356 }
357
358 kfree(dev);
359}
360
361int mac802154_llsec_dev_add(struct mac802154_llsec *sec,
362 const struct ieee802154_llsec_device *dev)
363{
364 struct mac802154_llsec_device *entry;
365 u32 skey = llsec_dev_hash_short(dev->short_addr, dev->pan_id);
366 u64 hwkey = llsec_dev_hash_long(dev->hwaddr);
367
368 BUILD_BUG_ON(sizeof(hwkey) != IEEE802154_ADDR_LEN);
369
370 if ((llsec_dev_use_shortaddr(dev->short_addr) &&
371 llsec_dev_find_short(sec, dev->short_addr, dev->pan_id)) ||
372 llsec_dev_find_long(sec, dev->hwaddr))
373 return -EEXIST;
374
375 entry = kmalloc(sizeof(*entry), GFP_KERNEL);
376 if (!entry)
377 return -ENOMEM;
378
379 entry->dev = *dev;
380 spin_lock_init(&entry->lock);
381 INIT_LIST_HEAD(&entry->dev.keys);
382
383 if (llsec_dev_use_shortaddr(dev->short_addr))
384 hash_add_rcu(sec->devices_short, &entry->bucket_s, skey);
385 else
386 INIT_HLIST_NODE(&entry->bucket_s);
387
388 hash_add_rcu(sec->devices_hw, &entry->bucket_hw, hwkey);
389 list_add_tail_rcu(&entry->dev.list, &sec->table.devices);
390
391 return 0;
392}
393
394static void llsec_dev_free_rcu(struct rcu_head *rcu)
395{
396 llsec_dev_free(container_of(rcu, struct mac802154_llsec_device, rcu));
397}
398
399int mac802154_llsec_dev_del(struct mac802154_llsec *sec, __le64 device_addr)
400{
401 struct mac802154_llsec_device *pos;
402
403 pos = llsec_dev_find_long(sec, device_addr);
404 if (!pos)
405 return -ENOENT;
406
407 hash_del_rcu(&pos->bucket_s);
408 hash_del_rcu(&pos->bucket_hw);
409 call_rcu(&pos->rcu, llsec_dev_free_rcu);
410
411 return 0;
412}
413
414
415
416static struct mac802154_llsec_device_key*
417llsec_devkey_find(struct mac802154_llsec_device *dev,
418 const struct ieee802154_llsec_key_id *key)
419{
420 struct ieee802154_llsec_device_key *devkey;
421
422 list_for_each_entry_rcu(devkey, &dev->dev.keys, list) {
423 if (!llsec_key_id_equal(key, &devkey->key_id))
424 continue;
425
426 return container_of(devkey, struct mac802154_llsec_device_key,
427 devkey);
428 }
429
430 return NULL;
431}
432
433int mac802154_llsec_devkey_add(struct mac802154_llsec *sec,
434 __le64 dev_addr,
435 const struct ieee802154_llsec_device_key *key)
436{
437 struct mac802154_llsec_device *dev;
438 struct mac802154_llsec_device_key *devkey;
439
440 dev = llsec_dev_find_long(sec, dev_addr);
441
442 if (!dev)
443 return -ENOENT;
444
445 if (llsec_devkey_find(dev, &key->key_id))
446 return -EEXIST;
447
448 devkey = kmalloc(sizeof(*devkey), GFP_KERNEL);
449 if (!devkey)
450 return -ENOMEM;
451
452 devkey->devkey = *key;
453 list_add_tail_rcu(&devkey->devkey.list, &dev->dev.keys);
454 return 0;
455}
456
457int mac802154_llsec_devkey_del(struct mac802154_llsec *sec,
458 __le64 dev_addr,
459 const struct ieee802154_llsec_device_key *key)
460{
461 struct mac802154_llsec_device *dev;
462 struct mac802154_llsec_device_key *devkey;
463
464 dev = llsec_dev_find_long(sec, dev_addr);
465
466 if (!dev)
467 return -ENOENT;
468
469 devkey = llsec_devkey_find(dev, &key->key_id);
470 if (!devkey)
471 return -ENOENT;
472
473 list_del_rcu(&devkey->devkey.list);
474 kfree_rcu(devkey, rcu);
475 return 0;
476}
477
478
479
480static struct mac802154_llsec_seclevel*
481llsec_find_seclevel(const struct mac802154_llsec *sec,
482 const struct ieee802154_llsec_seclevel *sl)
483{
484 struct ieee802154_llsec_seclevel *pos;
485
486 list_for_each_entry(pos, &sec->table.security_levels, list) {
487 if (pos->frame_type != sl->frame_type ||
488 (pos->frame_type == IEEE802154_FC_TYPE_MAC_CMD &&
489 pos->cmd_frame_id != sl->cmd_frame_id) ||
490 pos->device_override != sl->device_override ||
491 pos->sec_levels != sl->sec_levels)
492 continue;
493
494 return container_of(pos, struct mac802154_llsec_seclevel,
495 level);
496 }
497
498 return NULL;
499}
500
501int mac802154_llsec_seclevel_add(struct mac802154_llsec *sec,
502 const struct ieee802154_llsec_seclevel *sl)
503{
504 struct mac802154_llsec_seclevel *entry;
505
506 if (llsec_find_seclevel(sec, sl))
507 return -EEXIST;
508
509 entry = kmalloc(sizeof(*entry), GFP_KERNEL);
510 if (!entry)
511 return -ENOMEM;
512
513 entry->level = *sl;
514
515 list_add_tail_rcu(&entry->level.list, &sec->table.security_levels);
516
517 return 0;
518}
519
520int mac802154_llsec_seclevel_del(struct mac802154_llsec *sec,
521 const struct ieee802154_llsec_seclevel *sl)
522{
523 struct mac802154_llsec_seclevel *pos;
524
525 pos = llsec_find_seclevel(sec, sl);
526 if (!pos)
527 return -ENOENT;
528
529 list_del_rcu(&pos->level.list);
530 kfree_rcu(pos, rcu);
531
532 return 0;
533}
534
535
536
537static int llsec_recover_addr(struct mac802154_llsec *sec,
538 struct ieee802154_addr *addr)
539{
540 __le16 caddr = sec->params.coord_shortaddr;
541
542 addr->pan_id = sec->params.pan_id;
543
544 if (caddr == cpu_to_le16(IEEE802154_ADDR_BROADCAST)) {
545 return -EINVAL;
546 } else if (caddr == cpu_to_le16(IEEE802154_ADDR_UNDEF)) {
547 addr->extended_addr = sec->params.coord_hwaddr;
548 addr->mode = IEEE802154_ADDR_LONG;
549 } else {
550 addr->short_addr = sec->params.coord_shortaddr;
551 addr->mode = IEEE802154_ADDR_SHORT;
552 }
553
554 return 0;
555}
556
557static struct mac802154_llsec_key*
558llsec_lookup_key(struct mac802154_llsec *sec,
559 const struct ieee802154_hdr *hdr,
560 const struct ieee802154_addr *addr,
561 struct ieee802154_llsec_key_id *key_id)
562{
563 struct ieee802154_addr devaddr = *addr;
564 u8 key_id_mode = hdr->sec.key_id_mode;
565 struct ieee802154_llsec_key_entry *key_entry;
566 struct mac802154_llsec_key *key;
567
568 if (key_id_mode == IEEE802154_SCF_KEY_IMPLICIT &&
569 devaddr.mode == IEEE802154_ADDR_NONE) {
570 if (hdr->fc.type == IEEE802154_FC_TYPE_BEACON) {
571 devaddr.extended_addr = sec->params.coord_hwaddr;
572 devaddr.mode = IEEE802154_ADDR_LONG;
573 } else if (llsec_recover_addr(sec, &devaddr) < 0) {
574 return NULL;
575 }
576 }
577
578 list_for_each_entry_rcu(key_entry, &sec->table.keys, list) {
579 const struct ieee802154_llsec_key_id *id = &key_entry->id;
580
581 if (!(key_entry->key->frame_types & BIT(hdr->fc.type)))
582 continue;
583
584 if (id->mode != key_id_mode)
585 continue;
586
587 if (key_id_mode == IEEE802154_SCF_KEY_IMPLICIT) {
588 if (ieee802154_addr_equal(&devaddr, &id->device_addr))
589 goto found;
590 } else {
591 if (id->id != hdr->sec.key_id)
592 continue;
593
594 if ((key_id_mode == IEEE802154_SCF_KEY_INDEX) ||
595 (key_id_mode == IEEE802154_SCF_KEY_SHORT_INDEX &&
596 id->short_source == hdr->sec.short_src) ||
597 (key_id_mode == IEEE802154_SCF_KEY_HW_INDEX &&
598 id->extended_source == hdr->sec.extended_src))
599 goto found;
600 }
601 }
602
603 return NULL;
604
605found:
606 key = container_of(key_entry->key, struct mac802154_llsec_key, key);
607 if (key_id)
608 *key_id = key_entry->id;
609 return llsec_key_get(key);
610}
611
612
613static void llsec_geniv(u8 iv[16], __le64 addr,
614 const struct ieee802154_sechdr *sec)
615{
616 __be64 addr_bytes = (__force __be64) swab64((__force u64) addr);
617 __be32 frame_counter = (__force __be32) swab32((__force u32) sec->frame_counter);
618
619 iv[0] = 1;
620 memcpy(iv + 1, &addr_bytes, sizeof(addr_bytes));
621 memcpy(iv + 9, &frame_counter, sizeof(frame_counter));
622 iv[13] = sec->level;
623 iv[14] = 0;
624 iv[15] = 1;
625}
626
627static int
628llsec_do_encrypt_unauth(struct sk_buff *skb, const struct mac802154_llsec *sec,
629 const struct ieee802154_hdr *hdr,
630 struct mac802154_llsec_key *key)
631{
632 u8 iv[16];
633 struct scatterlist src;
634 struct blkcipher_desc req = {
635 .tfm = key->tfm0,
636 .info = iv,
637 .flags = 0,
638 };
639
640 llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
641 sg_init_one(&src, skb->data, skb->len);
642 return crypto_blkcipher_encrypt_iv(&req, &src, &src, skb->len);
643}
644
645static struct crypto_aead*
646llsec_tfm_by_len(struct mac802154_llsec_key *key, int authlen)
647{
648 int i;
649
650 for (i = 0; i < ARRAY_SIZE(key->tfm); i++)
651 if (crypto_aead_authsize(key->tfm[i]) == authlen)
652 return key->tfm[i];
653
654 BUG();
655}
656
657static int
658llsec_do_encrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec,
659 const struct ieee802154_hdr *hdr,
660 struct mac802154_llsec_key *key)
661{
662 u8 iv[16];
663 unsigned char *data;
664 int authlen, assoclen, datalen, rc;
665 struct scatterlist src, assoc[2], dst[2];
666 struct aead_request *req;
667
668 authlen = ieee802154_sechdr_authtag_len(&hdr->sec);
669 llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
670
671 req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC);
672 if (!req)
673 return -ENOMEM;
674
675 sg_init_table(assoc, 2);
676 sg_set_buf(&assoc[0], skb_mac_header(skb), skb->mac_len);
677 assoclen = skb->mac_len;
678
679 data = skb_mac_header(skb) + skb->mac_len;
680 datalen = skb_tail_pointer(skb) - data;
681
682 if (hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC) {
683 sg_set_buf(&assoc[1], data, 0);
684 } else {
685 sg_set_buf(&assoc[1], data, datalen);
686 assoclen += datalen;
687 datalen = 0;
688 }
689
690 sg_init_one(&src, data, datalen);
691
692 sg_init_table(dst, 2);
693 sg_set_buf(&dst[0], data, datalen);
694 sg_set_buf(&dst[1], skb_put(skb, authlen), authlen);
695
696 aead_request_set_callback(req, 0, NULL, NULL);
697 aead_request_set_assoc(req, assoc, assoclen);
698 aead_request_set_crypt(req, &src, dst, datalen, iv);
699
700 rc = crypto_aead_encrypt(req);
701
702 kfree(req);
703
704 return rc;
705}
706
707static int llsec_do_encrypt(struct sk_buff *skb,
708 const struct mac802154_llsec *sec,
709 const struct ieee802154_hdr *hdr,
710 struct mac802154_llsec_key *key)
711{
712 if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC)
713 return llsec_do_encrypt_unauth(skb, sec, hdr, key);
714 else
715 return llsec_do_encrypt_auth(skb, sec, hdr, key);
716}
717
718int mac802154_llsec_encrypt(struct mac802154_llsec *sec, struct sk_buff *skb)
719{
720 struct ieee802154_hdr hdr;
721 int rc, authlen, hlen;
722 struct mac802154_llsec_key *key;
723 u32 frame_ctr;
724
725 hlen = ieee802154_hdr_pull(skb, &hdr);
726
727 if (hlen < 0 || hdr.fc.type != IEEE802154_FC_TYPE_DATA)
728 return -EINVAL;
729
730 if (!hdr.fc.security_enabled || hdr.sec.level == 0) {
731 skb_push(skb, hlen);
732 return 0;
733 }
734
735 authlen = ieee802154_sechdr_authtag_len(&hdr.sec);
736
737 if (skb->len + hlen + authlen + IEEE802154_MFR_SIZE > IEEE802154_MTU)
738 return -EMSGSIZE;
739
740 rcu_read_lock();
741
742 read_lock_bh(&sec->lock);
743
744 if (!sec->params.enabled) {
745 rc = -EINVAL;
746 goto fail_read;
747 }
748
749 key = llsec_lookup_key(sec, &hdr, &hdr.dest, NULL);
750 if (!key) {
751 rc = -ENOKEY;
752 goto fail_read;
753 }
754
755 read_unlock_bh(&sec->lock);
756
757 write_lock_bh(&sec->lock);
758
759 frame_ctr = be32_to_cpu(sec->params.frame_counter);
760 hdr.sec.frame_counter = cpu_to_le32(frame_ctr);
761 if (frame_ctr == 0xFFFFFFFF) {
762 write_unlock_bh(&sec->lock);
763 llsec_key_put(key);
764 rc = -EOVERFLOW;
765 goto fail;
766 }
767
768 sec->params.frame_counter = cpu_to_be32(frame_ctr + 1);
769
770 write_unlock_bh(&sec->lock);
771
772 rcu_read_unlock();
773
774 skb->mac_len = ieee802154_hdr_push(skb, &hdr);
775 skb_reset_mac_header(skb);
776
777 rc = llsec_do_encrypt(skb, sec, &hdr, key);
778 llsec_key_put(key);
779
780 return rc;
781
782fail_read:
783 read_unlock_bh(&sec->lock);
784fail:
785 rcu_read_unlock();
786 return rc;
787}
788
789
790
791static struct mac802154_llsec_device*
792llsec_lookup_dev(struct mac802154_llsec *sec,
793 const struct ieee802154_addr *addr)
794{
795 struct ieee802154_addr devaddr = *addr;
796 struct mac802154_llsec_device *dev = NULL;
797
798 if (devaddr.mode == IEEE802154_ADDR_NONE &&
799 llsec_recover_addr(sec, &devaddr) < 0)
800 return NULL;
801
802 if (devaddr.mode == IEEE802154_ADDR_SHORT) {
803 u32 key = llsec_dev_hash_short(devaddr.short_addr,
804 devaddr.pan_id);
805
806 hash_for_each_possible_rcu(sec->devices_short, dev,
807 bucket_s, key) {
808 if (dev->dev.pan_id == devaddr.pan_id &&
809 dev->dev.short_addr == devaddr.short_addr)
810 return dev;
811 }
812 } else {
813 u64 key = llsec_dev_hash_long(devaddr.extended_addr);
814
815 hash_for_each_possible_rcu(sec->devices_hw, dev,
816 bucket_hw, key) {
817 if (dev->dev.hwaddr == devaddr.extended_addr)
818 return dev;
819 }
820 }
821
822 return NULL;
823}
824
825static int
826llsec_lookup_seclevel(const struct mac802154_llsec *sec,
827 u8 frame_type, u8 cmd_frame_id,
828 struct ieee802154_llsec_seclevel *rlevel)
829{
830 struct ieee802154_llsec_seclevel *level;
831
832 list_for_each_entry_rcu(level, &sec->table.security_levels, list) {
833 if (level->frame_type == frame_type &&
834 (frame_type != IEEE802154_FC_TYPE_MAC_CMD ||
835 level->cmd_frame_id == cmd_frame_id)) {
836 *rlevel = *level;
837 return 0;
838 }
839 }
840
841 return -EINVAL;
842}
843
844static int
845llsec_do_decrypt_unauth(struct sk_buff *skb, const struct mac802154_llsec *sec,
846 const struct ieee802154_hdr *hdr,
847 struct mac802154_llsec_key *key, __le64 dev_addr)
848{
849 u8 iv[16];
850 unsigned char *data;
851 int datalen;
852 struct scatterlist src;
853 struct blkcipher_desc req = {
854 .tfm = key->tfm0,
855 .info = iv,
856 .flags = 0,
857 };
858
859 llsec_geniv(iv, dev_addr, &hdr->sec);
860 data = skb_mac_header(skb) + skb->mac_len;
861 datalen = skb_tail_pointer(skb) - data;
862
863 sg_init_one(&src, data, datalen);
864
865 return crypto_blkcipher_decrypt_iv(&req, &src, &src, datalen);
866}
867
868static int
869llsec_do_decrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec,
870 const struct ieee802154_hdr *hdr,
871 struct mac802154_llsec_key *key, __le64 dev_addr)
872{
873 u8 iv[16];
874 unsigned char *data;
875 int authlen, datalen, assoclen, rc;
876 struct scatterlist src, assoc[2];
877 struct aead_request *req;
878
879 authlen = ieee802154_sechdr_authtag_len(&hdr->sec);
880 llsec_geniv(iv, dev_addr, &hdr->sec);
881
882 req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC);
883 if (!req)
884 return -ENOMEM;
885
886 sg_init_table(assoc, 2);
887 sg_set_buf(&assoc[0], skb_mac_header(skb), skb->mac_len);
888 assoclen = skb->mac_len;
889
890 data = skb_mac_header(skb) + skb->mac_len;
891 datalen = skb_tail_pointer(skb) - data;
892
893 if (hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC) {
894 sg_set_buf(&assoc[1], data, 0);
895 } else {
896 sg_set_buf(&assoc[1], data, datalen - authlen);
897 assoclen += datalen - authlen;
898 data += datalen - authlen;
899 datalen = authlen;
900 }
901
902 sg_init_one(&src, data, datalen);
903
904 aead_request_set_callback(req, 0, NULL, NULL);
905 aead_request_set_assoc(req, assoc, assoclen);
906 aead_request_set_crypt(req, &src, &src, datalen, iv);
907
908 rc = crypto_aead_decrypt(req);
909
910 kfree(req);
911 skb_trim(skb, skb->len - authlen);
912
913 return rc;
914}
915
916static int
917llsec_do_decrypt(struct sk_buff *skb, const struct mac802154_llsec *sec,
918 const struct ieee802154_hdr *hdr,
919 struct mac802154_llsec_key *key, __le64 dev_addr)
920{
921 if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC)
922 return llsec_do_decrypt_unauth(skb, sec, hdr, key, dev_addr);
923 else
924 return llsec_do_decrypt_auth(skb, sec, hdr, key, dev_addr);
925}
926
927static int
928llsec_update_devkey_record(struct mac802154_llsec_device *dev,
929 const struct ieee802154_llsec_key_id *in_key)
930{
931 struct mac802154_llsec_device_key *devkey;
932
933 devkey = llsec_devkey_find(dev, in_key);
934
935 if (!devkey) {
936 struct mac802154_llsec_device_key *next;
937
938 next = kzalloc(sizeof(*devkey), GFP_ATOMIC);
939 if (!next)
940 return -ENOMEM;
941
942 next->devkey.key_id = *in_key;
943
944 spin_lock_bh(&dev->lock);
945
946 devkey = llsec_devkey_find(dev, in_key);
947 if (!devkey)
948 list_add_rcu(&next->devkey.list, &dev->dev.keys);
949 else
950 kfree(next);
951
952 spin_unlock_bh(&dev->lock);
953 }
954
955 return 0;
956}
957
958static int
959llsec_update_devkey_info(struct mac802154_llsec_device *dev,
960 const struct ieee802154_llsec_key_id *in_key,
961 u32 frame_counter)
962{
963 struct mac802154_llsec_device_key *devkey = NULL;
964
965 if (dev->dev.key_mode == IEEE802154_LLSEC_DEVKEY_RESTRICT) {
966 devkey = llsec_devkey_find(dev, in_key);
967 if (!devkey)
968 return -ENOENT;
969 }
970
971 if (dev->dev.key_mode == IEEE802154_LLSEC_DEVKEY_RECORD) {
972 int rc = llsec_update_devkey_record(dev, in_key);
973
974 if (rc < 0)
975 return rc;
976 }
977
978 spin_lock_bh(&dev->lock);
979
980 if ((!devkey && frame_counter < dev->dev.frame_counter) ||
981 (devkey && frame_counter < devkey->devkey.frame_counter)) {
982 spin_unlock_bh(&dev->lock);
983 return -EINVAL;
984 }
985
986 if (devkey)
987 devkey->devkey.frame_counter = frame_counter + 1;
988 else
989 dev->dev.frame_counter = frame_counter + 1;
990
991 spin_unlock_bh(&dev->lock);
992
993 return 0;
994}
995
996int mac802154_llsec_decrypt(struct mac802154_llsec *sec, struct sk_buff *skb)
997{
998 struct ieee802154_hdr hdr;
999 struct mac802154_llsec_key *key;
1000 struct ieee802154_llsec_key_id key_id;
1001 struct mac802154_llsec_device *dev;
1002 struct ieee802154_llsec_seclevel seclevel;
1003 int err;
1004 __le64 dev_addr;
1005 u32 frame_ctr;
1006
1007 if (ieee802154_hdr_peek(skb, &hdr) < 0)
1008 return -EINVAL;
1009 if (!hdr.fc.security_enabled)
1010 return 0;
1011 if (hdr.fc.version == 0)
1012 return -EINVAL;
1013
1014 read_lock_bh(&sec->lock);
1015 if (!sec->params.enabled) {
1016 read_unlock_bh(&sec->lock);
1017 return -EINVAL;
1018 }
1019 read_unlock_bh(&sec->lock);
1020
1021 rcu_read_lock();
1022
1023 key = llsec_lookup_key(sec, &hdr, &hdr.source, &key_id);
1024 if (!key) {
1025 err = -ENOKEY;
1026 goto fail;
1027 }
1028
1029 dev = llsec_lookup_dev(sec, &hdr.source);
1030 if (!dev) {
1031 err = -EINVAL;
1032 goto fail_dev;
1033 }
1034
1035 if (llsec_lookup_seclevel(sec, hdr.fc.type, 0, &seclevel) < 0) {
1036 err = -EINVAL;
1037 goto fail_dev;
1038 }
1039
1040 if (!(seclevel.sec_levels & BIT(hdr.sec.level)) &&
1041 (hdr.sec.level == 0 && seclevel.device_override &&
1042 !dev->dev.seclevel_exempt)) {
1043 err = -EINVAL;
1044 goto fail_dev;
1045 }
1046
1047 frame_ctr = le32_to_cpu(hdr.sec.frame_counter);
1048
1049 if (frame_ctr == 0xffffffff) {
1050 err = -EOVERFLOW;
1051 goto fail_dev;
1052 }
1053
1054 err = llsec_update_devkey_info(dev, &key_id, frame_ctr);
1055 if (err)
1056 goto fail_dev;
1057
1058 dev_addr = dev->dev.hwaddr;
1059
1060 rcu_read_unlock();
1061
1062 err = llsec_do_decrypt(skb, sec, &hdr, key, dev_addr);
1063 llsec_key_put(key);
1064 return err;
1065
1066fail_dev:
1067 llsec_key_put(key);
1068fail:
1069 rcu_read_unlock();
1070 return err;
1071}
1072
