llsec.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * Copyright (C) 2014 Fraunhofer ITWM
   4 *
   5 * Written by:
   6 * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
   7 */
   8
   9#include <linux/err.h>
  10#include <linux/bug.h>
  11#include <linux/completion.h>
  12#include <linux/ieee802154.h>
  13#include <linux/rculist.h>
  14
  15#include <crypto/aead.h>
  16#include <crypto/skcipher.h>
  17
  18#include "ieee802154_i.h"
  19#include "llsec.h"
  20
  21static void llsec_key_put(struct mac802154_llsec_key *key);
  22static bool llsec_key_id_equal(const struct ieee802154_llsec_key_id *a,
  23			       const struct ieee802154_llsec_key_id *b);
  24
  25static void llsec_dev_free(struct mac802154_llsec_device *dev);
  26
  27void mac802154_llsec_init(struct mac802154_llsec *sec)
  28{
  29	memset(sec, 0, sizeof(*sec));
  30
  31	memset(&sec->params.default_key_source, 0xFF, IEEE802154_ADDR_LEN);
  32
  33	INIT_LIST_HEAD(&sec->table.security_levels);
  34	INIT_LIST_HEAD(&sec->table.devices);
  35	INIT_LIST_HEAD(&sec->table.keys);
  36	hash_init(sec->devices_short);
  37	hash_init(sec->devices_hw);
  38	rwlock_init(&sec->lock);
  39}
  40
  41void mac802154_llsec_destroy(struct mac802154_llsec *sec)
  42{
  43	struct ieee802154_llsec_seclevel *sl, *sn;
  44	struct ieee802154_llsec_device *dev, *dn;
  45	struct ieee802154_llsec_key_entry *key, *kn;
  46
  47	list_for_each_entry_safe(sl, sn, &sec->table.security_levels, list) {
  48		struct mac802154_llsec_seclevel *msl;
  49
  50		msl = container_of(sl, struct mac802154_llsec_seclevel, level);
  51		list_del(&sl->list);
  52		kfree_sensitive(msl);
  53	}
  54
  55	list_for_each_entry_safe(dev, dn, &sec->table.devices, list) {
  56		struct mac802154_llsec_device *mdev;
  57
  58		mdev = container_of(dev, struct mac802154_llsec_device, dev);
  59		list_del(&dev->list);
  60		llsec_dev_free(mdev);
  61	}
  62
  63	list_for_each_entry_safe(key, kn, &sec->table.keys, list) {
  64		struct mac802154_llsec_key *mkey;
  65
  66		mkey = container_of(key->key, struct mac802154_llsec_key, key);
  67		list_del(&key->list);
  68		llsec_key_put(mkey);
  69		kfree_sensitive(key);
  70	}
  71}
  72
  73int mac802154_llsec_get_params(struct mac802154_llsec *sec,
  74			       struct ieee802154_llsec_params *params)
  75{
  76	read_lock_bh(&sec->lock);
  77	*params = sec->params;
  78	read_unlock_bh(&sec->lock);
  79
  80	return 0;
  81}
  82
  83int mac802154_llsec_set_params(struct mac802154_llsec *sec,
  84			       const struct ieee802154_llsec_params *params,
  85			       int changed)
  86{
  87	write_lock_bh(&sec->lock);
  88
  89	if (changed & IEEE802154_LLSEC_PARAM_ENABLED)
  90		sec->params.enabled = params->enabled;
  91	if (changed & IEEE802154_LLSEC_PARAM_FRAME_COUNTER)
  92		sec->params.frame_counter = params->frame_counter;
  93	if (changed & IEEE802154_LLSEC_PARAM_OUT_LEVEL)
  94		sec->params.out_level = params->out_level;
  95	if (changed & IEEE802154_LLSEC_PARAM_OUT_KEY)
  96		sec->params.out_key = params->out_key;
  97	if (changed & IEEE802154_LLSEC_PARAM_KEY_SOURCE)
  98		sec->params.default_key_source = params->default_key_source;
  99	if (changed & IEEE802154_LLSEC_PARAM_PAN_ID)
 100		sec->params.pan_id = params->pan_id;
 101	if (changed & IEEE802154_LLSEC_PARAM_HWADDR)
 102		sec->params.hwaddr = params->hwaddr;
 103	if (changed & IEEE802154_LLSEC_PARAM_COORD_HWADDR)
 104		sec->params.coord_hwaddr = params->coord_hwaddr;
 105	if (changed & IEEE802154_LLSEC_PARAM_COORD_SHORTADDR)
 106		sec->params.coord_shortaddr = params->coord_shortaddr;
 107
 108	write_unlock_bh(&sec->lock);
 109
 110	return 0;
 111}
 112
 113static struct mac802154_llsec_key*
 114llsec_key_alloc(const struct ieee802154_llsec_key *template)
 115{
 116	const int authsizes[3] = { 4, 8, 16 };
 117	struct mac802154_llsec_key *key;
 118	int i;
 119
 120	key = kzalloc(sizeof(*key), GFP_KERNEL);
 121	if (!key)
 122		return NULL;
 123
 124	kref_init(&key->ref);
 125	key->key = *template;
 126
 127	BUILD_BUG_ON(ARRAY_SIZE(authsizes) != ARRAY_SIZE(key->tfm));
 128
 129	for (i = 0; i < ARRAY_SIZE(key->tfm); i++) {
 130		key->tfm[i] = crypto_alloc_aead("ccm(aes)", 0,
 131						CRYPTO_ALG_ASYNC);
 132		if (IS_ERR(key->tfm[i]))
 133			goto err_tfm;
 134		if (crypto_aead_setkey(key->tfm[i], template->key,
 135				       IEEE802154_LLSEC_KEY_SIZE))
 136			goto err_tfm;
 137		if (crypto_aead_setauthsize(key->tfm[i], authsizes[i]))
 138			goto err_tfm;
 139	}
 140
 141	key->tfm0 = crypto_alloc_sync_skcipher("ctr(aes)", 0, 0);
 142	if (IS_ERR(key->tfm0))
 143		goto err_tfm;
 144
 145	if (crypto_sync_skcipher_setkey(key->tfm0, template->key,
 146				   IEEE802154_LLSEC_KEY_SIZE))
 147		goto err_tfm0;
 148
 149	return key;
 150
 151err_tfm0:
 152	crypto_free_sync_skcipher(key->tfm0);
 153err_tfm:
 154	for (i = 0; i < ARRAY_SIZE(key->tfm); i++)
 155		if (!IS_ERR_OR_NULL(key->tfm[i]))
 156			crypto_free_aead(key->tfm[i]);
 157
 158	kfree_sensitive(key);
 159	return NULL;
 160}
 161
 162static void llsec_key_release(struct kref *ref)
 163{
 164	struct mac802154_llsec_key *key;
 165	int i;
 166
 167	key = container_of(ref, struct mac802154_llsec_key, ref);
 168
 169	for (i = 0; i < ARRAY_SIZE(key->tfm); i++)
 170		crypto_free_aead(key->tfm[i]);
 171
 172	crypto_free_sync_skcipher(key->tfm0);
 173	kfree_sensitive(key);
 174}
 175
 176static struct mac802154_llsec_key*
 177llsec_key_get(struct mac802154_llsec_key *key)
 178{
 179	kref_get(&key->ref);
 180	return key;
 181}
 182
 183static void llsec_key_put(struct mac802154_llsec_key *key)
 184{
 185	kref_put(&key->ref, llsec_key_release);
 186}
 187
 188static bool llsec_key_id_equal(const struct ieee802154_llsec_key_id *a,
 189			       const struct ieee802154_llsec_key_id *b)
 190{
 191	if (a->mode != b->mode)
 192		return false;
 193
 194	if (a->mode == IEEE802154_SCF_KEY_IMPLICIT)
 195		return ieee802154_addr_equal(&a->device_addr, &b->device_addr);
 196
 197	if (a->id != b->id)
 198		return false;
 199
 200	switch (a->mode) {
 201	case IEEE802154_SCF_KEY_INDEX:
 202		return true;
 203	case IEEE802154_SCF_KEY_SHORT_INDEX:
 204		return a->short_source == b->short_source;
 205	case IEEE802154_SCF_KEY_HW_INDEX:
 206		return a->extended_source == b->extended_source;
 207	}
 208
 209	return false;
 210}
 211
 212int mac802154_llsec_key_add(struct mac802154_llsec *sec,
 213			    const struct ieee802154_llsec_key_id *id,
 214			    const struct ieee802154_llsec_key *key)
 215{
 216	struct mac802154_llsec_key *mkey = NULL;
 217	struct ieee802154_llsec_key_entry *pos, *new;
 218
 219	if (!(key->frame_types & (1 << IEEE802154_FC_TYPE_MAC_CMD)) &&
 220	    key->cmd_frame_ids)
 221		return -EINVAL;
 222
 223	list_for_each_entry(pos, &sec->table.keys, list) {
 224		if (llsec_key_id_equal(&pos->id, id))
 225			return -EEXIST;
 226
 227		if (memcmp(pos->key->key, key->key,
 228			   IEEE802154_LLSEC_KEY_SIZE))
 229			continue;
 230
 231		mkey = container_of(pos->key, struct mac802154_llsec_key, key);
 232
 233		/* Don't allow multiple instances of the same AES key to have
 234		 * different allowed frame types/command frame ids, as this is
 235		 * not possible in the 802.15.4 PIB.
 236		 */
 237		if (pos->key->frame_types != key->frame_types ||
 238		    pos->key->cmd_frame_ids != key->cmd_frame_ids)
 239			return -EEXIST;
 240
 241		break;
 242	}
 243
 244	new = kzalloc(sizeof(*new), GFP_KERNEL);
 245	if (!new)
 246		return -ENOMEM;
 247
 248	if (!mkey)
 249		mkey = llsec_key_alloc(key);
 250	else
 251		mkey = llsec_key_get(mkey);
 252
 253	if (!mkey)
 254		goto fail;
 255
 256	new->id = *id;
 257	new->key = &mkey->key;
 258
 259	list_add_rcu(&new->list, &sec->table.keys);
 260
 261	return 0;
 262
 263fail:
 264	kfree_sensitive(new);
 265	return -ENOMEM;
 266}
 267
 268static void mac802154_llsec_key_del_rcu(struct rcu_head *rcu)
 269{
 270	struct ieee802154_llsec_key_entry *pos;
 271	struct mac802154_llsec_key *mkey;
 272
 273	pos = container_of(rcu, struct ieee802154_llsec_key_entry, rcu);
 274	mkey = container_of(pos->key, struct mac802154_llsec_key, key);
 275
 276	llsec_key_put(mkey);
 277	kfree_sensitive(pos);
 278}
 279
 280int mac802154_llsec_key_del(struct mac802154_llsec *sec,
 281			    const struct ieee802154_llsec_key_id *key)
 282{
 283	struct ieee802154_llsec_key_entry *pos;
 284
 285	list_for_each_entry(pos, &sec->table.keys, list) {
 286		if (llsec_key_id_equal(&pos->id, key)) {
 287			list_del_rcu(&pos->list);
 288			call_rcu(&pos->rcu, mac802154_llsec_key_del_rcu);
 289			return 0;
 290		}
 291	}
 292
 293	return -ENOENT;
 294}
 295
 296static 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
 302static 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
 307static u64 llsec_dev_hash_long(__le64 hwaddr)
 308{
 309	return (__force u64)hwaddr;
 310}
 311
 312static struct mac802154_llsec_device*
 313llsec_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
 328static struct mac802154_llsec_device*
 329llsec_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
 342static 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		kfree_sensitive(devkey);
 353	}
 354
 355	kfree_sensitive(dev);
 356}
 357
 358int 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
 391static void llsec_dev_free_rcu(struct rcu_head *rcu)
 392{
 393	llsec_dev_free(container_of(rcu, struct mac802154_llsec_device, rcu));
 394}
 395
 396int 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
 412static struct mac802154_llsec_device_key*
 413llsec_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
 429int 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
 453int 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
 474static struct mac802154_llsec_seclevel*
 475llsec_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
 495int 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
 514int 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
 529static 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
 549static struct mac802154_llsec_key*
 550llsec_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
 597found:
 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
 604static 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
 618static int
 619llsec_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	SYNC_SKCIPHER_REQUEST_ON_STACK(req, key->tfm0);
 626	int err, datalen;
 627	unsigned char *data;
 628
 629	llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
 630	/* Compute data payload offset and data length */
 631	data = skb_mac_header(skb) + skb->mac_len;
 632	datalen = skb_tail_pointer(skb) - data;
 633	sg_init_one(&src, data, datalen);
 634
 635	skcipher_request_set_sync_tfm(req, key->tfm0);
 636	skcipher_request_set_callback(req, 0, NULL, NULL);
 637	skcipher_request_set_crypt(req, &src, &src, datalen, iv);
 638	err = crypto_skcipher_encrypt(req);
 639	skcipher_request_zero(req);
 640	return err;
 641}
 642
 643static struct crypto_aead*
 644llsec_tfm_by_len(struct mac802154_llsec_key *key, int authlen)
 645{
 646	int i;
 647
 648	for (i = 0; i < ARRAY_SIZE(key->tfm); i++)
 649		if (crypto_aead_authsize(key->tfm[i]) == authlen)
 650			return key->tfm[i];
 651
 652	BUG();
 653}
 654
 655static int
 656llsec_do_encrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec,
 657		      const struct ieee802154_hdr *hdr,
 658		      struct mac802154_llsec_key *key)
 659{
 660	u8 iv[16];
 661	unsigned char *data;
 662	int authlen, assoclen, datalen, rc;
 663	struct scatterlist sg;
 664	struct aead_request *req;
 665
 666	authlen = ieee802154_sechdr_authtag_len(&hdr->sec);
 667	llsec_geniv(iv, sec->params.hwaddr, &hdr->sec);
 668
 669	req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC);
 670	if (!req)
 671		return -ENOMEM;
 672
 673	assoclen = skb->mac_len;
 674
 675	data = skb_mac_header(skb) + skb->mac_len;
 676	datalen = skb_tail_pointer(skb) - data;
 677
 678	skb_put(skb, authlen);
 679
 680	sg_init_one(&sg, skb_mac_header(skb), assoclen + datalen + authlen);
 681
 682	if (!(hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC)) {
 683		assoclen += datalen;
 684		datalen = 0;
 685	}
 686
 687	aead_request_set_callback(req, 0, NULL, NULL);
 688	aead_request_set_crypt(req, &sg, &sg, datalen, iv);
 689	aead_request_set_ad(req, assoclen);
 690
 691	rc = crypto_aead_encrypt(req);
 692
 693	kfree_sensitive(req);
 694
 695	return rc;
 696}
 697
 698static int llsec_do_encrypt(struct sk_buff *skb,
 699			    const struct mac802154_llsec *sec,
 700			    const struct ieee802154_hdr *hdr,
 701			    struct mac802154_llsec_key *key)
 702{
 703	if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC)
 704		return llsec_do_encrypt_unauth(skb, sec, hdr, key);
 705	else
 706		return llsec_do_encrypt_auth(skb, sec, hdr, key);
 707}
 708
 709int mac802154_llsec_encrypt(struct mac802154_llsec *sec, struct sk_buff *skb)
 710{
 711	struct ieee802154_hdr hdr;
 712	int rc, authlen, hlen;
 713	struct mac802154_llsec_key *key;
 714	u32 frame_ctr;
 715
 716	hlen = ieee802154_hdr_pull(skb, &hdr);
 717
 718	/* TODO: control frames security support */
 719	if (hlen < 0 ||
 720	    (hdr.fc.type != IEEE802154_FC_TYPE_DATA &&
 721	     hdr.fc.type != IEEE802154_FC_TYPE_BEACON))
 722		return -EINVAL;
 723
 724	if (!hdr.fc.security_enabled ||
 725	    (hdr.sec.level == IEEE802154_SCF_SECLEVEL_NONE)) {
 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;
 776
 777fail_read:
 778	read_unlock_bh(&sec->lock);
 779fail:
 780	rcu_read_unlock();
 781	return rc;
 782}
 783
 784static struct mac802154_llsec_device*
 785llsec_lookup_dev(struct mac802154_llsec *sec,
 786		 const struct ieee802154_addr *addr)
 787{
 788	struct ieee802154_addr devaddr = *addr;
 789	struct mac802154_llsec_device *dev = NULL;
 790
 791	if (devaddr.mode == IEEE802154_ADDR_NONE &&
 792	    llsec_recover_addr(sec, &devaddr) < 0)
 793		return NULL;
 794
 795	if (devaddr.mode == IEEE802154_ADDR_SHORT) {
 796		u32 key = llsec_dev_hash_short(devaddr.short_addr,
 797					       devaddr.pan_id);
 798
 799		hash_for_each_possible_rcu(sec->devices_short, dev,
 800					   bucket_s, key) {
 801			if (dev->dev.pan_id == devaddr.pan_id &&
 802			    dev->dev.short_addr == devaddr.short_addr)
 803				return dev;
 804		}
 805	} else {
 806		u64 key = llsec_dev_hash_long(devaddr.extended_addr);
 807
 808		hash_for_each_possible_rcu(sec->devices_hw, dev,
 809					   bucket_hw, key) {
 810			if (dev->dev.hwaddr == devaddr.extended_addr)
 811				return dev;
 812		}
 813	}
 814
 815	return NULL;
 816}
 817
 818static int
 819llsec_lookup_seclevel(const struct mac802154_llsec *sec,
 820		      u8 frame_type, u8 cmd_frame_id,
 821		      struct ieee802154_llsec_seclevel *rlevel)
 822{
 823	struct ieee802154_llsec_seclevel *level;
 824
 825	list_for_each_entry_rcu(level, &sec->table.security_levels, list) {
 826		if (level->frame_type == frame_type &&
 827		    (frame_type != IEEE802154_FC_TYPE_MAC_CMD ||
 828		     level->cmd_frame_id == cmd_frame_id)) {
 829			*rlevel = *level;
 830			return 0;
 831		}
 832	}
 833
 834	return -EINVAL;
 835}
 836
 837static int
 838llsec_do_decrypt_unauth(struct sk_buff *skb, const struct mac802154_llsec *sec,
 839			const struct ieee802154_hdr *hdr,
 840			struct mac802154_llsec_key *key, __le64 dev_addr)
 841{
 842	u8 iv[16];
 843	unsigned char *data;
 844	int datalen;
 845	struct scatterlist src;
 846	SYNC_SKCIPHER_REQUEST_ON_STACK(req, key->tfm0);
 847	int err;
 848
 849	llsec_geniv(iv, dev_addr, &hdr->sec);
 850	data = skb_mac_header(skb) + skb->mac_len;
 851	datalen = skb_tail_pointer(skb) - data;
 852
 853	sg_init_one(&src, data, datalen);
 854
 855	skcipher_request_set_sync_tfm(req, key->tfm0);
 856	skcipher_request_set_callback(req, 0, NULL, NULL);
 857	skcipher_request_set_crypt(req, &src, &src, datalen, iv);
 858
 859	err = crypto_skcipher_decrypt(req);
 860	skcipher_request_zero(req);
 861	return err;
 862}
 863
 864static int
 865llsec_do_decrypt_auth(struct sk_buff *skb, const struct mac802154_llsec *sec,
 866		      const struct ieee802154_hdr *hdr,
 867		      struct mac802154_llsec_key *key, __le64 dev_addr)
 868{
 869	u8 iv[16];
 870	unsigned char *data;
 871	int authlen, datalen, assoclen, rc;
 872	struct scatterlist sg;
 873	struct aead_request *req;
 874
 875	authlen = ieee802154_sechdr_authtag_len(&hdr->sec);
 876	llsec_geniv(iv, dev_addr, &hdr->sec);
 877
 878	req = aead_request_alloc(llsec_tfm_by_len(key, authlen), GFP_ATOMIC);
 879	if (!req)
 880		return -ENOMEM;
 881
 882	assoclen = skb->mac_len;
 883
 884	data = skb_mac_header(skb) + skb->mac_len;
 885	datalen = skb_tail_pointer(skb) - data;
 886
 887	sg_init_one(&sg, skb_mac_header(skb), assoclen + datalen);
 888
 889	if (!(hdr->sec.level & IEEE802154_SCF_SECLEVEL_ENC)) {
 890		assoclen += datalen - authlen;
 891		datalen = authlen;
 892	}
 893
 894	aead_request_set_callback(req, 0, NULL, NULL);
 895	aead_request_set_crypt(req, &sg, &sg, datalen, iv);
 896	aead_request_set_ad(req, assoclen);
 897
 898	rc = crypto_aead_decrypt(req);
 899
 900	kfree_sensitive(req);
 901	skb_trim(skb, skb->len - authlen);
 902
 903	return rc;
 904}
 905
 906static int
 907llsec_do_decrypt(struct sk_buff *skb, const struct mac802154_llsec *sec,
 908		 const struct ieee802154_hdr *hdr,
 909		 struct mac802154_llsec_key *key, __le64 dev_addr)
 910{
 911	if (hdr->sec.level == IEEE802154_SCF_SECLEVEL_ENC)
 912		return llsec_do_decrypt_unauth(skb, sec, hdr, key, dev_addr);
 913	else
 914		return llsec_do_decrypt_auth(skb, sec, hdr, key, dev_addr);
 915}
 916
 917static int
 918llsec_update_devkey_record(struct mac802154_llsec_device *dev,
 919			   const struct ieee802154_llsec_key_id *in_key)
 920{
 921	struct mac802154_llsec_device_key *devkey;
 922
 923	devkey = llsec_devkey_find(dev, in_key);
 924
 925	if (!devkey) {
 926		struct mac802154_llsec_device_key *next;
 927
 928		next = kzalloc(sizeof(*devkey), GFP_ATOMIC);
 929		if (!next)
 930			return -ENOMEM;
 931
 932		next->devkey.key_id = *in_key;
 933
 934		spin_lock_bh(&dev->lock);
 935
 936		devkey = llsec_devkey_find(dev, in_key);
 937		if (!devkey)
 938			list_add_rcu(&next->devkey.list, &dev->dev.keys);
 939		else
 940			kfree_sensitive(next);
 941
 942		spin_unlock_bh(&dev->lock);
 943	}
 944
 945	return 0;
 946}
 947
 948static int
 949llsec_update_devkey_info(struct mac802154_llsec_device *dev,
 950			 const struct ieee802154_llsec_key_id *in_key,
 951			 u32 frame_counter)
 952{
 953	struct mac802154_llsec_device_key *devkey = NULL;
 954
 955	if (dev->dev.key_mode == IEEE802154_LLSEC_DEVKEY_RESTRICT) {
 956		devkey = llsec_devkey_find(dev, in_key);
 957		if (!devkey)
 958			return -ENOENT;
 959	}
 960
 961	if (dev->dev.key_mode == IEEE802154_LLSEC_DEVKEY_RECORD) {
 962		int rc = llsec_update_devkey_record(dev, in_key);
 963
 964		if (rc < 0)
 965			return rc;
 966	}
 967
 968	spin_lock_bh(&dev->lock);
 969
 970	if ((!devkey && frame_counter < dev->dev.frame_counter) ||
 971	    (devkey && frame_counter < devkey->devkey.frame_counter)) {
 972		spin_unlock_bh(&dev->lock);
 973		return -EINVAL;
 974	}
 975
 976	if (devkey)
 977		devkey->devkey.frame_counter = frame_counter + 1;
 978	else
 979		dev->dev.frame_counter = frame_counter + 1;
 980
 981	spin_unlock_bh(&dev->lock);
 982
 983	return 0;
 984}
 985
 986int mac802154_llsec_decrypt(struct mac802154_llsec *sec, struct sk_buff *skb)
 987{
 988	struct ieee802154_hdr hdr;
 989	struct mac802154_llsec_key *key;
 990	struct ieee802154_llsec_key_id key_id;
 991	struct mac802154_llsec_device *dev;
 992	struct ieee802154_llsec_seclevel seclevel;
 993	int err;
 994	__le64 dev_addr;
 995	u32 frame_ctr;
 996
 997	if (ieee802154_hdr_peek(skb, &hdr) < 0)
 998		return -EINVAL;
 999	if (!hdr.fc.security_enabled)
1000		return 0;
1001	if (hdr.fc.version == 0)
1002		return -EINVAL;
1003
1004	read_lock_bh(&sec->lock);
1005	if (!sec->params.enabled) {
1006		read_unlock_bh(&sec->lock);
1007		return -EINVAL;
1008	}
1009	read_unlock_bh(&sec->lock);
1010
1011	rcu_read_lock();
1012
1013	key = llsec_lookup_key(sec, &hdr, &hdr.source, &key_id);
1014	if (!key) {
1015		err = -ENOKEY;
1016		goto fail;
1017	}
1018
1019	dev = llsec_lookup_dev(sec, &hdr.source);
1020	if (!dev) {
1021		err = -EINVAL;
1022		goto fail_dev;
1023	}
1024
1025	if (llsec_lookup_seclevel(sec, hdr.fc.type, 0, &seclevel) < 0) {
1026		err = -EINVAL;
1027		goto fail_dev;
1028	}
1029
1030	if (!(seclevel.sec_levels & BIT(hdr.sec.level)) &&
1031	    (hdr.sec.level == 0 && seclevel.device_override &&
1032	     !dev->dev.seclevel_exempt)) {
1033		err = -EINVAL;
1034		goto fail_dev;
1035	}
1036
1037	frame_ctr = le32_to_cpu(hdr.sec.frame_counter);
1038
1039	if (frame_ctr == 0xffffffff) {
1040		err = -EOVERFLOW;
1041		goto fail_dev;
1042	}
1043
1044	err = llsec_update_devkey_info(dev, &key_id, frame_ctr);
1045	if (err)
1046		goto fail_dev;
1047
1048	dev_addr = dev->dev.hwaddr;
1049
1050	rcu_read_unlock();
1051
1052	err = llsec_do_decrypt(skb, sec, &hdr, key, dev_addr);
1053	llsec_key_put(key);
1054	return err;
1055
1056fail_dev:
1057	llsec_key_put(key);
1058fail:
1059	rcu_read_unlock();
1060	return err;
1061}