net/mac802154/llsec.c

   1 /*
   2  * Copyright (C) 2014 Fraunhofer ITWM
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License version 2
   6  * as published by the Free Software Foundation.
   7  *
   8  * This program is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * Written by:
  14  * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
  15  */
  16
  17 #include <linux/err.h>
  18 #include <linux/bug.h>
  19 #include <linux/completion.h>
  20 #include <net/ieee802154.h>
  21
  22 #include "mac802154.h"
  23 #include "llsec.h"
  24
  25 static void llsec_key_put(struct mac802154_llsec_key *key);
  26 static bool llsec_key_id_equal(const struct ieee802154_llsec_key_id *a,
  27                                const struct ieee802154_llsec_key_id *b);
  28
  29 static void llsec_dev_free(struct mac802154_llsec_device *dev);
  30
  31 void mac802154_llsec_init(struct mac802154_llsec *sec)
  32 {
  33         memset(sec, 0, sizeof(*sec));
  34
  35         memset(&sec->params.default_key_source, 0xFF, IEEE802154_ADDR_LEN);
  36
  37         INIT_LIST_HEAD(&sec->table.security_levels);
  38         INIT_LIST_HEAD(&sec->table.devices);
  39         INIT_LIST_HEAD(&sec->table.keys);
  40         hash_init(sec->devices_short);
  41         hash_init(sec->devices_hw);
  42         rwlock_init(&sec->lock);
  43 }
  44
  45 void mac802154_llsec_destroy(struct mac802154_llsec *sec)
  46 {
  47         struct ieee802154_llsec_seclevel *sl, *sn;
  48         struct ieee802154_llsec_device *dev, *dn;
  49         struct ieee802154_llsec_key_entry *key, *kn;
  50
  51         list_for_each_entry_safe(sl, sn, &sec->table.security_levels, list) {
  52                 struct mac802154_llsec_seclevel *msl;
  53
  54                 msl = container_of(sl, struct mac802154_llsec_seclevel, level);
  55                 list_del(&sl->list);
  56                 kfree(msl);
  57         }
  58
  59         list_for_each_entry_safe(dev, dn, &sec->table.devices, list) {
  60                 struct mac802154_llsec_device *mdev;
  61
  62                 mdev = container_of(dev, struct mac802154_llsec_device, dev);
  63                 list_del(&dev->list);
  64                 llsec_dev_free(mdev);
  65         }
  66
  67         list_for_each_entry_safe(key, kn, &sec->table.keys, list) {
  68                 struct mac802154_llsec_key *mkey;
  69
  70                 mkey = container_of(key->key, struct mac802154_llsec_key, key);
  71                 list_del(&key->list);
  72                 llsec_key_put(mkey);
  73                 kfree(key);
  74         }
  75 }
  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
 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 (!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_blkcipher("ctr(aes)", 0, CRYPTO_ALG_ASYNC);
 150         if (!key->tfm0)
 151                 goto err_tfm;
 152
 153         if (crypto_blkcipher_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_blkcipher(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         kfree(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_blkcipher(key->tfm0);
 181         kfree(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_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         kfree(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                         llsec_key_put(mkey);
 286                         return 0;
 287                 }
 288         }
 289
 290         return -ENOENT;
 291 }
 292
 293
 294
 295 static bool llsec_dev_use_shortaddr(__le16 short_addr)
 296 {
 297         return short_addr != cpu_to_le16(IEEE802154_ADDR_UNDEF) &&
 298                 short_addr != cpu_to_le16(0xffff);
 299 }
 300
 301 static u32 llsec_dev_hash_short(__le16 short_addr, __le16 pan_id)
 302 {
 303         return ((__force u16) short_addr) << 16 | (__force u16) pan_id;
 304 }
 305
 306 static u64 llsec_dev_hash_long(__le64 hwaddr)
 307 {
 308         return (__force u64) hwaddr;
 309 }
 310
 311 static struct mac802154_llsec_device*
 312 llsec_dev_find_short(struct mac802154_llsec *sec, __le16 short_addr,
 313                      __le16 pan_id)
 314 {
 315         struct mac802154_llsec_device *dev;
 316         u32 key = llsec_dev_hash_short(short_addr, pan_id);
 317
 318         hash_for_each_possible_rcu(sec->devices_short, dev, bucket_s, key) {
 319                 if (dev->dev.short_addr == short_addr &&
 320                     dev->dev.pan_id == pan_id)
 321                         return dev;
 322         }
 323
 324         return NULL;
 325 }
 326
 327 static struct mac802154_llsec_device*
 328 llsec_dev_find_long(struct mac802154_llsec *sec, __le64 hwaddr)
 329 {
 330         struct mac802154_llsec_device *dev;
 331         u64 key = llsec_dev_hash_long(hwaddr);
 332
 333         hash_for_each_possible_rcu(sec->devices_hw, dev, bucket_hw, key) {
 334                 if (dev->dev.hwaddr == hwaddr)
 335                         return dev;
 336         }
 337
 338         return NULL;
 339 }
 340
 341 static void llsec_dev_free(struct mac802154_llsec_device *dev)
 342 {
 343         struct ieee802154_llsec_device_key *pos, *pn;
 344         struct mac802154_llsec_device_key *devkey;
 345
 346         list_for_each_entry_safe(pos, pn, &dev->dev.keys, list) {
 347                 devkey = container_of(pos, struct mac802154_llsec_device_key,
 348                                       devkey);
 349
 350                 list_del(&pos->list);
 351                 kfree(devkey);
 352         }
 353
 354         kfree(dev);
 355 }
 356
 357 int mac802154_llsec_dev_add(struct mac802154_llsec *sec,
 358                             const struct ieee802154_llsec_device *dev)
 359 {
 360         struct mac802154_llsec_device *entry;
 361         u32 skey = llsec_dev_hash_short(dev->short_addr, dev->pan_id);
 362         u64 hwkey = llsec_dev_hash_long(dev->hwaddr);
 363
 364         BUILD_BUG_ON(sizeof(hwkey) != IEEE802154_ADDR_LEN);
 365
 366         if ((llsec_dev_use_shortaddr(dev->short_addr) &&
 367              llsec_dev_find_short(sec, dev->short_addr, dev->pan_id)) ||
 368              llsec_dev_find_long(sec, dev->hwaddr))
 369                 return -EEXIST;
 370
 371         entry = kmalloc(sizeof(*entry), GFP_KERNEL);
 372         if (!entry)
 373                 return -ENOMEM;
 374
 375         entry->dev = *dev;
 376         spin_lock_init(&entry->lock);
 377         INIT_LIST_HEAD(&entry->dev.keys);
 378
 379         if (llsec_dev_use_shortaddr(dev->short_addr))
 380                 hash_add_rcu(sec->devices_short, &entry->bucket_s, skey);
 381         else
 382                 INIT_HLIST_NODE(&entry->bucket_s);
 383
 384         hash_add_rcu(sec->devices_hw, &entry->bucket_hw, hwkey);
 385         list_add_tail_rcu(&entry->dev.list, &sec->table.devices);
 386
 387         return 0;
 388 }
 389
 390 static void llsec_dev_free_rcu(struct rcu_head *rcu)
 391 {
 392         llsec_dev_free(container_of(rcu, struct mac802154_llsec_device, rcu));
 393 }
 394
 395 int mac802154_llsec_dev_del(struct mac802154_llsec *sec, __le64 device_addr)
 396 {
 397         struct mac802154_llsec_device *pos;
 398
 399         pos = llsec_dev_find_long(sec, device_addr);
 400         if (!pos)
 401                 return -ENOENT;
 402
 403         hash_del_rcu(&pos->bucket_s);
 404         hash_del_rcu(&pos->bucket_hw);
 405         call_rcu(&pos->rcu, llsec_dev_free_rcu);
 406
 407         return 0;
 408 }
 409
 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
 475
 476 static struct mac802154_llsec_seclevel*
 477 llsec_find_seclevel(const struct mac802154_llsec *sec,
 478                     const struct ieee802154_llsec_seclevel *sl)
 479 {
 480         struct ieee802154_llsec_seclevel *pos;
 481
 482         list_for_each_entry(pos, &sec->table.security_levels, list) {
 483                 if (pos->frame_type != sl->frame_type ||
 484                     (pos->frame_type == IEEE802154_FC_TYPE_MAC_CMD &&
 485                      pos->cmd_frame_id != sl->cmd_frame_id) ||
 486                     pos->device_override != sl->device_override ||
 487                     pos->sec_levels != sl->sec_levels)
 488                         continue;
 489
 490                 return container_of(pos, struct mac802154_llsec_seclevel,
 491                                     level);
 492         }
 493
 494         return NULL;
 495 }
 496
 497 int mac802154_llsec_seclevel_add(struct mac802154_llsec *sec,
 498                                  const struct ieee802154_llsec_seclevel *sl)
 499 {
 500         struct mac802154_llsec_seclevel *entry;
 501
 502         if (llsec_find_seclevel(sec, sl))
 503                 return -EEXIST;
 504
 505         entry = kmalloc(sizeof(*entry), GFP_KERNEL);
 506         if (!entry)
 507                 return -ENOMEM;
 508
 509         entry->level = *sl;
 510
 511         list_add_tail_rcu(&entry->level.list, &sec->table.security_levels);
 512
 513         return 0;
 514 }
 515
 516 int mac802154_llsec_seclevel_del(struct mac802154_llsec *sec,
 517                                  const struct ieee802154_llsec_seclevel *sl)
 518 {
 519         struct mac802154_llsec_seclevel *pos;
 520
 521         pos = llsec_find_seclevel(sec, sl);
 522         if (!pos)
 523                 return -ENOENT;
 524
 525         list_del_rcu(&pos->level.list);
 526         kfree_rcu(pos, rcu);
 527
 528         return 0;
 529 }
 530
 531
 532
 533 static int llsec_recover_addr(struct mac802154_llsec *sec,
 534                               struct ieee802154_addr *addr)
 535 {
 536         __le16 caddr = sec->params.coord_shortaddr;
 537         addr->pan_id = sec->params.pan_id;
 538
 539         if (caddr == cpu_to_le16(IEEE802154_ADDR_BROADCAST)) {
 540                 return -EINVAL;
 541         } else if (caddr == cpu_to_le16(IEEE802154_ADDR_UNDEF)) {
 542                 addr->extended_addr = sec->params.coord_hwaddr;
 543                 addr->mode = IEEE802154_ADDR_LONG;
 544         } else {
 545                 addr->short_addr = sec->params.coord_shortaddr;
 546                 addr->mode = IEEE802154_ADDR_SHORT;
 547         }
 548
 549         return 0;
 550 }
 551
 552 static struct mac802154_llsec_key*
 553 llsec_lookup_key(struct mac802154_llsec *sec,
 554                  const struct ieee802154_hdr *hdr,
 555                  const struct ieee802154_addr *addr,
 556                  struct ieee802154_llsec_key_id *key_id)
 557 {
 558         struct ieee802154_addr devaddr = *addr;
 559         u8 key_id_mode = hdr->sec.key_id_mode;
 560         struct ieee802154_llsec_key_entry *key_entry;
 561         struct mac802154_llsec_key *key;
 562
 563         if (key_id_mode == IEEE802154_SCF_KEY_IMPLICIT &&
 564             devaddr.mode == IEEE802154_ADDR_NONE) {
 565                 if (hdr->fc.type == IEEE802154_FC_TYPE_BEACON) {
 566                         devaddr.extended_addr = sec->params.coord_hwaddr;
 567                         devaddr.mode = IEEE802154_ADDR_LONG;
 568                 } else if (llsec_recover_addr(sec, &devaddr) < 0) {
 569                         return NULL;
 570                 }
 571         }
 572
 573         list_for_each_entry_rcu(key_entry, &sec->table.keys, list) {
 574                 const struct ieee802154_llsec_key_id *id = &key_entry->id;
 575
 576                 if (!(key_entry->key->frame_types & BIT(hdr->fc.type)))
 577                         continue;
 578
 579                 if (id->mode != key_id_mode)
 580                         continue;
 581
 582                 if (key_id_mode == IEEE802154_SCF_KEY_IMPLICIT) {
 583                         if (ieee802154_addr_equal(&devaddr, &id->device_addr))
 584                                 goto found;
 585                 } else {
 586                         if (id->id != hdr->sec.key_id)
 587                                 continue;
 588
 589                         if ((key_id_mode == IEEE802154_SCF_KEY_INDEX) ||
 590                             (key_id_mode == IEEE802154_SCF_KEY_SHORT_INDEX &&
 591                              id->short_source == hdr->sec.short_src) ||
 592                             (key_id_mode == IEEE802154_SCF_KEY_HW_INDEX &&
 593                              id->extended_source == hdr->sec.extended_src))
 594                                 goto found;
 595                 }
 596         }
 597
 598         return NULL;
 599
 600 found:
 601         key = container_of(key_entry->key, struct mac802154_llsec_key, key);
 602         if (key_id)
 603                 *key_id = key_entry->id;
 604         return llsec_key_get(key);
 605 }
 606
 607
 608 static void llsec_geniv(u8 iv[16], __le64 addr,
 609                         const struct ieee802154_sechdr *sec)
 610 {
 611         __be64 addr_bytes = (__force __be64) swab64((__force u64) addr);
 612         __be32 frame_counter = (__force __be32) swab32((__force u32) sec->frame_counter);
 613
 614         iv[0] = 1; /* L' = L - 1 = 1 */
 615         memcpy(iv + 1, &addr_bytes, sizeof(addr_bytes));
 616         memcpy(iv + 9, &frame_counter, sizeof(frame_counter));
 617         iv[13] = sec->level;
 618         iv[14] = 0;
 619         iv[15] = 1;
 620 }
 621
 622 static int
 623 llsec_do_encrypt_unauth(struct sk_buff *skb, const struct mac802154_llsec *sec,
 624                         const struct ieee802154_hdr *hdr,
 625                         struct mac802154_llsec_key *key)
 626 {
 627         u8 iv[16];
 628         struct scatterlist src;
 629         struct blkcipher_desc req = {
 630                 .tfm = key->tfm0,
 631                 .info = iv,
 632                 .flags = 0,
 633         };
 634
 635         llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
 636         sg_init_one(&src, skb->data, skb->len);
 637         return crypto_blkcipher_encrypt_iv(&req, &src, &src, skb->len);
 638 }
 639
 640 static struct crypto_aead*
 641 llsec_tfm_by_len(struct mac802154_llsec_key *key, int authlen)
 642 {
 643         int i;
 644
 645         for (i = 0; i < ARRAY_SIZE(key->tfm); i++)
 646                 if (crypto_aead_authsize(key->tfm[i]) == authlen)
 647                         return key->tfm[i];
 648
 649         BUG();
 650 }
 651
 652 static int
 653 llsec_do_encrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec,
 654                       const struct ieee802154_hdr *hdr,
 655                       struct mac802154_llsec_key *key)
 656 {
 657         u8 iv[16];
 658         unsigned char *data;
 659         int authlen, assoclen, datalen, rc;
 660         struct scatterlist src, assoc[2], dst[2];
 661         struct aead_request *req;
 662
 663         authlen = ieee802154_sechdr_authtag_len(&hdr->sec);
 664         llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
 665
 666         req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC);
 667         if (!req)
 668                 return -ENOMEM;
 669
 670         sg_init_table(assoc, 2);
 671         sg_set_buf(&assoc[0], skb_mac_header(skb), skb->mac_len);
 672         assoclen = skb->mac_len;
 673
 674         data = skb_mac_header(skb) + skb->mac_len;
 675         datalen = skb_tail_pointer(skb) - data;
 676
 677         if (hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC) {
 678                 sg_set_buf(&assoc[1], data, 0);
 679         } else {
 680                 sg_set_buf(&assoc[1], data, datalen);
 681                 assoclen += datalen;
 682                 datalen = 0;
 683         }
 684
 685         sg_init_one(&src, data, datalen);
 686
 687         sg_init_table(dst, 2);
 688         sg_set_buf(&dst[0], data, datalen);
 689         sg_set_buf(&dst[1], skb_put(skb, authlen), authlen);
 690
 691         aead_request_set_callback(req, 0, NULL, NULL);
 692         aead_request_set_assoc(req, assoc, assoclen);
 693         aead_request_set_crypt(req, &src, dst, datalen, iv);
 694
 695         rc = crypto_aead_encrypt(req);
 696
 697         kfree(req);
 698
 699         return rc;
 700 }
 701
 702 static int llsec_do_encrypt(struct sk_buff *skb,
 703                             const struct mac802154_llsec *sec,
 704                             const struct ieee802154_hdr *hdr,
 705                             struct mac802154_llsec_key *key)
 706 {
 707         if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC)
 708                 return llsec_do_encrypt_unauth(skb, sec, hdr, key);
 709         else
 710                 return llsec_do_encrypt_auth(skb, sec, hdr, key);
 711 }
 712
 713 int mac802154_llsec_encrypt(struct mac802154_llsec *sec, struct sk_buff *skb)
 714 {
 715         struct ieee802154_hdr hdr;
 716         int rc, authlen, hlen;
 717         struct mac802154_llsec_key *key;
 718         u32 frame_ctr;
 719
 720         hlen = ieee802154_hdr_pull(skb, &hdr);
 721
 722         if (hlen < 0 || hdr.fc.type != IEEE802154_FC_TYPE_DATA)
 723                 return -EINVAL;
 724
 725         if (!hdr.fc.security_enabled || hdr.sec.level == 0) {
 726                 skb_push(skb, hlen);
 727                 return 0;
 728         }
 729
 730         authlen = ieee802154_sechdr_authtag_len(&hdr.sec);
 731
 732         if (skb->len + hlen + authlen + IEEE802154_MFR_SIZE > IEEE802154_MTU)
 733                 return -EMSGSIZE;
 734
 735         rcu_read_lock();
 736
 737         read_lock_bh(&sec->lock);
 738
 739         if (!sec->params.enabled) {
 740                 rc = -EINVAL;
 741                 goto fail_read;
 742         }
 743
 744         key = llsec_lookup_key(sec, &hdr, &hdr.dest, NULL);
 745         if (!key) {
 746                 rc = -ENOKEY;
 747                 goto fail_read;
 748         }
 749
 750         read_unlock_bh(&sec->lock);
 751
 752         write_lock_bh(&sec->lock);
 753
 754         frame_ctr = be32_to_cpu(sec->params.frame_counter);
 755         hdr.sec.frame_counter = cpu_to_le32(frame_ctr);
 756         if (frame_ctr == 0xFFFFFFFF) {
 757                 write_unlock_bh(&sec->lock);
 758                 llsec_key_put(key);
 759                 rc = -EOVERFLOW;
 760                 goto fail;
 761         }
 762
 763         sec->params.frame_counter = cpu_to_be32(frame_ctr + 1);
 764
 765         write_unlock_bh(&sec->lock);
 766
 767         rcu_read_unlock();
 768
 769         skb->mac_len = ieee802154_hdr_push(skb, &hdr);
 770         skb_reset_mac_header(skb);
 771
 772         rc = llsec_do_encrypt(skb, sec, &hdr, key);
 773         llsec_key_put(key);
 774
 775         return rc < 0 ? rc : 0;
 776
 777 fail_read:
 778         read_unlock(&sec->lock);
 779 fail:
 780         rcu_read_unlock();
 781         return rc;
 782 }