net/mac802154/llsec.c

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