net/ieee802154/nl-mac.c

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