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 <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;
 627
 628         llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
 629         sg_init_one(&src, skb->data, skb->len);
 630         skcipher_request_set_tfm(req, key->tfm0);
 631         skcipher_request_set_callback(req, 0, NULL, NULL);
 632         skcipher_request_set_crypt(req, &src, &src, skb->len, iv);
 633         err = crypto_skcipher_encrypt(req);
 634         skcipher_request_zero(req);
 635         return err;
 636 }
 637
 638 static struct crypto_aead*
 639 llsec_tfm_by_len(struct mac802154_llsec_key *key, int authlen)
 640 {
 641         int i;
 642
 643         for (i = 0; i < ARRAY_SIZE(key->tfm); i++)
 644                 if (crypto_aead_authsize(key->tfm[i]) == authlen)
 645                         return key->tfm[i];
 646
 647         BUG();
 648 }
 649
 650 static int
 651 llsec_do_encrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec,
 652                       const struct ieee802154_hdr *hdr,
 653                       struct mac802154_llsec_key *key)
 654 {
 655         u8 iv[16];
 656         unsigned char *data;
 657         int authlen, assoclen, datalen, rc;
 658         struct scatterlist sg;
 659         struct aead_request *req;
 660
 661         authlen = ieee802154_sechdr_authtag_len(&hdr->sec);
 662         llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
 663
 664         req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC);
 665         if (!req)
 666                 return -ENOMEM;
 667
 668         assoclen = skb->mac_len;
 669
 670         data = skb_mac_header(skb) + skb->mac_len;
 671         datalen = skb_tail_pointer(skb) - data;
 672
 673         skb_put(skb, authlen);
 674
 675         sg_init_one(&sg, skb_mac_header(skb), assoclen + datalen + authlen);
 676
 677         if (!(hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC)) {
 678                 assoclen += datalen;
 679                 datalen = 0;
 680         }
 681
 682         aead_request_set_callback(req, 0, NULL, NULL);
 683         aead_request_set_crypt(req, &sg, &sg, datalen, iv);
 684         aead_request_set_ad(req, assoclen);
 685
 686         rc = crypto_aead_encrypt(req);
 687
 688         kzfree(req);
 689
 690         return rc;
 691 }
 692
 693 static int llsec_do_encrypt(struct sk_buff *skb,
 694                             const struct mac802154_llsec *sec,
 695                             const struct ieee802154_hdr *hdr,
 696                             struct mac802154_llsec_key *key)
 697 {
 698         if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC)
 699                 return llsec_do_encrypt_unauth(skb, sec, hdr, key);
 700         else
 701                 return llsec_do_encrypt_auth(skb, sec, hdr, key);
 702 }
 703
 704 int mac802154_llsec_encrypt(struct mac802154_llsec *sec, struct sk_buff *skb)
 705 {
 706         struct ieee802154_hdr hdr;
 707         int rc, authlen, hlen;
 708         struct mac802154_llsec_key *key;
 709         u32 frame_ctr;
 710
 711         hlen = ieee802154_hdr_pull(skb, &hdr);
 712
 713         if (hlen < 0 || hdr.fc.type != IEEE802154_FC_TYPE_DATA)
 714                 return -EINVAL;
 715
 716         if (!hdr.fc.security_enabled || hdr.sec.level == 0) {
 717                 skb_push(skb, hlen);
 718                 return 0;
 719         }
 720
 721         authlen = ieee802154_sechdr_authtag_len(&hdr.sec);
 722
 723         if (skb->len + hlen + authlen + IEEE802154_MFR_SIZE > IEEE802154_MTU)
 724                 return -EMSGSIZE;
 725
 726         rcu_read_lock();
 727
 728         read_lock_bh(&sec->lock);
 729
 730         if (!sec->params.enabled) {
 731                 rc = -EINVAL;
 732                 goto fail_read;
 733         }
 734
 735         key = llsec_lookup_key(sec, &hdr, &hdr.dest, NULL);
 736         if (!key) {
 737                 rc = -ENOKEY;
 738                 goto fail_read;
 739         }
 740
 741         read_unlock_bh(&sec->lock);
 742
 743         write_lock_bh(&sec->lock);
 744
 745         frame_ctr = be32_to_cpu(sec->params.frame_counter);
 746         hdr.sec.frame_counter = cpu_to_le32(frame_ctr);
 747         if (frame_ctr == 0xFFFFFFFF) {
 748                 write_unlock_bh(&sec->lock);
 749                 llsec_key_put(key);
 750                 rc = -EOVERFLOW;
 751                 goto fail;
 752         }
 753
 754         sec->params.frame_counter = cpu_to_be32(frame_ctr + 1);
 755
 756         write_unlock_bh(&sec->lock);
 757
 758         rcu_read_unlock();
 759
 760         skb->mac_len = ieee802154_hdr_push(skb, &hdr);
 761         skb_reset_mac_header(skb);
 762
 763         rc = llsec_do_encrypt(skb, sec, &hdr, key);
 764         llsec_key_put(key);
 765
 766         return rc;
 767
 768 fail_read:
 769         read_unlock_bh(&sec->lock);
 770 fail:
 771         rcu_read_unlock();
 772         return rc;
 773 }
 774
 775 static struct mac802154_llsec_device*
 776 llsec_lookup_dev(struct mac802154_llsec *sec,
 777                  const struct ieee802154_addr *addr)
 778 {
 779         struct ieee802154_addr devaddr = *addr;
 780         struct mac802154_llsec_device *dev = NULL;
 781
 782         if (devaddr.mode == IEEE802154_ADDR_NONE &&
 783             llsec_recover_addr(sec, &devaddr) < 0)
 784                 return NULL;
 785
 786         if (devaddr.mode == IEEE802154_ADDR_SHORT) {
 787                 u32 key = llsec_dev_hash_short(devaddr.short_addr,
 788                                                devaddr.pan_id);
 789
 790                 hash_for_each_possible_rcu(sec->devices_short, dev,
 791                                            bucket_s, key) {
 792                         if (dev->dev.pan_id == devaddr.pan_id &&
 793                             dev->dev.short_addr == devaddr.short_addr)
 794                                 return dev;
 795                 }
 796         } else {
 797                 u64 key = llsec_dev_hash_long(devaddr.extended_addr);
 798
 799                 hash_for_each_possible_rcu(sec->devices_hw, dev,
 800                                            bucket_hw, key) {
 801                         if (dev->dev.hwaddr == devaddr.extended_addr)
 802                                 return dev;
 803                 }
 804         }
 805
 806         return NULL;
 807 }
 808
 809 static int
 810 llsec_lookup_seclevel(const struct mac802154_llsec *sec,
 811                       u8 frame_type, u8 cmd_frame_id,
 812                       struct ieee802154_llsec_seclevel *rlevel)
 813 {
 814         struct ieee802154_llsec_seclevel *level;
 815
 816         list_for_each_entry_rcu(level, &sec->table.security_levels, list) {
 817                 if (level->frame_type == frame_type &&
 818                     (frame_type != IEEE802154_FC_TYPE_MAC_CMD ||
 819                      level->cmd_frame_id == cmd_frame_id)) {
 820                         *rlevel = *level;
 821                         return 0;
 822                 }
 823         }
 824
 825         return -EINVAL;
 826 }
 827
 828 static int
 829 llsec_do_decrypt_unauth(struct sk_buff *skb, const struct mac802154_llsec *sec,
 830                         const struct ieee802154_hdr *hdr,
 831                         struct mac802154_llsec_key *key, __le64 dev_addr)
 832 {
 833         u8 iv[16];
 834         unsigned char *data;
 835         int datalen;
 836         struct scatterlist src;
 837         SKCIPHER_REQUEST_ON_STACK(req, key->tfm0);
 838         int err;
 839
 840         llsec_geniv(iv, dev_addr, &hdr->sec);
 841         data = skb_mac_header(skb) + skb->mac_len;
 842         datalen = skb_tail_pointer(skb) - data;
 843
 844         sg_init_one(&src, data, datalen);
 845
 846         skcipher_request_set_tfm(req, key->tfm0);
 847         skcipher_request_set_callback(req, 0, NULL, NULL);
 848         skcipher_request_set_crypt(req, &src, &src, datalen, iv);
 849
 850         err = crypto_skcipher_decrypt(req);
 851         skcipher_request_zero(req);
 852         return err;
 853 }
 854
 855 static int
 856 llsec_do_decrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec,
 857                       const struct ieee802154_hdr *hdr,
 858                       struct mac802154_llsec_key *key, __le64 dev_addr)
 859 {
 860         u8 iv[16];
 861         unsigned char *data;
 862         int authlen, datalen, assoclen, rc;
 863         struct scatterlist sg;
 864         struct aead_request *req;
 865
 866         authlen = ieee802154_sechdr_authtag_len(&hdr->sec);
 867         llsec_geniv(iv, dev_addr, &hdr->sec);
 868
 869         req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC);
 870         if (!req)
 871                 return -ENOMEM;
 872
 873         assoclen = skb->mac_len;
 874
 875         data = skb_mac_header(skb) + skb->mac_len;
 876         datalen = skb_tail_pointer(skb) - data;
 877
 878         sg_init_one(&sg, skb_mac_header(skb), assoclen + datalen);
 879
 880         if (!(hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC)) {
 881                 assoclen += datalen - authlen;
 882                 datalen = authlen;
 883         }
 884
 885         aead_request_set_callback(req, 0, NULL, NULL);
 886         aead_request_set_crypt(req, &sg, &sg, datalen, iv);
 887         aead_request_set_ad(req, assoclen);
 888
 889         rc = crypto_aead_decrypt(req);
 890
 891         kzfree(req);
 892         skb_trim(skb, skb->len - authlen);
 893
 894         return rc;
 895 }
 896
 897 static int
 898 llsec_do_decrypt(struct sk_buff *skb, const struct mac802154_llsec *sec,
 899                  const struct ieee802154_hdr *hdr,
 900                  struct mac802154_llsec_key *key, __le64 dev_addr)
 901 {
 902         if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC)
 903                 return llsec_do_decrypt_unauth(skb, sec, hdr, key, dev_addr);
 904         else
 905                 return llsec_do_decrypt_auth(skb, sec, hdr, key, dev_addr);
 906 }
 907
 908 static int
 909 llsec_update_devkey_record(struct mac802154_llsec_device *dev,
 910                            const struct ieee802154_llsec_key_id *in_key)
 911 {
 912         struct mac802154_llsec_device_key *devkey;
 913
 914         devkey = llsec_devkey_find(dev, in_key);
 915
 916         if (!devkey) {
 917                 struct mac802154_llsec_device_key *next;
 918
 919                 next = kzalloc(sizeof(*devkey), GFP_ATOMIC);
 920                 if (!next)
 921                         return -ENOMEM;
 922
 923                 next->devkey.key_id = *in_key;
 924
 925                 spin_lock_bh(&dev->lock);
 926
 927                 devkey = llsec_devkey_find(dev, in_key);
 928                 if (!devkey)
 929                         list_add_rcu(&next->devkey.list, &dev->dev.keys);
 930                 else
 931                         kzfree(next);
 932
 933                 spin_unlock_bh(&dev->lock);
 934         }
 935
 936         return 0;
 937 }
 938
 939 static int
 940 llsec_update_devkey_info(struct mac802154_llsec_device *dev,
 941                          const struct ieee802154_llsec_key_id *in_key,
 942                          u32 frame_counter)
 943 {
 944         struct mac802154_llsec_device_key *devkey = NULL;
 945
 946         if (dev->dev.key_mode == IEEE802154_LLSEC_DEVKEY_RESTRICT) {
 947                 devkey = llsec_devkey_find(dev, in_key);
 948                 if (!devkey)
 949                         return -ENOENT;
 950         }
 951
 952         if (dev->dev.key_mode == IEEE802154_LLSEC_DEVKEY_RECORD) {
 953                 int rc = llsec_update_devkey_record(dev, in_key);
 954
 955                 if (rc < 0)
 956                         return rc;
 957         }
 958
 959         spin_lock_bh(&dev->lock);
 960
 961         if ((!devkey && frame_counter < dev->dev.frame_counter) ||
 962             (devkey && frame_counter < devkey->devkey.frame_counter)) {
 963                 spin_unlock_bh(&dev->lock);
 964                 return -EINVAL;
 965         }
 966
 967         if (devkey)
 968                 devkey->devkey.frame_counter = frame_counter + 1;
 969         else
 970                 dev->dev.frame_counter = frame_counter + 1;
 971
 972         spin_unlock_bh(&dev->lock);
 973
 974         return 0;
 975 }
 976
 977 int mac802154_llsec_decrypt(struct mac802154_llsec *sec, struct sk_buff *skb)
 978 {
 979         struct ieee802154_hdr hdr;
 980         struct mac802154_llsec_key *key;
 981         struct ieee802154_llsec_key_id key_id;
 982         struct mac802154_llsec_device *dev;
 983         struct ieee802154_llsec_seclevel seclevel;
 984         int err;
 985         __le64 dev_addr;
 986         u32 frame_ctr;
 987
 988         if (ieee802154_hdr_peek(skb, &hdr) < 0)
 989                 return -EINVAL;
 990         if (!hdr.fc.security_enabled)
 991                 return 0;
 992         if (hdr.fc.version == 0)
 993                 return -EINVAL;
 994
 995         read_lock_bh(&sec->lock);
 996         if (!sec->params.enabled) {
 997                 read_unlock_bh(&sec->lock);
 998                 return -EINVAL;
 999         }
1000         read_unlock_bh(&sec->lock);
1001
1002         rcu_read_lock();
1003
1004         key = llsec_lookup_key(sec, &hdr, &hdr.source, &key_id);
1005         if (!key) {
1006                 err = -ENOKEY;
1007                 goto fail;
1008         }
1009
1010         dev = llsec_lookup_dev(sec, &hdr.source);
1011         if (!dev) {
1012                 err = -EINVAL;
1013                 goto fail_dev;
1014         }
1015
1016         if (llsec_lookup_seclevel(sec, hdr.fc.type, 0, &seclevel) < 0) {
1017                 err = -EINVAL;
1018                 goto fail_dev;
1019         }
1020
1021         if (!(seclevel.sec_levels & BIT(hdr.sec.level)) &&
1022             (hdr.sec.level == 0 && seclevel.device_override &&
1023              !dev->dev.seclevel_exempt)) {
1024                 err = -EINVAL;
1025                 goto fail_dev;
1026         }
1027
1028         frame_ctr = le32_to_cpu(hdr.sec.frame_counter);
1029
1030         if (frame_ctr == 0xffffffff) {
1031                 err = -EOVERFLOW;
1032                 goto fail_dev;
1033         }
1034
1035         err = llsec_update_devkey_info(dev, &key_id, frame_ctr);
1036         if (err)
1037                 goto fail_dev;
1038
1039         dev_addr = dev->dev.hwaddr;
1040
1041         rcu_read_unlock();
1042
1043         err = llsec_do_decrypt(skb, sec, &hdr, key, dev_addr);
1044         llsec_key_put(key);
1045         return err;
1046
1047 fail_dev:
1048         llsec_key_put(key);
1049 fail:
1050         rcu_read_unlock();
1051         return err;
1052 }