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