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 } 1053