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