kernel/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                     !(info->attrs[IEEE802154_ATTR_SHORT_ADDR] ||
 562                       info->attrs[IEEE802154_ATTR_HW_ADDR]))
 563                         return -EINVAL;
 564
 565                 desc->device_addr.pan_id = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_PAN_ID]);
 566
 567                 if (info->attrs[IEEE802154_ATTR_SHORT_ADDR]) {
 568                         desc->device_addr.mode = IEEE802154_ADDR_SHORT;
 569                         desc->device_addr.short_addr = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_SHORT_ADDR]);
 570                 } else {
 571                         desc->device_addr.mode = IEEE802154_ADDR_LONG;
 572                         desc->device_addr.extended_addr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
 573                 }
 574         }
 575
 576         if (desc->mode != IEEE802154_SCF_KEY_IMPLICIT &&
 577             !info->attrs[IEEE802154_ATTR_LLSEC_KEY_ID])
 578                 return -EINVAL;
 579
 580         if (desc->mode == IEEE802154_SCF_KEY_SHORT_INDEX &&
 581             !info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_SHORT])
 582                 return -EINVAL;
 583
 584         if (desc->mode == IEEE802154_SCF_KEY_HW_INDEX &&
 585             !info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_EXTENDED])
 586                 return -EINVAL;
 587
 588         if (desc->mode != IEEE802154_SCF_KEY_IMPLICIT)
 589                 desc->id = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_KEY_ID]);
 590
 591         switch (desc->mode) {
 592         case IEEE802154_SCF_KEY_SHORT_INDEX:
 593         {
 594                 u32 source = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_SHORT]);
 595
 596                 desc->short_source = cpu_to_le32(source);
 597                 break;
 598         }
 599         case IEEE802154_SCF_KEY_HW_INDEX:
 600                 desc->extended_source = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_EXTENDED]);
 601                 break;
 602         }
 603
 604         return 0;
 605 }
 606
 607 static int
 608 ieee802154_llsec_fill_key_id(struct sk_buff *msg,
 609                              const struct ieee802154_llsec_key_id *desc)
 610 {
 611         if (nla_put_u8(msg, IEEE802154_ATTR_LLSEC_KEY_MODE, desc->mode))
 612                 return -EMSGSIZE;
 613
 614         if (desc->mode == IEEE802154_SCF_KEY_IMPLICIT) {
 615                 if (nla_put_shortaddr(msg, IEEE802154_ATTR_PAN_ID,
 616                                       desc->device_addr.pan_id))
 617                         return -EMSGSIZE;
 618
 619                 if (desc->device_addr.mode == IEEE802154_ADDR_SHORT &&
 620                     nla_put_shortaddr(msg, IEEE802154_ATTR_SHORT_ADDR,
 621                                       desc->device_addr.short_addr))
 622                         return -EMSGSIZE;
 623
 624                 if (desc->device_addr.mode == IEEE802154_ADDR_LONG &&
 625                     nla_put_hwaddr(msg, IEEE802154_ATTR_HW_ADDR,
 626                                    desc->device_addr.extended_addr))
 627                         return -EMSGSIZE;
 628         }
 629
 630         if (desc->mode != IEEE802154_SCF_KEY_IMPLICIT &&
 631             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_KEY_ID, desc->id))
 632                 return -EMSGSIZE;
 633
 634         if (desc->mode == IEEE802154_SCF_KEY_SHORT_INDEX &&
 635             nla_put_u32(msg, IEEE802154_ATTR_LLSEC_KEY_SOURCE_SHORT,
 636                         le32_to_cpu(desc->short_source)))
 637                 return -EMSGSIZE;
 638
 639         if (desc->mode == IEEE802154_SCF_KEY_HW_INDEX &&
 640             nla_put_hwaddr(msg, IEEE802154_ATTR_LLSEC_KEY_SOURCE_EXTENDED,
 641                            desc->extended_source))
 642                 return -EMSGSIZE;
 643
 644         return 0;
 645 }
 646
 647 int ieee802154_llsec_getparams(struct sk_buff *skb, struct genl_info *info)
 648 {
 649         struct sk_buff *msg;
 650         struct net_device *dev = NULL;
 651         int rc = -ENOBUFS;
 652         struct ieee802154_mlme_ops *ops;
 653         void *hdr;
 654         struct ieee802154_llsec_params params;
 655
 656         pr_debug("%s\n", __func__);
 657
 658         dev = ieee802154_nl_get_dev(info);
 659         if (!dev)
 660                 return -ENODEV;
 661
 662         ops = ieee802154_mlme_ops(dev);
 663         if (!ops->llsec) {
 664                 rc = -EOPNOTSUPP;
 665                 goto out_dev;
 666         }
 667
 668         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 669         if (!msg)
 670                 goto out_dev;
 671
 672         hdr = genlmsg_put(msg, 0, info->snd_seq, &nl802154_family, 0,
 673                           IEEE802154_LLSEC_GETPARAMS);
 674         if (!hdr)
 675                 goto out_free;
 676
 677         rc = ops->llsec->get_params(dev, &params);
 678         if (rc < 0)
 679                 goto out_free;
 680
 681         if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
 682             nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
 683             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_ENABLED, params.enabled) ||
 684             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_SECLEVEL, params.out_level) ||
 685             nla_put_u32(msg, IEEE802154_ATTR_LLSEC_FRAME_COUNTER,
 686                         be32_to_cpu(params.frame_counter)) ||
 687             ieee802154_llsec_fill_key_id(msg, &params.out_key))
 688                 goto out_free;
 689
 690         dev_put(dev);
 691
 692         return ieee802154_nl_reply(msg, info);
 693 out_free:
 694         nlmsg_free(msg);
 695 out_dev:
 696         dev_put(dev);
 697         return rc;
 698 }
 699
 700 int ieee802154_llsec_setparams(struct sk_buff *skb, struct genl_info *info)
 701 {
 702         struct net_device *dev = NULL;
 703         int rc = -EINVAL;
 704         struct ieee802154_mlme_ops *ops;
 705         struct ieee802154_llsec_params params;
 706         int changed = 0;
 707
 708         pr_debug("%s\n", __func__);
 709
 710         dev = ieee802154_nl_get_dev(info);
 711         if (!dev)
 712                 return -ENODEV;
 713
 714         if (!info->attrs[IEEE802154_ATTR_LLSEC_ENABLED] &&
 715             !info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE] &&
 716             !info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL])
 717                 goto out;
 718
 719         ops = ieee802154_mlme_ops(dev);
 720         if (!ops->llsec) {
 721                 rc = -EOPNOTSUPP;
 722                 goto out;
 723         }
 724
 725         if (info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL] &&
 726             nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL]) > 7)
 727                 goto out;
 728
 729         if (info->attrs[IEEE802154_ATTR_LLSEC_ENABLED]) {
 730                 params.enabled = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_ENABLED]);
 731                 changed |= IEEE802154_LLSEC_PARAM_ENABLED;
 732         }
 733
 734         if (info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE]) {
 735                 if (ieee802154_llsec_parse_key_id(info, &params.out_key))
 736                         goto out;
 737
 738                 changed |= IEEE802154_LLSEC_PARAM_OUT_KEY;
 739         }
 740
 741         if (info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL]) {
 742                 params.out_level = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL]);
 743                 changed |= IEEE802154_LLSEC_PARAM_OUT_LEVEL;
 744         }
 745
 746         if (info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]) {
 747                 u32 fc = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]);
 748
 749                 params.frame_counter = cpu_to_be32(fc);
 750                 changed |= IEEE802154_LLSEC_PARAM_FRAME_COUNTER;
 751         }
 752
 753         rc = ops->llsec->set_params(dev, &params, changed);
 754
 755         dev_put(dev);
 756
 757         return rc;
 758 out:
 759         dev_put(dev);
 760         return rc;
 761 }
 762
 763 struct llsec_dump_data {
 764         struct sk_buff *skb;
 765         int s_idx, s_idx2;
 766         int portid;
 767         int nlmsg_seq;
 768         struct net_device *dev;
 769         struct ieee802154_mlme_ops *ops;
 770         struct ieee802154_llsec_table *table;
 771 };
 772
 773 static int
 774 ieee802154_llsec_dump_table(struct sk_buff *skb, struct netlink_callback *cb,
 775                             int (*step)(struct llsec_dump_data *))
 776 {
 777         struct net *net = sock_net(skb->sk);
 778         struct net_device *dev;
 779         struct llsec_dump_data data;
 780         int idx = 0;
 781         int first_dev = cb->args[0];
 782         int rc;
 783
 784         for_each_netdev(net, dev) {
 785                 if (idx < first_dev || dev->type != ARPHRD_IEEE802154)
 786                         goto skip;
 787
 788                 data.ops = ieee802154_mlme_ops(dev);
 789                 if (!data.ops->llsec)
 790                         goto skip;
 791
 792                 data.skb = skb;
 793                 data.s_idx = cb->args[1];
 794                 data.s_idx2 = cb->args[2];
 795                 data.dev = dev;
 796                 data.portid = NETLINK_CB(cb->skb).portid;
 797                 data.nlmsg_seq = cb->nlh->nlmsg_seq;
 798
 799                 data.ops->llsec->lock_table(dev);
 800                 data.ops->llsec->get_table(data.dev, &data.table);
 801                 rc = step(&data);
 802                 data.ops->llsec->unlock_table(dev);
 803
 804                 if (rc < 0)
 805                         break;
 806
 807 skip:
 808                 idx++;
 809         }
 810         cb->args[0] = idx;
 811
 812         return skb->len;
 813 }
 814
 815 static int
 816 ieee802154_nl_llsec_change(struct sk_buff *skb, struct genl_info *info,
 817                            int (*fn)(struct net_device*, struct genl_info*))
 818 {
 819         struct net_device *dev = NULL;
 820         int rc = -EINVAL;
 821
 822         dev = ieee802154_nl_get_dev(info);
 823         if (!dev)
 824                 return -ENODEV;
 825
 826         if (!ieee802154_mlme_ops(dev)->llsec)
 827                 rc = -EOPNOTSUPP;
 828         else
 829                 rc = fn(dev, info);
 830
 831         dev_put(dev);
 832         return rc;
 833 }
 834
 835 static int
 836 ieee802154_llsec_parse_key(struct genl_info *info,
 837                            struct ieee802154_llsec_key *key)
 838 {
 839         u8 frames;
 840         u32 commands[256 / 32];
 841
 842         memset(key, 0, sizeof(*key));
 843
 844         if (!info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_FRAME_TYPES] ||
 845             !info->attrs[IEEE802154_ATTR_LLSEC_KEY_BYTES])
 846                 return -EINVAL;
 847
 848         frames = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_FRAME_TYPES]);
 849         if ((frames & BIT(IEEE802154_FC_TYPE_MAC_CMD)) &&
 850             !info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS])
 851                 return -EINVAL;
 852
 853         if (info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS]) {
 854                 nla_memcpy(commands,
 855                            info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS],
 856                            256 / 8);
 857
 858                 if (commands[0] || commands[1] || commands[2] || commands[3] ||
 859                     commands[4] || commands[5] || commands[6] ||
 860                     commands[7] >= BIT(IEEE802154_CMD_GTS_REQ + 1))
 861                         return -EINVAL;
 862
 863                 key->cmd_frame_ids = commands[7];
 864         }
 865
 866         key->frame_types = frames;
 867
 868         nla_memcpy(key->key, info->attrs[IEEE802154_ATTR_LLSEC_KEY_BYTES],
 869                    IEEE802154_LLSEC_KEY_SIZE);
 870
 871         return 0;
 872 }
 873
 874 static int llsec_add_key(struct net_device *dev, struct genl_info *info)
 875 {
 876         struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
 877         struct ieee802154_llsec_key key;
 878         struct ieee802154_llsec_key_id id;
 879
 880         if (ieee802154_llsec_parse_key(info, &key) ||
 881             ieee802154_llsec_parse_key_id(info, &id))
 882                 return -EINVAL;
 883
 884         return ops->llsec->add_key(dev, &id, &key);
 885 }
 886
 887 int ieee802154_llsec_add_key(struct sk_buff *skb, struct genl_info *info)
 888 {
 889         if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
 890             (NLM_F_CREATE | NLM_F_EXCL))
 891                 return -EINVAL;
 892
 893         return ieee802154_nl_llsec_change(skb, info, llsec_add_key);
 894 }
 895
 896 static int llsec_remove_key(struct net_device *dev, struct genl_info *info)
 897 {
 898         struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
 899         struct ieee802154_llsec_key_id id;
 900
 901         if (ieee802154_llsec_parse_key_id(info, &id))
 902                 return -EINVAL;
 903
 904         return ops->llsec->del_key(dev, &id);
 905 }
 906
 907 int ieee802154_llsec_del_key(struct sk_buff *skb, struct genl_info *info)
 908 {
 909         return ieee802154_nl_llsec_change(skb, info, llsec_remove_key);
 910 }
 911
 912 static int
 913 ieee802154_nl_fill_key(struct sk_buff *msg, u32 portid, u32 seq,
 914                        const struct ieee802154_llsec_key_entry *key,
 915                        const struct net_device *dev)
 916 {
 917         void *hdr;
 918         u32 commands[256 / 32];
 919
 920         hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
 921                           IEEE802154_LLSEC_LIST_KEY);
 922         if (!hdr)
 923                 goto out;
 924
 925         if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
 926             nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
 927             ieee802154_llsec_fill_key_id(msg, &key->id) ||
 928             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_KEY_USAGE_FRAME_TYPES,
 929                        key->key->frame_types))
 930                 goto nla_put_failure;
 931
 932         if (key->key->frame_types & BIT(IEEE802154_FC_TYPE_MAC_CMD)) {
 933                 memset(commands, 0, sizeof(commands));
 934                 commands[7] = key->key->cmd_frame_ids;
 935                 if (nla_put(msg, IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS,
 936                             sizeof(commands), commands))
 937                         goto nla_put_failure;
 938         }
 939
 940         if (nla_put(msg, IEEE802154_ATTR_LLSEC_KEY_BYTES,
 941                     IEEE802154_LLSEC_KEY_SIZE, key->key->key))
 942                 goto nla_put_failure;
 943
 944         genlmsg_end(msg, hdr);
 945         return 0;
 946
 947 nla_put_failure:
 948         genlmsg_cancel(msg, hdr);
 949 out:
 950         return -EMSGSIZE;
 951 }
 952
 953 static int llsec_iter_keys(struct llsec_dump_data *data)
 954 {
 955         struct ieee802154_llsec_key_entry *pos;
 956         int rc = 0, idx = 0;
 957
 958         list_for_each_entry(pos, &data->table->keys, list) {
 959                 if (idx++ < data->s_idx)
 960                         continue;
 961
 962                 if (ieee802154_nl_fill_key(data->skb, data->portid,
 963                                            data->nlmsg_seq, pos, data->dev)) {
 964                         rc = -EMSGSIZE;
 965                         break;
 966                 }
 967
 968                 data->s_idx++;
 969         }
 970
 971         return rc;
 972 }
 973
 974 int ieee802154_llsec_dump_keys(struct sk_buff *skb, struct netlink_callback *cb)
 975 {
 976         return ieee802154_llsec_dump_table(skb, cb, llsec_iter_keys);
 977 }
 978
 979 static int
 980 llsec_parse_dev(struct genl_info *info,
 981                 struct ieee802154_llsec_device *dev)
 982 {
 983         memset(dev, 0, sizeof(*dev));
 984
 985         if (!info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER] ||
 986             !info->attrs[IEEE802154_ATTR_HW_ADDR] ||
 987             !info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE] ||
 988             !info->attrs[IEEE802154_ATTR_LLSEC_DEV_KEY_MODE] ||
 989             (!!info->attrs[IEEE802154_ATTR_PAN_ID] !=
 990              !!info->attrs[IEEE802154_ATTR_SHORT_ADDR]))
 991                 return -EINVAL;
 992
 993         if (info->attrs[IEEE802154_ATTR_PAN_ID]) {
 994                 dev->pan_id = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_PAN_ID]);
 995                 dev->short_addr = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_SHORT_ADDR]);
 996         } else {
 997                 dev->short_addr = cpu_to_le16(IEEE802154_ADDR_UNDEF);
 998         }
 999
1000         dev->hwaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1001         dev->frame_counter = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]);
1002         dev->seclevel_exempt = !!nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE]);
1003         dev->key_mode = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_DEV_KEY_MODE]);
1004
1005         if (dev->key_mode >= __IEEE802154_LLSEC_DEVKEY_MAX)
1006                 return -EINVAL;
1007
1008         return 0;
1009 }
1010
1011 static int llsec_add_dev(struct net_device *dev, struct genl_info *info)
1012 {
1013         struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1014         struct ieee802154_llsec_device desc;
1015
1016         if (llsec_parse_dev(info, &desc))
1017                 return -EINVAL;
1018
1019         return ops->llsec->add_dev(dev, &desc);
1020 }
1021
1022 int ieee802154_llsec_add_dev(struct sk_buff *skb, struct genl_info *info)
1023 {
1024         if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
1025             (NLM_F_CREATE | NLM_F_EXCL))
1026                 return -EINVAL;
1027
1028         return ieee802154_nl_llsec_change(skb, info, llsec_add_dev);
1029 }
1030
1031 static int llsec_del_dev(struct net_device *dev, struct genl_info *info)
1032 {
1033         struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1034         __le64 devaddr;
1035
1036         if (!info->attrs[IEEE802154_ATTR_HW_ADDR])
1037                 return -EINVAL;
1038
1039         devaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1040
1041         return ops->llsec->del_dev(dev, devaddr);
1042 }
1043
1044 int ieee802154_llsec_del_dev(struct sk_buff *skb, struct genl_info *info)
1045 {
1046         return ieee802154_nl_llsec_change(skb, info, llsec_del_dev);
1047 }
1048
1049 static int
1050 ieee802154_nl_fill_dev(struct sk_buff *msg, u32 portid, u32 seq,
1051                        const struct ieee802154_llsec_device *desc,
1052                        const struct net_device *dev)
1053 {
1054         void *hdr;
1055
1056         hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
1057                           IEEE802154_LLSEC_LIST_DEV);
1058         if (!hdr)
1059                 goto out;
1060
1061         if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
1062             nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
1063             nla_put_shortaddr(msg, IEEE802154_ATTR_PAN_ID, desc->pan_id) ||
1064             nla_put_shortaddr(msg, IEEE802154_ATTR_SHORT_ADDR,
1065                               desc->short_addr) ||
1066             nla_put_hwaddr(msg, IEEE802154_ATTR_HW_ADDR, desc->hwaddr) ||
1067             nla_put_u32(msg, IEEE802154_ATTR_LLSEC_FRAME_COUNTER,
1068                         desc->frame_counter) ||
1069             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_DEV_OVERRIDE,
1070                        desc->seclevel_exempt) ||
1071             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_DEV_KEY_MODE, desc->key_mode))
1072                 goto nla_put_failure;
1073
1074         genlmsg_end(msg, hdr);
1075         return 0;
1076
1077 nla_put_failure:
1078         genlmsg_cancel(msg, hdr);
1079 out:
1080         return -EMSGSIZE;
1081 }
1082
1083 static int llsec_iter_devs(struct llsec_dump_data *data)
1084 {
1085         struct ieee802154_llsec_device *pos;
1086         int rc = 0, idx = 0;
1087
1088         list_for_each_entry(pos, &data->table->devices, list) {
1089                 if (idx++ < data->s_idx)
1090                         continue;
1091
1092                 if (ieee802154_nl_fill_dev(data->skb, data->portid,
1093                                            data->nlmsg_seq, pos, data->dev)) {
1094                         rc = -EMSGSIZE;
1095                         break;
1096                 }
1097
1098                 data->s_idx++;
1099         }
1100
1101         return rc;
1102 }
1103
1104 int ieee802154_llsec_dump_devs(struct sk_buff *skb, struct netlink_callback *cb)
1105 {
1106         return ieee802154_llsec_dump_table(skb, cb, llsec_iter_devs);
1107 }
1108
1109 static int llsec_add_devkey(struct net_device *dev, struct genl_info *info)
1110 {
1111         struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1112         struct ieee802154_llsec_device_key key;
1113         __le64 devaddr;
1114
1115         if (!info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER] ||
1116             !info->attrs[IEEE802154_ATTR_HW_ADDR] ||
1117             ieee802154_llsec_parse_key_id(info, &key.key_id))
1118                 return -EINVAL;
1119
1120         devaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1121         key.frame_counter = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]);
1122
1123         return ops->llsec->add_devkey(dev, devaddr, &key);
1124 }
1125
1126 int ieee802154_llsec_add_devkey(struct sk_buff *skb, struct genl_info *info)
1127 {
1128         if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
1129             (NLM_F_CREATE | NLM_F_EXCL))
1130                 return -EINVAL;
1131
1132         return ieee802154_nl_llsec_change(skb, info, llsec_add_devkey);
1133 }
1134
1135 static int llsec_del_devkey(struct net_device *dev, struct genl_info *info)
1136 {
1137         struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1138         struct ieee802154_llsec_device_key key;
1139         __le64 devaddr;
1140
1141         if (!info->attrs[IEEE802154_ATTR_HW_ADDR] ||
1142             ieee802154_llsec_parse_key_id(info, &key.key_id))
1143                 return -EINVAL;
1144
1145         devaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1146
1147         return ops->llsec->del_devkey(dev, devaddr, &key);
1148 }
1149
1150 int ieee802154_llsec_del_devkey(struct sk_buff *skb, struct genl_info *info)
1151 {
1152         return ieee802154_nl_llsec_change(skb, info, llsec_del_devkey);
1153 }
1154
1155 static int
1156 ieee802154_nl_fill_devkey(struct sk_buff *msg, u32 portid, u32 seq,
1157                           __le64 devaddr,
1158                           const struct ieee802154_llsec_device_key *devkey,
1159                           const struct net_device *dev)
1160 {
1161         void *hdr;
1162
1163         hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
1164                           IEEE802154_LLSEC_LIST_DEVKEY);
1165         if (!hdr)
1166                 goto out;
1167
1168         if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
1169             nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
1170             nla_put_hwaddr(msg, IEEE802154_ATTR_HW_ADDR, devaddr) ||
1171             nla_put_u32(msg, IEEE802154_ATTR_LLSEC_FRAME_COUNTER,
1172                         devkey->frame_counter) ||
1173             ieee802154_llsec_fill_key_id(msg, &devkey->key_id))
1174                 goto nla_put_failure;
1175
1176         genlmsg_end(msg, hdr);
1177         return 0;
1178
1179 nla_put_failure:
1180         genlmsg_cancel(msg, hdr);
1181 out:
1182         return -EMSGSIZE;
1183 }
1184
1185 static int llsec_iter_devkeys(struct llsec_dump_data *data)
1186 {
1187         struct ieee802154_llsec_device *dpos;
1188         struct ieee802154_llsec_device_key *kpos;
1189         int rc = 0, idx = 0, idx2;
1190
1191         list_for_each_entry(dpos, &data->table->devices, list) {
1192                 if (idx++ < data->s_idx)
1193                         continue;
1194
1195                 idx2 = 0;
1196
1197                 list_for_each_entry(kpos, &dpos->keys, list) {
1198                         if (idx2++ < data->s_idx2)
1199                                 continue;
1200
1201                         if (ieee802154_nl_fill_devkey(data->skb, data->portid,
1202                                                       data->nlmsg_seq,
1203                                                       dpos->hwaddr, kpos,
1204                                                       data->dev)) {
1205                                 return rc = -EMSGSIZE;
1206                         }
1207
1208                         data->s_idx2++;
1209                 }
1210
1211                 data->s_idx++;
1212         }
1213
1214         return rc;
1215 }
1216
1217 int ieee802154_llsec_dump_devkeys(struct sk_buff *skb,
1218                                   struct netlink_callback *cb)
1219 {
1220         return ieee802154_llsec_dump_table(skb, cb, llsec_iter_devkeys);
1221 }
1222
1223 static int
1224 llsec_parse_seclevel(struct genl_info *info,
1225                      struct ieee802154_llsec_seclevel *sl)
1226 {
1227         memset(sl, 0, sizeof(*sl));
1228
1229         if (!info->attrs[IEEE802154_ATTR_LLSEC_FRAME_TYPE] ||
1230             !info->attrs[IEEE802154_ATTR_LLSEC_SECLEVELS] ||
1231             !info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE])
1232                 return -EINVAL;
1233
1234         sl->frame_type = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_TYPE]);
1235         if (sl->frame_type == IEEE802154_FC_TYPE_MAC_CMD) {
1236                 if (!info->attrs[IEEE802154_ATTR_LLSEC_CMD_FRAME_ID])
1237                         return -EINVAL;
1238
1239                 sl->cmd_frame_id = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_CMD_FRAME_ID]);
1240         }
1241
1242         sl->sec_levels = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_SECLEVELS]);
1243         sl->device_override = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE]);
1244
1245         return 0;
1246 }
1247
1248 static int llsec_add_seclevel(struct net_device *dev, struct genl_info *info)
1249 {
1250         struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1251         struct ieee802154_llsec_seclevel sl;
1252
1253         if (llsec_parse_seclevel(info, &sl))
1254                 return -EINVAL;
1255
1256         return ops->llsec->add_seclevel(dev, &sl);
1257 }
1258
1259 int ieee802154_llsec_add_seclevel(struct sk_buff *skb, struct genl_info *info)
1260 {
1261         if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
1262             (NLM_F_CREATE | NLM_F_EXCL))
1263                 return -EINVAL;
1264
1265         return ieee802154_nl_llsec_change(skb, info, llsec_add_seclevel);
1266 }
1267
1268 static int llsec_del_seclevel(struct net_device *dev, struct genl_info *info)
1269 {
1270         struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1271         struct ieee802154_llsec_seclevel sl;
1272
1273         if (llsec_parse_seclevel(info, &sl))
1274                 return -EINVAL;
1275
1276         return ops->llsec->del_seclevel(dev, &sl);
1277 }
1278
1279 int ieee802154_llsec_del_seclevel(struct sk_buff *skb, struct genl_info *info)
1280 {
1281         return ieee802154_nl_llsec_change(skb, info, llsec_del_seclevel);
1282 }
1283
1284 static int
1285 ieee802154_nl_fill_seclevel(struct sk_buff *msg, u32 portid, u32 seq,
1286                             const struct ieee802154_llsec_seclevel *sl,
1287                             const struct net_device *dev)
1288 {
1289         void *hdr;
1290
1291         hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
1292                           IEEE802154_LLSEC_LIST_SECLEVEL);
1293         if (!hdr)
1294                 goto out;
1295
1296         if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
1297             nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
1298             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_FRAME_TYPE, sl->frame_type) ||
1299             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_SECLEVELS, sl->sec_levels) ||
1300             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_DEV_OVERRIDE,
1301                        sl->device_override))
1302                 goto nla_put_failure;
1303
1304         if (sl->frame_type == IEEE802154_FC_TYPE_MAC_CMD &&
1305             nla_put_u8(msg, IEEE802154_ATTR_LLSEC_CMD_FRAME_ID,
1306                        sl->cmd_frame_id))
1307                 goto nla_put_failure;
1308
1309         genlmsg_end(msg, hdr);
1310         return 0;
1311
1312 nla_put_failure:
1313         genlmsg_cancel(msg, hdr);
1314 out:
1315         return -EMSGSIZE;
1316 }
1317
1318 static int llsec_iter_seclevels(struct llsec_dump_data *data)
1319 {
1320         struct ieee802154_llsec_seclevel *pos;
1321         int rc = 0, idx = 0;
1322
1323         list_for_each_entry(pos, &data->table->security_levels, list) {
1324                 if (idx++ < data->s_idx)
1325                         continue;
1326
1327                 if (ieee802154_nl_fill_seclevel(data->skb, data->portid,
1328                                                 data->nlmsg_seq, pos,
1329                                                 data->dev)) {
1330                         rc = -EMSGSIZE;
1331                         break;
1332                 }
1333
1334                 data->s_idx++;
1335         }
1336
1337         return rc;
1338 }
1339
1340 int ieee802154_llsec_dump_seclevels(struct sk_buff *skb,
1341                                     struct netlink_callback *cb)
1342 {
1343         return ieee802154_llsec_dump_table(skb, cb, llsec_iter_seclevels);
1344 }