net/ieee802154/nl-mac.c

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