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