net/mac802154/llsec.c

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