net/ieee802154/nl-mac.c

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