blob: 2a4b68e2a934f608f03ea45afdce890119eed46c [file] [log] [blame]
Phoebe Buckheister5d637d52014-05-16 17:46:37 +02001/*
2 * Copyright (C) 2014 Fraunhofer ITWM
3 *
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2
6 * as published by the Free Software Foundation.
7 *
8 * This program is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * Written by:
14 * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
15 */
16
17#include <linux/err.h>
18#include <linux/bug.h>
19#include <linux/completion.h>
20#include <net/ieee802154.h>
21
22#include "mac802154.h"
23#include "llsec.h"
24
25static void llsec_key_put(struct mac802154_llsec_key *key);
26static bool llsec_key_id_equal(const struct ieee802154_llsec_key_id *a,
27 const struct ieee802154_llsec_key_id *b);
28
29static void llsec_dev_free(struct mac802154_llsec_device *dev);
30
31void mac802154_llsec_init(struct mac802154_llsec *sec)
32{
33 memset(sec, 0, sizeof(*sec));
34
35 memset(&sec->params.default_key_source, 0xFF, IEEE802154_ADDR_LEN);
36
37 INIT_LIST_HEAD(&sec->table.security_levels);
38 INIT_LIST_HEAD(&sec->table.devices);
39 INIT_LIST_HEAD(&sec->table.keys);
40 hash_init(sec->devices_short);
41 hash_init(sec->devices_hw);
42 rwlock_init(&sec->lock);
43}
44
45void mac802154_llsec_destroy(struct mac802154_llsec *sec)
46{
47 struct ieee802154_llsec_seclevel *sl, *sn;
48 struct ieee802154_llsec_device *dev, *dn;
49 struct ieee802154_llsec_key_entry *key, *kn;
50
51 list_for_each_entry_safe(sl, sn, &sec->table.security_levels, list) {
52 struct mac802154_llsec_seclevel *msl;
53
54 msl = container_of(sl, struct mac802154_llsec_seclevel, level);
55 list_del(&sl->list);
56 kfree(msl);
57 }
58
59 list_for_each_entry_safe(dev, dn, &sec->table.devices, list) {
60 struct mac802154_llsec_device *mdev;
61
62 mdev = container_of(dev, struct mac802154_llsec_device, dev);
63 list_del(&dev->list);
64 llsec_dev_free(mdev);
65 }
66
67 list_for_each_entry_safe(key, kn, &sec->table.keys, list) {
68 struct mac802154_llsec_key *mkey;
69
70 mkey = container_of(key->key, struct mac802154_llsec_key, key);
71 list_del(&key->list);
72 llsec_key_put(mkey);
73 kfree(key);
74 }
75}
76
77
78
79int mac802154_llsec_get_params(struct mac802154_llsec *sec,
80 struct ieee802154_llsec_params *params)
81{
82 read_lock_bh(&sec->lock);
83 *params = sec->params;
84 read_unlock_bh(&sec->lock);
85
86 return 0;
87}
88
89int mac802154_llsec_set_params(struct mac802154_llsec *sec,
90 const struct ieee802154_llsec_params *params,
91 int changed)
92{
93 write_lock_bh(&sec->lock);
94
95 if (changed & IEEE802154_LLSEC_PARAM_ENABLED)
96 sec->params.enabled = params->enabled;
97 if (changed & IEEE802154_LLSEC_PARAM_FRAME_COUNTER)
98 sec->params.frame_counter = params->frame_counter;
99 if (changed & IEEE802154_LLSEC_PARAM_OUT_LEVEL)
100 sec->params.out_level = params->out_level;
101 if (changed & IEEE802154_LLSEC_PARAM_OUT_KEY)
102 sec->params.out_key = params->out_key;
103 if (changed & IEEE802154_LLSEC_PARAM_KEY_SOURCE)
104 sec->params.default_key_source = params->default_key_source;
105 if (changed & IEEE802154_LLSEC_PARAM_PAN_ID)
106 sec->params.pan_id = params->pan_id;
107 if (changed & IEEE802154_LLSEC_PARAM_HWADDR)
108 sec->params.hwaddr = params->hwaddr;
109 if (changed & IEEE802154_LLSEC_PARAM_COORD_HWADDR)
110 sec->params.coord_hwaddr = params->coord_hwaddr;
111 if (changed & IEEE802154_LLSEC_PARAM_COORD_SHORTADDR)
112 sec->params.coord_shortaddr = params->coord_shortaddr;
113
114 write_unlock_bh(&sec->lock);
115
116 return 0;
117}
118
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 (!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_blkcipher("ctr(aes)", 0, CRYPTO_ALG_ASYNC);
150 if (!key->tfm0)
151 goto err_tfm;
152
153 if (crypto_blkcipher_setkey(key->tfm0, template->key,
154 IEEE802154_LLSEC_KEY_SIZE))
155 goto err_tfm0;
156
157 return key;
158
159err_tfm0:
160 crypto_free_blkcipher(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 kfree(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_blkcipher(key->tfm0);
181 kfree(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_SHORT_INDEX:
210 return a->short_source == b->short_source;
211 case IEEE802154_SCF_KEY_HW_INDEX:
212 return a->extended_source == b->extended_source;
213 }
214
215 return false;
216}
217
218int mac802154_llsec_key_add(struct mac802154_llsec *sec,
219 const struct ieee802154_llsec_key_id *id,
220 const struct ieee802154_llsec_key *key)
221{
222 struct mac802154_llsec_key *mkey = NULL;
223 struct ieee802154_llsec_key_entry *pos, *new;
224
225 if (!(key->frame_types & (1 << IEEE802154_FC_TYPE_MAC_CMD)) &&
226 key->cmd_frame_ids)
227 return -EINVAL;
228
229 list_for_each_entry(pos, &sec->table.keys, list) {
230 if (llsec_key_id_equal(&pos->id, id))
231 return -EEXIST;
232
233 if (memcmp(pos->key->key, key->key,
234 IEEE802154_LLSEC_KEY_SIZE))
235 continue;
236
237 mkey = container_of(pos->key, struct mac802154_llsec_key, key);
238
239 /* Don't allow multiple instances of the same AES key to have
240 * different allowed frame types/command frame ids, as this is
241 * not possible in the 802.15.4 PIB.
242 */
243 if (pos->key->frame_types != key->frame_types ||
244 pos->key->cmd_frame_ids != key->cmd_frame_ids)
245 return -EEXIST;
246
247 break;
248 }
249
250 new = kzalloc(sizeof(*new), GFP_KERNEL);
251 if (!new)
252 return -ENOMEM;
253
254 if (!mkey)
255 mkey = llsec_key_alloc(key);
256 else
257 mkey = llsec_key_get(mkey);
258
259 if (!mkey)
260 goto fail;
261
262 new->id = *id;
263 new->key = &mkey->key;
264
265 list_add_rcu(&new->list, &sec->table.keys);
266
267 return 0;
268
269fail:
270 kfree(new);
271 return -ENOMEM;
272}
273
274int mac802154_llsec_key_del(struct mac802154_llsec *sec,
275 const struct ieee802154_llsec_key_id *key)
276{
277 struct ieee802154_llsec_key_entry *pos;
278
279 list_for_each_entry(pos, &sec->table.keys, list) {
280 struct mac802154_llsec_key *mkey;
281
282 mkey = container_of(pos->key, struct mac802154_llsec_key, key);
283
284 if (llsec_key_id_equal(&pos->id, key)) {
285 llsec_key_put(mkey);
286 return 0;
287 }
288 }
289
290 return -ENOENT;
291}
292
293
294
295static bool llsec_dev_use_shortaddr(__le16 short_addr)
296{
297 return short_addr != cpu_to_le16(IEEE802154_ADDR_UNDEF) &&
298 short_addr != cpu_to_le16(0xffff);
299}
300
301static u32 llsec_dev_hash_short(__le16 short_addr, __le16 pan_id)
302{
303 return ((__force u16) short_addr) << 16 | (__force u16) pan_id;
304}
305
306static u64 llsec_dev_hash_long(__le64 hwaddr)
307{
308 return (__force u64) hwaddr;
309}
310
311static struct mac802154_llsec_device*
312llsec_dev_find_short(struct mac802154_llsec *sec, __le16 short_addr,
313 __le16 pan_id)
314{
315 struct mac802154_llsec_device *dev;
316 u32 key = llsec_dev_hash_short(short_addr, pan_id);
317
318 hash_for_each_possible_rcu(sec->devices_short, dev, bucket_s, key) {
319 if (dev->dev.short_addr == short_addr &&
320 dev->dev.pan_id == pan_id)
321 return dev;
322 }
323
324 return NULL;
325}
326
327static struct mac802154_llsec_device*
328llsec_dev_find_long(struct mac802154_llsec *sec, __le64 hwaddr)
329{
330 struct mac802154_llsec_device *dev;
331 u64 key = llsec_dev_hash_long(hwaddr);
332
333 hash_for_each_possible_rcu(sec->devices_hw, dev, bucket_hw, key) {
334 if (dev->dev.hwaddr == hwaddr)
335 return dev;
336 }
337
338 return NULL;
339}
340
341static void llsec_dev_free(struct mac802154_llsec_device *dev)
342{
343 struct ieee802154_llsec_device_key *pos, *pn;
344 struct mac802154_llsec_device_key *devkey;
345
346 list_for_each_entry_safe(pos, pn, &dev->dev.keys, list) {
347 devkey = container_of(pos, struct mac802154_llsec_device_key,
348 devkey);
349
350 list_del(&pos->list);
351 kfree(devkey);
352 }
353
354 kfree(dev);
355}
356
357int mac802154_llsec_dev_add(struct mac802154_llsec *sec,
358 const struct ieee802154_llsec_device *dev)
359{
360 struct mac802154_llsec_device *entry;
361 u32 skey = llsec_dev_hash_short(dev->short_addr, dev->pan_id);
362 u64 hwkey = llsec_dev_hash_long(dev->hwaddr);
363
364 BUILD_BUG_ON(sizeof(hwkey) != IEEE802154_ADDR_LEN);
365
366 if ((llsec_dev_use_shortaddr(dev->short_addr) &&
367 llsec_dev_find_short(sec, dev->short_addr, dev->pan_id)) ||
368 llsec_dev_find_long(sec, dev->hwaddr))
369 return -EEXIST;
370
371 entry = kmalloc(sizeof(*entry), GFP_KERNEL);
372 if (!entry)
373 return -ENOMEM;
374
375 entry->dev = *dev;
376 spin_lock_init(&entry->lock);
377 INIT_LIST_HEAD(&entry->dev.keys);
378
379 if (llsec_dev_use_shortaddr(dev->short_addr))
380 hash_add_rcu(sec->devices_short, &entry->bucket_s, skey);
381 else
382 INIT_HLIST_NODE(&entry->bucket_s);
383
384 hash_add_rcu(sec->devices_hw, &entry->bucket_hw, hwkey);
385 list_add_tail_rcu(&entry->dev.list, &sec->table.devices);
386
387 return 0;
388}
389
390static void llsec_dev_free_rcu(struct rcu_head *rcu)
391{
392 llsec_dev_free(container_of(rcu, struct mac802154_llsec_device, rcu));
393}
394
395int mac802154_llsec_dev_del(struct mac802154_llsec *sec, __le64 device_addr)
396{
397 struct mac802154_llsec_device *pos;
398
399 pos = llsec_dev_find_long(sec, device_addr);
400 if (!pos)
401 return -ENOENT;
402
403 hash_del_rcu(&pos->bucket_s);
404 hash_del_rcu(&pos->bucket_hw);
405 call_rcu(&pos->rcu, llsec_dev_free_rcu);
406
407 return 0;
408}
409
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
474
475
476static struct mac802154_llsec_seclevel*
477llsec_find_seclevel(const struct mac802154_llsec *sec,
478 const struct ieee802154_llsec_seclevel *sl)
479{
480 struct ieee802154_llsec_seclevel *pos;
481
482 list_for_each_entry(pos, &sec->table.security_levels, list) {
483 if (pos->frame_type != sl->frame_type ||
484 (pos->frame_type == IEEE802154_FC_TYPE_MAC_CMD &&
485 pos->cmd_frame_id != sl->cmd_frame_id) ||
486 pos->device_override != sl->device_override ||
487 pos->sec_levels != sl->sec_levels)
488 continue;
489
490 return container_of(pos, struct mac802154_llsec_seclevel,
491 level);
492 }
493
494 return NULL;
495}
496
497int mac802154_llsec_seclevel_add(struct mac802154_llsec *sec,
498 const struct ieee802154_llsec_seclevel *sl)
499{
500 struct mac802154_llsec_seclevel *entry;
501
502 if (llsec_find_seclevel(sec, sl))
503 return -EEXIST;
504
505 entry = kmalloc(sizeof(*entry), GFP_KERNEL);
506 if (!entry)
507 return -ENOMEM;
508
509 entry->level = *sl;
510
511 list_add_tail_rcu(&entry->level.list, &sec->table.security_levels);
512
513 return 0;
514}
515
516int mac802154_llsec_seclevel_del(struct mac802154_llsec *sec,
517 const struct ieee802154_llsec_seclevel *sl)
518{
519 struct mac802154_llsec_seclevel *pos;
520
521 pos = llsec_find_seclevel(sec, sl);
522 if (!pos)
523 return -ENOENT;
524
525 list_del_rcu(&pos->level.list);
526 kfree_rcu(pos, rcu);
527
528 return 0;
529}
Phoebe Buckheister03556e42014-05-16 17:46:38 +0200530
531
532
533static int llsec_recover_addr(struct mac802154_llsec *sec,
534 struct ieee802154_addr *addr)
535{
536 __le16 caddr = sec->params.coord_shortaddr;
537 addr->pan_id = sec->params.pan_id;
538
539 if (caddr == cpu_to_le16(IEEE802154_ADDR_BROADCAST)) {
540 return -EINVAL;
541 } else if (caddr == cpu_to_le16(IEEE802154_ADDR_UNDEF)) {
542 addr->extended_addr = sec->params.coord_hwaddr;
543 addr->mode = IEEE802154_ADDR_LONG;
544 } else {
545 addr->short_addr = sec->params.coord_shortaddr;
546 addr->mode = IEEE802154_ADDR_SHORT;
547 }
548
549 return 0;
550}
551
552static struct mac802154_llsec_key*
553llsec_lookup_key(struct mac802154_llsec *sec,
554 const struct ieee802154_hdr *hdr,
555 const struct ieee802154_addr *addr,
556 struct ieee802154_llsec_key_id *key_id)
557{
558 struct ieee802154_addr devaddr = *addr;
559 u8 key_id_mode = hdr->sec.key_id_mode;
560 struct ieee802154_llsec_key_entry *key_entry;
561 struct mac802154_llsec_key *key;
562
563 if (key_id_mode == IEEE802154_SCF_KEY_IMPLICIT &&
564 devaddr.mode == IEEE802154_ADDR_NONE) {
565 if (hdr->fc.type == IEEE802154_FC_TYPE_BEACON) {
566 devaddr.extended_addr = sec->params.coord_hwaddr;
567 devaddr.mode = IEEE802154_ADDR_LONG;
568 } else if (llsec_recover_addr(sec, &devaddr) < 0) {
569 return NULL;
570 }
571 }
572
573 list_for_each_entry_rcu(key_entry, &sec->table.keys, list) {
574 const struct ieee802154_llsec_key_id *id = &key_entry->id;
575
576 if (!(key_entry->key->frame_types & BIT(hdr->fc.type)))
577 continue;
578
579 if (id->mode != key_id_mode)
580 continue;
581
582 if (key_id_mode == IEEE802154_SCF_KEY_IMPLICIT) {
583 if (ieee802154_addr_equal(&devaddr, &id->device_addr))
584 goto found;
585 } else {
586 if (id->id != hdr->sec.key_id)
587 continue;
588
589 if ((key_id_mode == IEEE802154_SCF_KEY_INDEX) ||
590 (key_id_mode == IEEE802154_SCF_KEY_SHORT_INDEX &&
591 id->short_source == hdr->sec.short_src) ||
592 (key_id_mode == IEEE802154_SCF_KEY_HW_INDEX &&
593 id->extended_source == hdr->sec.extended_src))
594 goto found;
595 }
596 }
597
598 return NULL;
599
600found:
601 key = container_of(key_entry->key, struct mac802154_llsec_key, key);
602 if (key_id)
603 *key_id = key_entry->id;
604 return llsec_key_get(key);
605}
606
607
608static void llsec_geniv(u8 iv[16], __le64 addr,
609 const struct ieee802154_sechdr *sec)
610{
611 __be64 addr_bytes = (__force __be64) swab64((__force u64) addr);
612 __be32 frame_counter = (__force __be32) swab32((__force u32) sec->frame_counter);
613
614 iv[0] = 1; /* L' = L - 1 = 1 */
615 memcpy(iv + 1, &addr_bytes, sizeof(addr_bytes));
616 memcpy(iv + 9, &frame_counter, sizeof(frame_counter));
617 iv[13] = sec->level;
618 iv[14] = 0;
619 iv[15] = 1;
620}
621
622static int
623llsec_do_encrypt_unauth(struct sk_buff *skb, const struct mac802154_llsec *sec,
624 const struct ieee802154_hdr *hdr,
625 struct mac802154_llsec_key *key)
626{
627 u8 iv[16];
628 struct scatterlist src;
629 struct blkcipher_desc req = {
630 .tfm = key->tfm0,
631 .info = iv,
632 .flags = 0,
633 };
634
635 llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
636 sg_init_one(&src, skb->data, skb->len);
637 return crypto_blkcipher_encrypt_iv(&req, &src, &src, skb->len);
638}
639
640static struct crypto_aead*
641llsec_tfm_by_len(struct mac802154_llsec_key *key, int authlen)
642{
643 int i;
644
645 for (i = 0; i < ARRAY_SIZE(key->tfm); i++)
646 if (crypto_aead_authsize(key->tfm[i]) == authlen)
647 return key->tfm[i];
648
649 BUG();
650}
651
652static int
653llsec_do_encrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec,
654 const struct ieee802154_hdr *hdr,
655 struct mac802154_llsec_key *key)
656{
657 u8 iv[16];
658 unsigned char *data;
659 int authlen, assoclen, datalen, rc;
660 struct scatterlist src, assoc[2], dst[2];
661 struct aead_request *req;
662
663 authlen = ieee802154_sechdr_authtag_len(&hdr->sec);
664 llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
665
666 req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC);
667 if (!req)
668 return -ENOMEM;
669
670 sg_init_table(assoc, 2);
671 sg_set_buf(&assoc[0], skb_mac_header(skb), skb->mac_len);
672 assoclen = skb->mac_len;
673
674 data = skb_mac_header(skb) + skb->mac_len;
675 datalen = skb_tail_pointer(skb) - data;
676
677 if (hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC) {
678 sg_set_buf(&assoc[1], data, 0);
679 } else {
680 sg_set_buf(&assoc[1], data, datalen);
681 assoclen += datalen;
682 datalen = 0;
683 }
684
685 sg_init_one(&src, data, datalen);
686
687 sg_init_table(dst, 2);
688 sg_set_buf(&dst[0], data, datalen);
689 sg_set_buf(&dst[1], skb_put(skb, authlen), authlen);
690
691 aead_request_set_callback(req, 0, NULL, NULL);
692 aead_request_set_assoc(req, assoc, assoclen);
693 aead_request_set_crypt(req, &src, dst, datalen, iv);
694
695 rc = crypto_aead_encrypt(req);
696
697 kfree(req);
698
699 return rc;
700}
701
702static int llsec_do_encrypt(struct sk_buff *skb,
703 const struct mac802154_llsec *sec,
704 const struct ieee802154_hdr *hdr,
705 struct mac802154_llsec_key *key)
706{
707 if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC)
708 return llsec_do_encrypt_unauth(skb, sec, hdr, key);
709 else
710 return llsec_do_encrypt_auth(skb, sec, hdr, key);
711}
712
713int mac802154_llsec_encrypt(struct mac802154_llsec *sec, struct sk_buff *skb)
714{
715 struct ieee802154_hdr hdr;
716 int rc, authlen, hlen;
717 struct mac802154_llsec_key *key;
718 u32 frame_ctr;
719
720 hlen = ieee802154_hdr_pull(skb, &hdr);
721
722 if (hlen < 0 || hdr.fc.type != IEEE802154_FC_TYPE_DATA)
723 return -EINVAL;
724
725 if (!hdr.fc.security_enabled || hdr.sec.level == 0) {
726 skb_push(skb, hlen);
727 return 0;
728 }
729
730 authlen = ieee802154_sechdr_authtag_len(&hdr.sec);
731
732 if (skb->len + hlen + authlen + IEEE802154_MFR_SIZE > IEEE802154_MTU)
733 return -EMSGSIZE;
734
735 rcu_read_lock();
736
737 read_lock_bh(&sec->lock);
738
739 if (!sec->params.enabled) {
740 rc = -EINVAL;
741 goto fail_read;
742 }
743
744 key = llsec_lookup_key(sec, &hdr, &hdr.dest, NULL);
745 if (!key) {
746 rc = -ENOKEY;
747 goto fail_read;
748 }
749
750 read_unlock_bh(&sec->lock);
751
752 write_lock_bh(&sec->lock);
753
754 frame_ctr = be32_to_cpu(sec->params.frame_counter);
755 hdr.sec.frame_counter = cpu_to_le32(frame_ctr);
756 if (frame_ctr == 0xFFFFFFFF) {
757 write_unlock_bh(&sec->lock);
758 llsec_key_put(key);
759 rc = -EOVERFLOW;
760 goto fail;
761 }
762
763 sec->params.frame_counter = cpu_to_be32(frame_ctr + 1);
764
765 write_unlock_bh(&sec->lock);
766
767 rcu_read_unlock();
768
769 skb->mac_len = ieee802154_hdr_push(skb, &hdr);
770 skb_reset_mac_header(skb);
771
772 rc = llsec_do_encrypt(skb, sec, &hdr, key);
773 llsec_key_put(key);
774
775 return rc < 0 ? rc : 0;
776
777fail_read:
778 read_unlock(&sec->lock);
779fail:
780 rcu_read_unlock();
781 return rc;
782}