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