nl-mac.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * Netlink interface for IEEE 802.15.4 stack
   4 *
   5 * Copyright 2007, 2008 Siemens AG
   6 *
   7 * Written by:
   8 * Sergey Lapin <slapin@ossfans.org>
   9 * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
  10 * Maxim Osipov <maxim.osipov@siemens.com>
  11 */
  12
  13#include <linux/gfp.h>
  14#include <linux/kernel.h>
  15#include <linux/if_arp.h>
  16#include <linux/netdevice.h>
  17#include <linux/ieee802154.h>
  18#include <net/netlink.h>
  19#include <net/genetlink.h>
  20#include <net/sock.h>
  21#include <linux/nl802154.h>
  22#include <linux/export.h>
  23#include <net/af_ieee802154.h>
  24#include <net/ieee802154_netdev.h>
  25#include <net/cfg802154.h>
  26
  27#include "ieee802154.h"
  28
  29static int nla_put_hwaddr(struct sk_buff *msg, int type, __le64 hwaddr,
  30			  int padattr)
  31{
  32	return nla_put_u64_64bit(msg, type, swab64((__force u64)hwaddr),
  33				 padattr);
  34}
  35
  36static __le64 nla_get_hwaddr(const struct nlattr *nla)
  37{
  38	return ieee802154_devaddr_from_raw(nla_data(nla));
  39}
  40
  41static int nla_put_shortaddr(struct sk_buff *msg, int type, __le16 addr)
  42{
  43	return nla_put_u16(msg, type, le16_to_cpu(addr));
  44}
  45
  46static __le16 nla_get_shortaddr(const struct nlattr *nla)
  47{
  48	return cpu_to_le16(nla_get_u16(nla));
  49}
  50
  51static int ieee802154_nl_start_confirm(struct net_device *dev, u8 status)
  52{
  53	struct sk_buff *msg;
  54
  55	pr_debug("%s\n", __func__);
  56
  57	msg = ieee802154_nl_create(0, IEEE802154_START_CONF);
  58	if (!msg)
  59		return -ENOBUFS;
  60
  61	if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
  62	    nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
  63	    nla_put(msg, IEEE802154_ATTR_HW_ADDR, IEEE802154_ADDR_LEN,
  64		    dev->dev_addr) ||
  65	    nla_put_u8(msg, IEEE802154_ATTR_STATUS, status))
  66		goto nla_put_failure;
  67	return ieee802154_nl_mcast(msg, IEEE802154_COORD_MCGRP);
  68
  69nla_put_failure:
  70	nlmsg_free(msg);
  71	return -ENOBUFS;
  72}
  73
  74static int ieee802154_nl_fill_iface(struct sk_buff *msg, u32 portid,
  75				    u32 seq, int flags, struct net_device *dev)
  76{
  77	void *hdr;
  78	struct wpan_phy *phy;
  79	struct ieee802154_mlme_ops *ops;
  80	__le16 short_addr, pan_id;
  81
  82	pr_debug("%s\n", __func__);
  83
  84	hdr = genlmsg_put(msg, 0, seq, &nl802154_family, flags,
  85			  IEEE802154_LIST_IFACE);
  86	if (!hdr)
  87		goto out;
  88
  89	ops = ieee802154_mlme_ops(dev);
  90	phy = dev->ieee802154_ptr->wpan_phy;
  91	BUG_ON(!phy);
  92	get_device(&phy->dev);
  93
  94	rtnl_lock();
  95	short_addr = dev->ieee802154_ptr->short_addr;
  96	pan_id = dev->ieee802154_ptr->pan_id;
  97	rtnl_unlock();
  98
  99	if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
 100	    nla_put_string(msg, IEEE802154_ATTR_PHY_NAME, wpan_phy_name(phy)) ||
 101	    nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
 102	    nla_put(msg, IEEE802154_ATTR_HW_ADDR, IEEE802154_ADDR_LEN,
 103		    dev->dev_addr) ||
 104	    nla_put_shortaddr(msg, IEEE802154_ATTR_SHORT_ADDR, short_addr) ||
 105	    nla_put_shortaddr(msg, IEEE802154_ATTR_PAN_ID, pan_id))
 106		goto nla_put_failure;
 107
 108	if (ops->get_mac_params) {
 109		struct ieee802154_mac_params params;
 110
 111		rtnl_lock();
 112		ops->get_mac_params(dev, &params);
 113		rtnl_unlock();
 114
 115		if (nla_put_s8(msg, IEEE802154_ATTR_TXPOWER,
 116			       params.transmit_power / 100) ||
 117		    nla_put_u8(msg, IEEE802154_ATTR_LBT_ENABLED, params.lbt) ||
 118		    nla_put_u8(msg, IEEE802154_ATTR_CCA_MODE,
 119			       params.cca.mode) ||
 120		    nla_put_s32(msg, IEEE802154_ATTR_CCA_ED_LEVEL,
 121				params.cca_ed_level / 100) ||
 122		    nla_put_u8(msg, IEEE802154_ATTR_CSMA_RETRIES,
 123			       params.csma_retries) ||
 124		    nla_put_u8(msg, IEEE802154_ATTR_CSMA_MIN_BE,
 125			       params.min_be) ||
 126		    nla_put_u8(msg, IEEE802154_ATTR_CSMA_MAX_BE,
 127			       params.max_be) ||
 128		    nla_put_s8(msg, IEEE802154_ATTR_FRAME_RETRIES,
 129			       params.frame_retries))
 130			goto nla_put_failure;
 131	}
 132
 133	wpan_phy_put(phy);
 134	genlmsg_end(msg, hdr);
 135	return 0;
 136
 137nla_put_failure:
 138	wpan_phy_put(phy);
 139	genlmsg_cancel(msg, hdr);
 140out:
 141	return -EMSGSIZE;
 142}
 143
 144/* Requests from userspace */
 145static struct net_device *ieee802154_nl_get_dev(struct genl_info *info)
 146{
 147	struct net_device *dev;
 148
 149	if (info->attrs[IEEE802154_ATTR_DEV_NAME]) {
 150		char name[IFNAMSIZ + 1];
 151
 152		nla_strscpy(name, info->attrs[IEEE802154_ATTR_DEV_NAME],
 153			    sizeof(name));
 154		dev = dev_get_by_name(&init_net, name);
 155	} else if (info->attrs[IEEE802154_ATTR_DEV_INDEX]) {
 156		dev = dev_get_by_index(&init_net,
 157			nla_get_u32(info->attrs[IEEE802154_ATTR_DEV_INDEX]));
 158	} else {
 159		return NULL;
 160	}
 161
 162	if (!dev)
 163		return NULL;
 164
 165	if (dev->type != ARPHRD_IEEE802154) {
 166		dev_put(dev);
 167		return NULL;
 168	}
 169
 170	return dev;
 171}
 172
 173int ieee802154_associate_req(struct sk_buff *skb, struct genl_info *info)
 174{
 175	struct net_device *dev;
 176	struct ieee802154_addr addr;
 177	u8 page;
 178	int ret = -EOPNOTSUPP;
 179
 180	if (!info->attrs[IEEE802154_ATTR_CHANNEL] ||
 181	    !info->attrs[IEEE802154_ATTR_COORD_PAN_ID] ||
 182	    (!info->attrs[IEEE802154_ATTR_COORD_HW_ADDR] &&
 183		!info->attrs[IEEE802154_ATTR_COORD_SHORT_ADDR]) ||
 184	    !info->attrs[IEEE802154_ATTR_CAPABILITY])
 185		return -EINVAL;
 186
 187	dev = ieee802154_nl_get_dev(info);
 188	if (!dev)
 189		return -ENODEV;
 190	if (!ieee802154_mlme_ops(dev)->assoc_req)
 191		goto out;
 192
 193	if (info->attrs[IEEE802154_ATTR_COORD_HW_ADDR]) {
 194		addr.mode = IEEE802154_ADDR_LONG;
 195		addr.extended_addr = nla_get_hwaddr(
 196				info->attrs[IEEE802154_ATTR_COORD_HW_ADDR]);
 197	} else {
 198		addr.mode = IEEE802154_ADDR_SHORT;
 199		addr.short_addr = nla_get_shortaddr(
 200				info->attrs[IEEE802154_ATTR_COORD_SHORT_ADDR]);
 201	}
 202	addr.pan_id = nla_get_shortaddr(
 203			info->attrs[IEEE802154_ATTR_COORD_PAN_ID]);
 204
 205	page = nla_get_u8_default(info->attrs[IEEE802154_ATTR_PAGE], 0);
 206
 207	ret = ieee802154_mlme_ops(dev)->assoc_req(dev, &addr,
 208			nla_get_u8(info->attrs[IEEE802154_ATTR_CHANNEL]),
 209			page,
 210			nla_get_u8(info->attrs[IEEE802154_ATTR_CAPABILITY]));
 211
 212out:
 213	dev_put(dev);
 214	return ret;
 215}
 216
 217int ieee802154_associate_resp(struct sk_buff *skb, struct genl_info *info)
 218{
 219	struct net_device *dev;
 220	struct ieee802154_addr addr;
 221	int ret = -EOPNOTSUPP;
 222
 223	if (!info->attrs[IEEE802154_ATTR_STATUS] ||
 224	    !info->attrs[IEEE802154_ATTR_DEST_HW_ADDR] ||
 225	    !info->attrs[IEEE802154_ATTR_DEST_SHORT_ADDR])
 226		return -EINVAL;
 227
 228	dev = ieee802154_nl_get_dev(info);
 229	if (!dev)
 230		return -ENODEV;
 231	if (!ieee802154_mlme_ops(dev)->assoc_resp)
 232		goto out;
 233
 234	addr.mode = IEEE802154_ADDR_LONG;
 235	addr.extended_addr = nla_get_hwaddr(
 236			info->attrs[IEEE802154_ATTR_DEST_HW_ADDR]);
 237	rtnl_lock();
 238	addr.pan_id = dev->ieee802154_ptr->pan_id;
 239	rtnl_unlock();
 240
 241	ret = ieee802154_mlme_ops(dev)->assoc_resp(dev, &addr,
 242		nla_get_shortaddr(info->attrs[IEEE802154_ATTR_DEST_SHORT_ADDR]),
 243		nla_get_u8(info->attrs[IEEE802154_ATTR_STATUS]));
 244
 245out:
 246	dev_put(dev);
 247	return ret;
 248}
 249
 250int ieee802154_disassociate_req(struct sk_buff *skb, struct genl_info *info)
 251{
 252	struct net_device *dev;
 253	struct ieee802154_addr addr;
 254	int ret = -EOPNOTSUPP;
 255
 256	if ((!info->attrs[IEEE802154_ATTR_DEST_HW_ADDR] &&
 257	    !info->attrs[IEEE802154_ATTR_DEST_SHORT_ADDR]) ||
 258	    !info->attrs[IEEE802154_ATTR_REASON])
 259		return -EINVAL;
 260
 261	dev = ieee802154_nl_get_dev(info);
 262	if (!dev)
 263		return -ENODEV;
 264	if (!ieee802154_mlme_ops(dev)->disassoc_req)
 265		goto out;
 266
 267	if (info->attrs[IEEE802154_ATTR_DEST_HW_ADDR]) {
 268		addr.mode = IEEE802154_ADDR_LONG;
 269		addr.extended_addr = nla_get_hwaddr(
 270				info->attrs[IEEE802154_ATTR_DEST_HW_ADDR]);
 271	} else {
 272		addr.mode = IEEE802154_ADDR_SHORT;
 273		addr.short_addr = nla_get_shortaddr(
 274				info->attrs[IEEE802154_ATTR_DEST_SHORT_ADDR]);
 275	}
 276	rtnl_lock();
 277	addr.pan_id = dev->ieee802154_ptr->pan_id;
 278	rtnl_unlock();
 279
 280	ret = ieee802154_mlme_ops(dev)->disassoc_req(dev, &addr,
 281			nla_get_u8(info->attrs[IEEE802154_ATTR_REASON]));
 282
 283out:
 284	dev_put(dev);
 285	return ret;
 286}
 287
 288/* PANid, channel, beacon_order = 15, superframe_order = 15,
 289 * PAN_coordinator, battery_life_extension = 0,
 290 * coord_realignment = 0, security_enable = 0
 291*/
 292int ieee802154_start_req(struct sk_buff *skb, struct genl_info *info)
 293{
 294	struct net_device *dev;
 295	struct ieee802154_addr addr;
 296
 297	u8 channel, bcn_ord, sf_ord;
 298	u8 page;
 299	int pan_coord, blx, coord_realign;
 300	int ret = -EBUSY;
 301
 302	if (!info->attrs[IEEE802154_ATTR_COORD_PAN_ID] ||
 303	    !info->attrs[IEEE802154_ATTR_COORD_SHORT_ADDR] ||
 304	    !info->attrs[IEEE802154_ATTR_CHANNEL] ||
 305	    !info->attrs[IEEE802154_ATTR_BCN_ORD] ||
 306	    !info->attrs[IEEE802154_ATTR_SF_ORD] ||
 307	    !info->attrs[IEEE802154_ATTR_PAN_COORD] ||
 308	    !info->attrs[IEEE802154_ATTR_BAT_EXT] ||
 309	    !info->attrs[IEEE802154_ATTR_COORD_REALIGN]
 310	 )
 311		return -EINVAL;
 312
 313	dev = ieee802154_nl_get_dev(info);
 314	if (!dev)
 315		return -ENODEV;
 316
 317	if (netif_running(dev))
 318		goto out;
 319
 320	if (!ieee802154_mlme_ops(dev)->start_req) {
 321		ret = -EOPNOTSUPP;
 322		goto out;
 323	}
 324
 325	addr.mode = IEEE802154_ADDR_SHORT;
 326	addr.short_addr = nla_get_shortaddr(
 327			info->attrs[IEEE802154_ATTR_COORD_SHORT_ADDR]);
 328	addr.pan_id = nla_get_shortaddr(
 329			info->attrs[IEEE802154_ATTR_COORD_PAN_ID]);
 330
 331	channel = nla_get_u8(info->attrs[IEEE802154_ATTR_CHANNEL]);
 332	bcn_ord = nla_get_u8(info->attrs[IEEE802154_ATTR_BCN_ORD]);
 333	sf_ord = nla_get_u8(info->attrs[IEEE802154_ATTR_SF_ORD]);
 334	pan_coord = nla_get_u8(info->attrs[IEEE802154_ATTR_PAN_COORD]);
 335	blx = nla_get_u8(info->attrs[IEEE802154_ATTR_BAT_EXT]);
 336	coord_realign = nla_get_u8(info->attrs[IEEE802154_ATTR_COORD_REALIGN]);
 337
 338	page = nla_get_u8_default(info->attrs[IEEE802154_ATTR_PAGE], 0);
 339
 340	if (addr.short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST)) {
 341		ieee802154_nl_start_confirm(dev, IEEE802154_NO_SHORT_ADDRESS);
 342		dev_put(dev);
 343		return -EINVAL;
 344	}
 345
 346	rtnl_lock();
 347	ret = ieee802154_mlme_ops(dev)->start_req(dev, &addr, channel, page,
 348		bcn_ord, sf_ord, pan_coord, blx, coord_realign);
 349	rtnl_unlock();
 350
 351	/* FIXME: add validation for unused parameters to be sane
 352	 * for SoftMAC
 353	 */
 354	ieee802154_nl_start_confirm(dev, IEEE802154_SUCCESS);
 355
 356out:
 357	dev_put(dev);
 358	return ret;
 359}
 360
 361int ieee802154_scan_req(struct sk_buff *skb, struct genl_info *info)
 362{
 363	struct net_device *dev;
 364	int ret = -EOPNOTSUPP;
 365	u8 type;
 366	u32 channels;
 367	u8 duration;
 368	u8 page;
 369
 370	if (!info->attrs[IEEE802154_ATTR_SCAN_TYPE] ||
 371	    !info->attrs[IEEE802154_ATTR_CHANNELS] ||
 372	    !info->attrs[IEEE802154_ATTR_DURATION])
 373		return -EINVAL;
 374
 375	dev = ieee802154_nl_get_dev(info);
 376	if (!dev)
 377		return -ENODEV;
 378	if (!ieee802154_mlme_ops(dev)->scan_req)
 379		goto out;
 380
 381	type = nla_get_u8(info->attrs[IEEE802154_ATTR_SCAN_TYPE]);
 382	channels = nla_get_u32(info->attrs[IEEE802154_ATTR_CHANNELS]);
 383	duration = nla_get_u8(info->attrs[IEEE802154_ATTR_DURATION]);
 384
 385	page = nla_get_u8_default(info->attrs[IEEE802154_ATTR_PAGE], 0);
 386
 387	ret = ieee802154_mlme_ops(dev)->scan_req(dev, type, channels,
 388						 page, duration);
 389
 390out:
 391	dev_put(dev);
 392	return ret;
 393}
 394
 395int ieee802154_list_iface(struct sk_buff *skb, struct genl_info *info)
 396{
 397	/* Request for interface name, index, type, IEEE address,
 398	 * PAN Id, short address
 399	 */
 400	struct sk_buff *msg;
 401	struct net_device *dev = NULL;
 402	int rc = -ENOBUFS;
 403
 404	pr_debug("%s\n", __func__);
 405
 406	dev = ieee802154_nl_get_dev(info);
 407	if (!dev)
 408		return -ENODEV;
 409
 410	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 411	if (!msg)
 412		goto out_dev;
 413
 414	rc = ieee802154_nl_fill_iface(msg, info->snd_portid, info->snd_seq,
 415				      0, dev);
 416	if (rc < 0)
 417		goto out_free;
 418
 419	dev_put(dev);
 420
 421	return genlmsg_reply(msg, info);
 422out_free:
 423	nlmsg_free(msg);
 424out_dev:
 425	dev_put(dev);
 426	return rc;
 427}
 428
 429int ieee802154_dump_iface(struct sk_buff *skb, struct netlink_callback *cb)
 430{
 431	struct net *net = sock_net(skb->sk);
 432	struct net_device *dev;
 433	int idx;
 434	int s_idx = cb->args[0];
 435
 436	pr_debug("%s\n", __func__);
 437
 438	idx = 0;
 439	for_each_netdev(net, dev) {
 440		if (idx < s_idx || dev->type != ARPHRD_IEEE802154)
 441			goto cont;
 442
 443		if (ieee802154_nl_fill_iface(skb, NETLINK_CB(cb->skb).portid,
 444					     cb->nlh->nlmsg_seq,
 445					     NLM_F_MULTI, dev) < 0)
 446			break;
 447cont:
 448		idx++;
 449	}
 450	cb->args[0] = idx;
 451
 452	return skb->len;
 453}
 454
 455int ieee802154_set_macparams(struct sk_buff *skb, struct genl_info *info)
 456{
 457	struct net_device *dev = NULL;
 458	struct ieee802154_mlme_ops *ops;
 459	struct ieee802154_mac_params params;
 460	struct wpan_phy *phy;
 461	int rc = -EINVAL;
 462
 463	pr_debug("%s\n", __func__);
 464
 465	dev = ieee802154_nl_get_dev(info);
 466	if (!dev)
 467		return -ENODEV;
 468
 469	ops = ieee802154_mlme_ops(dev);
 470
 471	if (!ops->get_mac_params || !ops->set_mac_params) {
 472		rc = -EOPNOTSUPP;
 473		goto out;
 474	}
 475
 476	if (netif_running(dev)) {
 477		rc = -EBUSY;
 478		goto out;
 479	}
 480
 481	if (!info->attrs[IEEE802154_ATTR_LBT_ENABLED] &&
 482	    !info->attrs[IEEE802154_ATTR_CCA_MODE] &&
 483	    !info->attrs[IEEE802154_ATTR_CCA_ED_LEVEL] &&
 484	    !info->attrs[IEEE802154_ATTR_CSMA_RETRIES] &&
 485	    !info->attrs[IEEE802154_ATTR_CSMA_MIN_BE] &&
 486	    !info->attrs[IEEE802154_ATTR_CSMA_MAX_BE] &&
 487	    !info->attrs[IEEE802154_ATTR_FRAME_RETRIES])
 488		goto out;
 489
 490	phy = dev->ieee802154_ptr->wpan_phy;
 491	get_device(&phy->dev);
 492
 493	rtnl_lock();
 494	ops->get_mac_params(dev, &params);
 495
 496	if (info->attrs[IEEE802154_ATTR_TXPOWER])
 497		params.transmit_power = nla_get_s8(info->attrs[IEEE802154_ATTR_TXPOWER]) * 100;
 498
 499	if (info->attrs[IEEE802154_ATTR_LBT_ENABLED])
 500		params.lbt = nla_get_u8(info->attrs[IEEE802154_ATTR_LBT_ENABLED]);
 501
 502	if (info->attrs[IEEE802154_ATTR_CCA_MODE])
 503		params.cca.mode = nla_get_u8(info->attrs[IEEE802154_ATTR_CCA_MODE]);
 504
 505	if (info->attrs[IEEE802154_ATTR_CCA_ED_LEVEL])
 506		params.cca_ed_level = nla_get_s32(info->attrs[IEEE802154_ATTR_CCA_ED_LEVEL]) * 100;
 507
 508	if (info->attrs[IEEE802154_ATTR_CSMA_RETRIES])
 509		params.csma_retries = nla_get_u8(info->attrs[IEEE802154_ATTR_CSMA_RETRIES]);
 510
 511	if (info->attrs[IEEE802154_ATTR_CSMA_MIN_BE])
 512		params.min_be = nla_get_u8(info->attrs[IEEE802154_ATTR_CSMA_MIN_BE]);
 513
 514	if (info->attrs[IEEE802154_ATTR_CSMA_MAX_BE])
 515		params.max_be = nla_get_u8(info->attrs[IEEE802154_ATTR_CSMA_MAX_BE]);
 516
 517	if (info->attrs[IEEE802154_ATTR_FRAME_RETRIES])
 518		params.frame_retries = nla_get_s8(info->attrs[IEEE802154_ATTR_FRAME_RETRIES]);
 519
 520	rc = ops->set_mac_params(dev, &params);
 521	rtnl_unlock();
 522
 523	wpan_phy_put(phy);
 524	dev_put(dev);
 525
 526	return 0;
 527
 528out:
 529	dev_put(dev);
 530	return rc;
 531}
 532
 533static int
 534ieee802154_llsec_parse_key_id(struct genl_info *info,
 535			      struct ieee802154_llsec_key_id *desc)
 536{
 537	memset(desc, 0, sizeof(*desc));
 538
 539	if (!info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE])
 540		return -EINVAL;
 541
 542	desc->mode = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE]);
 543
 544	if (desc->mode == IEEE802154_SCF_KEY_IMPLICIT) {
 545		if (!info->attrs[IEEE802154_ATTR_PAN_ID])
 546			return -EINVAL;
 547
 548		desc->device_addr.pan_id = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_PAN_ID]);
 549
 550		if (info->attrs[IEEE802154_ATTR_SHORT_ADDR]) {
 551			desc->device_addr.mode = IEEE802154_ADDR_SHORT;
 552			desc->device_addr.short_addr = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_SHORT_ADDR]);
 553		} else {
 554			if (!info->attrs[IEEE802154_ATTR_HW_ADDR])
 555				return -EINVAL;
 556
 557			desc->device_addr.mode = IEEE802154_ADDR_LONG;
 558			desc->device_addr.extended_addr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
 559		}
 560	}
 561
 562	if (desc->mode != IEEE802154_SCF_KEY_IMPLICIT &&
 563	    !info->attrs[IEEE802154_ATTR_LLSEC_KEY_ID])
 564		return -EINVAL;
 565
 566	if (desc->mode == IEEE802154_SCF_KEY_SHORT_INDEX &&
 567	    !info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_SHORT])
 568		return -EINVAL;
 569
 570	if (desc->mode == IEEE802154_SCF_KEY_HW_INDEX &&
 571	    !info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_EXTENDED])
 572		return -EINVAL;
 573
 574	if (desc->mode != IEEE802154_SCF_KEY_IMPLICIT)
 575		desc->id = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_KEY_ID]);
 576
 577	switch (desc->mode) {
 578	case IEEE802154_SCF_KEY_SHORT_INDEX:
 579	{
 580		u32 source = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_SHORT]);
 581
 582		desc->short_source = cpu_to_le32(source);
 583		break;
 584	}
 585	case IEEE802154_SCF_KEY_HW_INDEX:
 586		desc->extended_source = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_EXTENDED]);
 587		break;
 588	}
 589
 590	return 0;
 591}
 592
 593static int
 594ieee802154_llsec_fill_key_id(struct sk_buff *msg,
 595			     const struct ieee802154_llsec_key_id *desc)
 596{
 597	if (nla_put_u8(msg, IEEE802154_ATTR_LLSEC_KEY_MODE, desc->mode))
 598		return -EMSGSIZE;
 599
 600	if (desc->mode == IEEE802154_SCF_KEY_IMPLICIT) {
 601		if (nla_put_shortaddr(msg, IEEE802154_ATTR_PAN_ID,
 602				      desc->device_addr.pan_id))
 603			return -EMSGSIZE;
 604
 605		if (desc->device_addr.mode == IEEE802154_ADDR_SHORT &&
 606		    nla_put_shortaddr(msg, IEEE802154_ATTR_SHORT_ADDR,
 607				      desc->device_addr.short_addr))
 608			return -EMSGSIZE;
 609
 610		if (desc->device_addr.mode == IEEE802154_ADDR_LONG &&
 611		    nla_put_hwaddr(msg, IEEE802154_ATTR_HW_ADDR,
 612				   desc->device_addr.extended_addr,
 613				   IEEE802154_ATTR_PAD))
 614			return -EMSGSIZE;
 615	}
 616
 617	if (desc->mode != IEEE802154_SCF_KEY_IMPLICIT &&
 618	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_KEY_ID, desc->id))
 619		return -EMSGSIZE;
 620
 621	if (desc->mode == IEEE802154_SCF_KEY_SHORT_INDEX &&
 622	    nla_put_u32(msg, IEEE802154_ATTR_LLSEC_KEY_SOURCE_SHORT,
 623			le32_to_cpu(desc->short_source)))
 624		return -EMSGSIZE;
 625
 626	if (desc->mode == IEEE802154_SCF_KEY_HW_INDEX &&
 627	    nla_put_hwaddr(msg, IEEE802154_ATTR_LLSEC_KEY_SOURCE_EXTENDED,
 628			   desc->extended_source, IEEE802154_ATTR_PAD))
 629		return -EMSGSIZE;
 630
 631	return 0;
 632}
 633
 634int ieee802154_llsec_getparams(struct sk_buff *skb, struct genl_info *info)
 635{
 636	struct sk_buff *msg;
 637	struct net_device *dev = NULL;
 638	int rc = -ENOBUFS;
 639	struct ieee802154_mlme_ops *ops;
 640	void *hdr;
 641	struct ieee802154_llsec_params params;
 642
 643	pr_debug("%s\n", __func__);
 644
 645	dev = ieee802154_nl_get_dev(info);
 646	if (!dev)
 647		return -ENODEV;
 648
 649	ops = ieee802154_mlme_ops(dev);
 650	if (!ops->llsec) {
 651		rc = -EOPNOTSUPP;
 652		goto out_dev;
 653	}
 654
 655	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 656	if (!msg)
 657		goto out_dev;
 658
 659	hdr = genlmsg_put(msg, 0, info->snd_seq, &nl802154_family, 0,
 660			  IEEE802154_LLSEC_GETPARAMS);
 661	if (!hdr)
 662		goto out_free;
 663
 664	rc = ops->llsec->get_params(dev, &params);
 665	if (rc < 0)
 666		goto out_free;
 667
 668	if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
 669	    nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
 670	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_ENABLED, params.enabled) ||
 671	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_SECLEVEL, params.out_level) ||
 672	    nla_put_u32(msg, IEEE802154_ATTR_LLSEC_FRAME_COUNTER,
 673			be32_to_cpu(params.frame_counter)) ||
 674	    ieee802154_llsec_fill_key_id(msg, &params.out_key)) {
 675		rc = -ENOBUFS;
 676		goto out_free;
 677	}
 678
 679	dev_put(dev);
 680
 681	return ieee802154_nl_reply(msg, info);
 682out_free:
 683	nlmsg_free(msg);
 684out_dev:
 685	dev_put(dev);
 686	return rc;
 687}
 688
 689int ieee802154_llsec_setparams(struct sk_buff *skb, struct genl_info *info)
 690{
 691	struct net_device *dev = NULL;
 692	int rc = -EINVAL;
 693	struct ieee802154_mlme_ops *ops;
 694	struct ieee802154_llsec_params params;
 695	int changed = 0;
 696
 697	pr_debug("%s\n", __func__);
 698
 699	dev = ieee802154_nl_get_dev(info);
 700	if (!dev)
 701		return -ENODEV;
 702
 703	if (!info->attrs[IEEE802154_ATTR_LLSEC_ENABLED] &&
 704	    !info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE] &&
 705	    !info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL])
 706		goto out;
 707
 708	ops = ieee802154_mlme_ops(dev);
 709	if (!ops->llsec) {
 710		rc = -EOPNOTSUPP;
 711		goto out;
 712	}
 713
 714	if (info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL] &&
 715	    nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL]) > 7)
 716		goto out;
 717
 718	if (info->attrs[IEEE802154_ATTR_LLSEC_ENABLED]) {
 719		params.enabled = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_ENABLED]);
 720		changed |= IEEE802154_LLSEC_PARAM_ENABLED;
 721	}
 722
 723	if (info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE]) {
 724		if (ieee802154_llsec_parse_key_id(info, &params.out_key))
 725			goto out;
 726
 727		changed |= IEEE802154_LLSEC_PARAM_OUT_KEY;
 728	}
 729
 730	if (info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL]) {
 731		params.out_level = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL]);
 732		changed |= IEEE802154_LLSEC_PARAM_OUT_LEVEL;
 733	}
 734
 735	if (info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]) {
 736		u32 fc = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]);
 737
 738		params.frame_counter = cpu_to_be32(fc);
 739		changed |= IEEE802154_LLSEC_PARAM_FRAME_COUNTER;
 740	}
 741
 742	rc = ops->llsec->set_params(dev, &params, changed);
 743
 744	dev_put(dev);
 745
 746	return rc;
 747out:
 748	dev_put(dev);
 749	return rc;
 750}
 751
 752struct llsec_dump_data {
 753	struct sk_buff *skb;
 754	int s_idx, s_idx2;
 755	int portid;
 756	int nlmsg_seq;
 757	struct net_device *dev;
 758	struct ieee802154_mlme_ops *ops;
 759	struct ieee802154_llsec_table *table;
 760};
 761
 762static int
 763ieee802154_llsec_dump_table(struct sk_buff *skb, struct netlink_callback *cb,
 764			    int (*step)(struct llsec_dump_data *))
 765{
 766	struct net *net = sock_net(skb->sk);
 767	struct net_device *dev;
 768	struct llsec_dump_data data;
 769	int idx = 0;
 770	int first_dev = cb->args[0];
 771	int rc;
 772
 773	for_each_netdev(net, dev) {
 774		if (idx < first_dev || dev->type != ARPHRD_IEEE802154)
 775			goto skip;
 776
 777		data.ops = ieee802154_mlme_ops(dev);
 778		if (!data.ops->llsec)
 779			goto skip;
 780
 781		data.skb = skb;
 782		data.s_idx = cb->args[1];
 783		data.s_idx2 = cb->args[2];
 784		data.dev = dev;
 785		data.portid = NETLINK_CB(cb->skb).portid;
 786		data.nlmsg_seq = cb->nlh->nlmsg_seq;
 787
 788		data.ops->llsec->lock_table(dev);
 789		data.ops->llsec->get_table(data.dev, &data.table);
 790		rc = step(&data);
 791		data.ops->llsec->unlock_table(dev);
 792
 793		if (rc < 0)
 794			break;
 795
 796skip:
 797		idx++;
 798	}
 799	cb->args[0] = idx;
 800
 801	return skb->len;
 802}
 803
 804static int
 805ieee802154_nl_llsec_change(struct sk_buff *skb, struct genl_info *info,
 806			   int (*fn)(struct net_device*, struct genl_info*))
 807{
 808	struct net_device *dev = NULL;
 809	int rc = -EINVAL;
 810
 811	dev = ieee802154_nl_get_dev(info);
 812	if (!dev)
 813		return -ENODEV;
 814
 815	if (!ieee802154_mlme_ops(dev)->llsec)
 816		rc = -EOPNOTSUPP;
 817	else
 818		rc = fn(dev, info);
 819
 820	dev_put(dev);
 821	return rc;
 822}
 823
 824static int
 825ieee802154_llsec_parse_key(struct genl_info *info,
 826			   struct ieee802154_llsec_key *key)
 827{
 828	u8 frames;
 829	u32 commands[256 / 32];
 830
 831	memset(key, 0, sizeof(*key));
 832
 833	if (!info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_FRAME_TYPES] ||
 834	    !info->attrs[IEEE802154_ATTR_LLSEC_KEY_BYTES])
 835		return -EINVAL;
 836
 837	frames = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_FRAME_TYPES]);
 838	if ((frames & BIT(IEEE802154_FC_TYPE_MAC_CMD)) &&
 839	    !info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS])
 840		return -EINVAL;
 841
 842	if (info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS]) {
 843		nla_memcpy(commands,
 844			   info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS],
 845			   256 / 8);
 846
 847		if (commands[0] || commands[1] || commands[2] || commands[3] ||
 848		    commands[4] || commands[5] || commands[6] ||
 849		    commands[7] >= BIT(IEEE802154_CMD_GTS_REQ + 1))
 850			return -EINVAL;
 851
 852		key->cmd_frame_ids = commands[7];
 853	}
 854
 855	key->frame_types = frames;
 856
 857	nla_memcpy(key->key, info->attrs[IEEE802154_ATTR_LLSEC_KEY_BYTES],
 858		   IEEE802154_LLSEC_KEY_SIZE);
 859
 860	return 0;
 861}
 862
 863static int llsec_add_key(struct net_device *dev, struct genl_info *info)
 864{
 865	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
 866	struct ieee802154_llsec_key key;
 867	struct ieee802154_llsec_key_id id;
 868
 869	if (ieee802154_llsec_parse_key(info, &key) ||
 870	    ieee802154_llsec_parse_key_id(info, &id))
 871		return -EINVAL;
 872
 873	return ops->llsec->add_key(dev, &id, &key);
 874}
 875
 876int ieee802154_llsec_add_key(struct sk_buff *skb, struct genl_info *info)
 877{
 878	if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
 879	    (NLM_F_CREATE | NLM_F_EXCL))
 880		return -EINVAL;
 881
 882	return ieee802154_nl_llsec_change(skb, info, llsec_add_key);
 883}
 884
 885static int llsec_remove_key(struct net_device *dev, struct genl_info *info)
 886{
 887	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
 888	struct ieee802154_llsec_key_id id;
 889
 890	if (ieee802154_llsec_parse_key_id(info, &id))
 891		return -EINVAL;
 892
 893	return ops->llsec->del_key(dev, &id);
 894}
 895
 896int ieee802154_llsec_del_key(struct sk_buff *skb, struct genl_info *info)
 897{
 898	return ieee802154_nl_llsec_change(skb, info, llsec_remove_key);
 899}
 900
 901static int
 902ieee802154_nl_fill_key(struct sk_buff *msg, u32 portid, u32 seq,
 903		       const struct ieee802154_llsec_key_entry *key,
 904		       const struct net_device *dev)
 905{
 906	void *hdr;
 907	u32 commands[256 / 32];
 908
 909	hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
 910			  IEEE802154_LLSEC_LIST_KEY);
 911	if (!hdr)
 912		goto out;
 913
 914	if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
 915	    nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
 916	    ieee802154_llsec_fill_key_id(msg, &key->id) ||
 917	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_KEY_USAGE_FRAME_TYPES,
 918		       key->key->frame_types))
 919		goto nla_put_failure;
 920
 921	if (key->key->frame_types & BIT(IEEE802154_FC_TYPE_MAC_CMD)) {
 922		memset(commands, 0, sizeof(commands));
 923		commands[7] = key->key->cmd_frame_ids;
 924		if (nla_put(msg, IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS,
 925			    sizeof(commands), commands))
 926			goto nla_put_failure;
 927	}
 928
 929	if (nla_put(msg, IEEE802154_ATTR_LLSEC_KEY_BYTES,
 930		    IEEE802154_LLSEC_KEY_SIZE, key->key->key))
 931		goto nla_put_failure;
 932
 933	genlmsg_end(msg, hdr);
 934	return 0;
 935
 936nla_put_failure:
 937	genlmsg_cancel(msg, hdr);
 938out:
 939	return -EMSGSIZE;
 940}
 941
 942static int llsec_iter_keys(struct llsec_dump_data *data)
 943{
 944	struct ieee802154_llsec_key_entry *pos;
 945	int rc = 0, idx = 0;
 946
 947	list_for_each_entry(pos, &data->table->keys, list) {
 948		if (idx++ < data->s_idx)
 949			continue;
 950
 951		if (ieee802154_nl_fill_key(data->skb, data->portid,
 952					   data->nlmsg_seq, pos, data->dev)) {
 953			rc = -EMSGSIZE;
 954			break;
 955		}
 956
 957		data->s_idx++;
 958	}
 959
 960	return rc;
 961}
 962
 963int ieee802154_llsec_dump_keys(struct sk_buff *skb, struct netlink_callback *cb)
 964{
 965	return ieee802154_llsec_dump_table(skb, cb, llsec_iter_keys);
 966}
 967
 968static int
 969llsec_parse_dev(struct genl_info *info,
 970		struct ieee802154_llsec_device *dev)
 971{
 972	memset(dev, 0, sizeof(*dev));
 973
 974	if (!info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER] ||
 975	    !info->attrs[IEEE802154_ATTR_HW_ADDR] ||
 976	    !info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE] ||
 977	    !info->attrs[IEEE802154_ATTR_LLSEC_DEV_KEY_MODE] ||
 978	    (!!info->attrs[IEEE802154_ATTR_PAN_ID] !=
 979	     !!info->attrs[IEEE802154_ATTR_SHORT_ADDR]))
 980		return -EINVAL;
 981
 982	if (info->attrs[IEEE802154_ATTR_PAN_ID]) {
 983		dev->pan_id = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_PAN_ID]);
 984		dev->short_addr = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_SHORT_ADDR]);
 985	} else {
 986		dev->short_addr = cpu_to_le16(IEEE802154_ADDR_UNDEF);
 987	}
 988
 989	dev->hwaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
 990	dev->frame_counter = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]);
 991	dev->seclevel_exempt = !!nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE]);
 992	dev->key_mode = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_DEV_KEY_MODE]);
 993
 994	if (dev->key_mode >= __IEEE802154_LLSEC_DEVKEY_MAX)
 995		return -EINVAL;
 996
 997	return 0;
 998}
 999
1000static int llsec_add_dev(struct net_device *dev, struct genl_info *info)
1001{
1002	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1003	struct ieee802154_llsec_device desc;
1004
1005	if (llsec_parse_dev(info, &desc))
1006		return -EINVAL;
1007
1008	return ops->llsec->add_dev(dev, &desc);
1009}
1010
1011int ieee802154_llsec_add_dev(struct sk_buff *skb, struct genl_info *info)
1012{
1013	if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
1014	    (NLM_F_CREATE | NLM_F_EXCL))
1015		return -EINVAL;
1016
1017	return ieee802154_nl_llsec_change(skb, info, llsec_add_dev);
1018}
1019
1020static int llsec_del_dev(struct net_device *dev, struct genl_info *info)
1021{
1022	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1023	__le64 devaddr;
1024
1025	if (!info->attrs[IEEE802154_ATTR_HW_ADDR])
1026		return -EINVAL;
1027
1028	devaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1029
1030	return ops->llsec->del_dev(dev, devaddr);
1031}
1032
1033int ieee802154_llsec_del_dev(struct sk_buff *skb, struct genl_info *info)
1034{
1035	return ieee802154_nl_llsec_change(skb, info, llsec_del_dev);
1036}
1037
1038static int
1039ieee802154_nl_fill_dev(struct sk_buff *msg, u32 portid, u32 seq,
1040		       const struct ieee802154_llsec_device *desc,
1041		       const struct net_device *dev)
1042{
1043	void *hdr;
1044
1045	hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
1046			  IEEE802154_LLSEC_LIST_DEV);
1047	if (!hdr)
1048		goto out;
1049
1050	if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
1051	    nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
1052	    nla_put_shortaddr(msg, IEEE802154_ATTR_PAN_ID, desc->pan_id) ||
1053	    nla_put_shortaddr(msg, IEEE802154_ATTR_SHORT_ADDR,
1054			      desc->short_addr) ||
1055	    nla_put_hwaddr(msg, IEEE802154_ATTR_HW_ADDR, desc->hwaddr,
1056			   IEEE802154_ATTR_PAD) ||
1057	    nla_put_u32(msg, IEEE802154_ATTR_LLSEC_FRAME_COUNTER,
1058			desc->frame_counter) ||
1059	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_DEV_OVERRIDE,
1060		       desc->seclevel_exempt) ||
1061	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_DEV_KEY_MODE, desc->key_mode))
1062		goto nla_put_failure;
1063
1064	genlmsg_end(msg, hdr);
1065	return 0;
1066
1067nla_put_failure:
1068	genlmsg_cancel(msg, hdr);
1069out:
1070	return -EMSGSIZE;
1071}
1072
1073static int llsec_iter_devs(struct llsec_dump_data *data)
1074{
1075	struct ieee802154_llsec_device *pos;
1076	int rc = 0, idx = 0;
1077
1078	list_for_each_entry(pos, &data->table->devices, list) {
1079		if (idx++ < data->s_idx)
1080			continue;
1081
1082		if (ieee802154_nl_fill_dev(data->skb, data->portid,
1083					   data->nlmsg_seq, pos, data->dev)) {
1084			rc = -EMSGSIZE;
1085			break;
1086		}
1087
1088		data->s_idx++;
1089	}
1090
1091	return rc;
1092}
1093
1094int ieee802154_llsec_dump_devs(struct sk_buff *skb, struct netlink_callback *cb)
1095{
1096	return ieee802154_llsec_dump_table(skb, cb, llsec_iter_devs);
1097}
1098
1099static int llsec_add_devkey(struct net_device *dev, struct genl_info *info)
1100{
1101	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1102	struct ieee802154_llsec_device_key key;
1103	__le64 devaddr;
1104
1105	if (!info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER] ||
1106	    !info->attrs[IEEE802154_ATTR_HW_ADDR] ||
1107	    ieee802154_llsec_parse_key_id(info, &key.key_id))
1108		return -EINVAL;
1109
1110	devaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1111	key.frame_counter = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]);
1112
1113	return ops->llsec->add_devkey(dev, devaddr, &key);
1114}
1115
1116int ieee802154_llsec_add_devkey(struct sk_buff *skb, struct genl_info *info)
1117{
1118	if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
1119	    (NLM_F_CREATE | NLM_F_EXCL))
1120		return -EINVAL;
1121
1122	return ieee802154_nl_llsec_change(skb, info, llsec_add_devkey);
1123}
1124
1125static int llsec_del_devkey(struct net_device *dev, struct genl_info *info)
1126{
1127	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1128	struct ieee802154_llsec_device_key key;
1129	__le64 devaddr;
1130
1131	if (!info->attrs[IEEE802154_ATTR_HW_ADDR] ||
1132	    ieee802154_llsec_parse_key_id(info, &key.key_id))
1133		return -EINVAL;
1134
1135	devaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1136
1137	return ops->llsec->del_devkey(dev, devaddr, &key);
1138}
1139
1140int ieee802154_llsec_del_devkey(struct sk_buff *skb, struct genl_info *info)
1141{
1142	return ieee802154_nl_llsec_change(skb, info, llsec_del_devkey);
1143}
1144
1145static int
1146ieee802154_nl_fill_devkey(struct sk_buff *msg, u32 portid, u32 seq,
1147			  __le64 devaddr,
1148			  const struct ieee802154_llsec_device_key *devkey,
1149			  const struct net_device *dev)
1150{
1151	void *hdr;
1152
1153	hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
1154			  IEEE802154_LLSEC_LIST_DEVKEY);
1155	if (!hdr)
1156		goto out;
1157
1158	if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
1159	    nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
1160	    nla_put_hwaddr(msg, IEEE802154_ATTR_HW_ADDR, devaddr,
1161			   IEEE802154_ATTR_PAD) ||
1162	    nla_put_u32(msg, IEEE802154_ATTR_LLSEC_FRAME_COUNTER,
1163			devkey->frame_counter) ||
1164	    ieee802154_llsec_fill_key_id(msg, &devkey->key_id))
1165		goto nla_put_failure;
1166
1167	genlmsg_end(msg, hdr);
1168	return 0;
1169
1170nla_put_failure:
1171	genlmsg_cancel(msg, hdr);
1172out:
1173	return -EMSGSIZE;
1174}
1175
1176static int llsec_iter_devkeys(struct llsec_dump_data *data)
1177{
1178	struct ieee802154_llsec_device *dpos;
1179	struct ieee802154_llsec_device_key *kpos;
1180	int idx = 0, idx2;
1181
1182	list_for_each_entry(dpos, &data->table->devices, list) {
1183		if (idx++ < data->s_idx)
1184			continue;
1185
1186		idx2 = 0;
1187
1188		list_for_each_entry(kpos, &dpos->keys, list) {
1189			if (idx2++ < data->s_idx2)
1190				continue;
1191
1192			if (ieee802154_nl_fill_devkey(data->skb, data->portid,
1193						      data->nlmsg_seq,
1194						      dpos->hwaddr, kpos,
1195						      data->dev)) {
1196				return -EMSGSIZE;
1197			}
1198
1199			data->s_idx2++;
1200		}
1201
1202		data->s_idx++;
1203	}
1204
1205	return 0;
1206}
1207
1208int ieee802154_llsec_dump_devkeys(struct sk_buff *skb,
1209				  struct netlink_callback *cb)
1210{
1211	return ieee802154_llsec_dump_table(skb, cb, llsec_iter_devkeys);
1212}
1213
1214static int
1215llsec_parse_seclevel(struct genl_info *info,
1216		     struct ieee802154_llsec_seclevel *sl)
1217{
1218	memset(sl, 0, sizeof(*sl));
1219
1220	if (!info->attrs[IEEE802154_ATTR_LLSEC_FRAME_TYPE] ||
1221	    !info->attrs[IEEE802154_ATTR_LLSEC_SECLEVELS] ||
1222	    !info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE])
1223		return -EINVAL;
1224
1225	sl->frame_type = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_TYPE]);
1226	if (sl->frame_type == IEEE802154_FC_TYPE_MAC_CMD) {
1227		if (!info->attrs[IEEE802154_ATTR_LLSEC_CMD_FRAME_ID])
1228			return -EINVAL;
1229
1230		sl->cmd_frame_id = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_CMD_FRAME_ID]);
1231	}
1232
1233	sl->sec_levels = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_SECLEVELS]);
1234	sl->device_override = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE]);
1235
1236	return 0;
1237}
1238
1239static int llsec_add_seclevel(struct net_device *dev, struct genl_info *info)
1240{
1241	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1242	struct ieee802154_llsec_seclevel sl;
1243
1244	if (llsec_parse_seclevel(info, &sl))
1245		return -EINVAL;
1246
1247	return ops->llsec->add_seclevel(dev, &sl);
1248}
1249
1250int ieee802154_llsec_add_seclevel(struct sk_buff *skb, struct genl_info *info)
1251{
1252	if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
1253	    (NLM_F_CREATE | NLM_F_EXCL))
1254		return -EINVAL;
1255
1256	return ieee802154_nl_llsec_change(skb, info, llsec_add_seclevel);
1257}
1258
1259static int llsec_del_seclevel(struct net_device *dev, struct genl_info *info)
1260{
1261	struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1262	struct ieee802154_llsec_seclevel sl;
1263
1264	if (llsec_parse_seclevel(info, &sl))
1265		return -EINVAL;
1266
1267	return ops->llsec->del_seclevel(dev, &sl);
1268}
1269
1270int ieee802154_llsec_del_seclevel(struct sk_buff *skb, struct genl_info *info)
1271{
1272	return ieee802154_nl_llsec_change(skb, info, llsec_del_seclevel);
1273}
1274
1275static int
1276ieee802154_nl_fill_seclevel(struct sk_buff *msg, u32 portid, u32 seq,
1277			    const struct ieee802154_llsec_seclevel *sl,
1278			    const struct net_device *dev)
1279{
1280	void *hdr;
1281
1282	hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
1283			  IEEE802154_LLSEC_LIST_SECLEVEL);
1284	if (!hdr)
1285		goto out;
1286
1287	if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
1288	    nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
1289	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_FRAME_TYPE, sl->frame_type) ||
1290	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_SECLEVELS, sl->sec_levels) ||
1291	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_DEV_OVERRIDE,
1292		       sl->device_override))
1293		goto nla_put_failure;
1294
1295	if (sl->frame_type == IEEE802154_FC_TYPE_MAC_CMD &&
1296	    nla_put_u8(msg, IEEE802154_ATTR_LLSEC_CMD_FRAME_ID,
1297		       sl->cmd_frame_id))
1298		goto nla_put_failure;
1299
1300	genlmsg_end(msg, hdr);
1301	return 0;
1302
1303nla_put_failure:
1304	genlmsg_cancel(msg, hdr);
1305out:
1306	return -EMSGSIZE;
1307}
1308
1309static int llsec_iter_seclevels(struct llsec_dump_data *data)
1310{
1311	struct ieee802154_llsec_seclevel *pos;
1312	int rc = 0, idx = 0;
1313
1314	list_for_each_entry(pos, &data->table->security_levels, list) {
1315		if (idx++ < data->s_idx)
1316			continue;
1317
1318		if (ieee802154_nl_fill_seclevel(data->skb, data->portid,
1319						data->nlmsg_seq, pos,
1320						data->dev)) {
1321			rc = -EMSGSIZE;
1322			break;
1323		}
1324
1325		data->s_idx++;
1326	}
1327
1328	return rc;
1329}
1330
1331int ieee802154_llsec_dump_seclevels(struct sk_buff *skb,
1332				    struct netlink_callback *cb)
1333{
1334	return ieee802154_llsec_dump_table(skb, cb, llsec_iter_seclevels);
1335}