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