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