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