Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Netlink interface for IEEE 802.15.4 stack
0004  *
0005  * Copyright 2007, 2008 Siemens AG
0006  *
0007  * Written by:
0008  * Sergey Lapin <slapin@ossfans.org>
0009  * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
0010  * Maxim Osipov <maxim.osipov@siemens.com>
0011  */
0012 
0013 #include <linux/gfp.h>
0014 #include <linux/kernel.h>
0015 #include <linux/if_arp.h>
0016 #include <linux/netdevice.h>
0017 #include <linux/ieee802154.h>
0018 #include <net/netlink.h>
0019 #include <net/genetlink.h>
0020 #include <net/sock.h>
0021 #include <linux/nl802154.h>
0022 #include <linux/export.h>
0023 #include <net/af_ieee802154.h>
0024 #include <net/ieee802154_netdev.h>
0025 #include <net/cfg802154.h>
0026 
0027 #include "ieee802154.h"
0028 
0029 static int nla_put_hwaddr(struct sk_buff *msg, int type, __le64 hwaddr,
0030               int padattr)
0031 {
0032     return nla_put_u64_64bit(msg, type, swab64((__force u64)hwaddr),
0033                  padattr);
0034 }
0035 
0036 static __le64 nla_get_hwaddr(const struct nlattr *nla)
0037 {
0038     return ieee802154_devaddr_from_raw(nla_data(nla));
0039 }
0040 
0041 static int nla_put_shortaddr(struct sk_buff *msg, int type, __le16 addr)
0042 {
0043     return nla_put_u16(msg, type, le16_to_cpu(addr));
0044 }
0045 
0046 static __le16 nla_get_shortaddr(const struct nlattr *nla)
0047 {
0048     return cpu_to_le16(nla_get_u16(nla));
0049 }
0050 
0051 static int ieee802154_nl_start_confirm(struct net_device *dev, u8 status)
0052 {
0053     struct sk_buff *msg;
0054 
0055     pr_debug("%s\n", __func__);
0056 
0057     msg = ieee802154_nl_create(0, IEEE802154_START_CONF);
0058     if (!msg)
0059         return -ENOBUFS;
0060 
0061     if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
0062         nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
0063         nla_put(msg, IEEE802154_ATTR_HW_ADDR, IEEE802154_ADDR_LEN,
0064             dev->dev_addr) ||
0065         nla_put_u8(msg, IEEE802154_ATTR_STATUS, status))
0066         goto nla_put_failure;
0067     return ieee802154_nl_mcast(msg, IEEE802154_COORD_MCGRP);
0068 
0069 nla_put_failure:
0070     nlmsg_free(msg);
0071     return -ENOBUFS;
0072 }
0073 
0074 static int ieee802154_nl_fill_iface(struct sk_buff *msg, u32 portid,
0075                     u32 seq, int flags, struct net_device *dev)
0076 {
0077     void *hdr;
0078     struct wpan_phy *phy;
0079     struct ieee802154_mlme_ops *ops;
0080     __le16 short_addr, pan_id;
0081 
0082     pr_debug("%s\n", __func__);
0083 
0084     hdr = genlmsg_put(msg, 0, seq, &nl802154_family, flags,
0085               IEEE802154_LIST_IFACE);
0086     if (!hdr)
0087         goto out;
0088 
0089     ops = ieee802154_mlme_ops(dev);
0090     phy = dev->ieee802154_ptr->wpan_phy;
0091     BUG_ON(!phy);
0092     get_device(&phy->dev);
0093 
0094     rtnl_lock();
0095     short_addr = dev->ieee802154_ptr->short_addr;
0096     pan_id = dev->ieee802154_ptr->pan_id;
0097     rtnl_unlock();
0098 
0099     if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
0100         nla_put_string(msg, IEEE802154_ATTR_PHY_NAME, wpan_phy_name(phy)) ||
0101         nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
0102         nla_put(msg, IEEE802154_ATTR_HW_ADDR, IEEE802154_ADDR_LEN,
0103             dev->dev_addr) ||
0104         nla_put_shortaddr(msg, IEEE802154_ATTR_SHORT_ADDR, short_addr) ||
0105         nla_put_shortaddr(msg, IEEE802154_ATTR_PAN_ID, pan_id))
0106         goto nla_put_failure;
0107 
0108     if (ops->get_mac_params) {
0109         struct ieee802154_mac_params params;
0110 
0111         rtnl_lock();
0112         ops->get_mac_params(dev, &params);
0113         rtnl_unlock();
0114 
0115         if (nla_put_s8(msg, IEEE802154_ATTR_TXPOWER,
0116                    params.transmit_power / 100) ||
0117             nla_put_u8(msg, IEEE802154_ATTR_LBT_ENABLED, params.lbt) ||
0118             nla_put_u8(msg, IEEE802154_ATTR_CCA_MODE,
0119                    params.cca.mode) ||
0120             nla_put_s32(msg, IEEE802154_ATTR_CCA_ED_LEVEL,
0121                 params.cca_ed_level / 100) ||
0122             nla_put_u8(msg, IEEE802154_ATTR_CSMA_RETRIES,
0123                    params.csma_retries) ||
0124             nla_put_u8(msg, IEEE802154_ATTR_CSMA_MIN_BE,
0125                    params.min_be) ||
0126             nla_put_u8(msg, IEEE802154_ATTR_CSMA_MAX_BE,
0127                    params.max_be) ||
0128             nla_put_s8(msg, IEEE802154_ATTR_FRAME_RETRIES,
0129                    params.frame_retries))
0130             goto nla_put_failure;
0131     }
0132 
0133     wpan_phy_put(phy);
0134     genlmsg_end(msg, hdr);
0135     return 0;
0136 
0137 nla_put_failure:
0138     wpan_phy_put(phy);
0139     genlmsg_cancel(msg, hdr);
0140 out:
0141     return -EMSGSIZE;
0142 }
0143 
0144 /* Requests from userspace */
0145 static struct net_device *ieee802154_nl_get_dev(struct genl_info *info)
0146 {
0147     struct net_device *dev;
0148 
0149     if (info->attrs[IEEE802154_ATTR_DEV_NAME]) {
0150         char name[IFNAMSIZ + 1];
0151 
0152         nla_strscpy(name, info->attrs[IEEE802154_ATTR_DEV_NAME],
0153                 sizeof(name));
0154         dev = dev_get_by_name(&init_net, name);
0155     } else if (info->attrs[IEEE802154_ATTR_DEV_INDEX]) {
0156         dev = dev_get_by_index(&init_net,
0157             nla_get_u32(info->attrs[IEEE802154_ATTR_DEV_INDEX]));
0158     } else {
0159         return NULL;
0160     }
0161 
0162     if (!dev)
0163         return NULL;
0164 
0165     if (dev->type != ARPHRD_IEEE802154) {
0166         dev_put(dev);
0167         return NULL;
0168     }
0169 
0170     return dev;
0171 }
0172 
0173 int ieee802154_associate_req(struct sk_buff *skb, struct genl_info *info)
0174 {
0175     struct net_device *dev;
0176     struct ieee802154_addr addr;
0177     u8 page;
0178     int ret = -EOPNOTSUPP;
0179 
0180     if (!info->attrs[IEEE802154_ATTR_CHANNEL] ||
0181         !info->attrs[IEEE802154_ATTR_COORD_PAN_ID] ||
0182         (!info->attrs[IEEE802154_ATTR_COORD_HW_ADDR] &&
0183         !info->attrs[IEEE802154_ATTR_COORD_SHORT_ADDR]) ||
0184         !info->attrs[IEEE802154_ATTR_CAPABILITY])
0185         return -EINVAL;
0186 
0187     dev = ieee802154_nl_get_dev(info);
0188     if (!dev)
0189         return -ENODEV;
0190     if (!ieee802154_mlme_ops(dev)->assoc_req)
0191         goto out;
0192 
0193     if (info->attrs[IEEE802154_ATTR_COORD_HW_ADDR]) {
0194         addr.mode = IEEE802154_ADDR_LONG;
0195         addr.extended_addr = nla_get_hwaddr(
0196                 info->attrs[IEEE802154_ATTR_COORD_HW_ADDR]);
0197     } else {
0198         addr.mode = IEEE802154_ADDR_SHORT;
0199         addr.short_addr = nla_get_shortaddr(
0200                 info->attrs[IEEE802154_ATTR_COORD_SHORT_ADDR]);
0201     }
0202     addr.pan_id = nla_get_shortaddr(
0203             info->attrs[IEEE802154_ATTR_COORD_PAN_ID]);
0204 
0205     if (info->attrs[IEEE802154_ATTR_PAGE])
0206         page = nla_get_u8(info->attrs[IEEE802154_ATTR_PAGE]);
0207     else
0208         page = 0;
0209 
0210     ret = ieee802154_mlme_ops(dev)->assoc_req(dev, &addr,
0211             nla_get_u8(info->attrs[IEEE802154_ATTR_CHANNEL]),
0212             page,
0213             nla_get_u8(info->attrs[IEEE802154_ATTR_CAPABILITY]));
0214 
0215 out:
0216     dev_put(dev);
0217     return ret;
0218 }
0219 
0220 int ieee802154_associate_resp(struct sk_buff *skb, struct genl_info *info)
0221 {
0222     struct net_device *dev;
0223     struct ieee802154_addr addr;
0224     int ret = -EOPNOTSUPP;
0225 
0226     if (!info->attrs[IEEE802154_ATTR_STATUS] ||
0227         !info->attrs[IEEE802154_ATTR_DEST_HW_ADDR] ||
0228         !info->attrs[IEEE802154_ATTR_DEST_SHORT_ADDR])
0229         return -EINVAL;
0230 
0231     dev = ieee802154_nl_get_dev(info);
0232     if (!dev)
0233         return -ENODEV;
0234     if (!ieee802154_mlme_ops(dev)->assoc_resp)
0235         goto out;
0236 
0237     addr.mode = IEEE802154_ADDR_LONG;
0238     addr.extended_addr = nla_get_hwaddr(
0239             info->attrs[IEEE802154_ATTR_DEST_HW_ADDR]);
0240     rtnl_lock();
0241     addr.pan_id = dev->ieee802154_ptr->pan_id;
0242     rtnl_unlock();
0243 
0244     ret = ieee802154_mlme_ops(dev)->assoc_resp(dev, &addr,
0245         nla_get_shortaddr(info->attrs[IEEE802154_ATTR_DEST_SHORT_ADDR]),
0246         nla_get_u8(info->attrs[IEEE802154_ATTR_STATUS]));
0247 
0248 out:
0249     dev_put(dev);
0250     return ret;
0251 }
0252 
0253 int ieee802154_disassociate_req(struct sk_buff *skb, struct genl_info *info)
0254 {
0255     struct net_device *dev;
0256     struct ieee802154_addr addr;
0257     int ret = -EOPNOTSUPP;
0258 
0259     if ((!info->attrs[IEEE802154_ATTR_DEST_HW_ADDR] &&
0260         !info->attrs[IEEE802154_ATTR_DEST_SHORT_ADDR]) ||
0261         !info->attrs[IEEE802154_ATTR_REASON])
0262         return -EINVAL;
0263 
0264     dev = ieee802154_nl_get_dev(info);
0265     if (!dev)
0266         return -ENODEV;
0267     if (!ieee802154_mlme_ops(dev)->disassoc_req)
0268         goto out;
0269 
0270     if (info->attrs[IEEE802154_ATTR_DEST_HW_ADDR]) {
0271         addr.mode = IEEE802154_ADDR_LONG;
0272         addr.extended_addr = nla_get_hwaddr(
0273                 info->attrs[IEEE802154_ATTR_DEST_HW_ADDR]);
0274     } else {
0275         addr.mode = IEEE802154_ADDR_SHORT;
0276         addr.short_addr = nla_get_shortaddr(
0277                 info->attrs[IEEE802154_ATTR_DEST_SHORT_ADDR]);
0278     }
0279     rtnl_lock();
0280     addr.pan_id = dev->ieee802154_ptr->pan_id;
0281     rtnl_unlock();
0282 
0283     ret = ieee802154_mlme_ops(dev)->disassoc_req(dev, &addr,
0284             nla_get_u8(info->attrs[IEEE802154_ATTR_REASON]));
0285 
0286 out:
0287     dev_put(dev);
0288     return ret;
0289 }
0290 
0291 /* PANid, channel, beacon_order = 15, superframe_order = 15,
0292  * PAN_coordinator, battery_life_extension = 0,
0293  * coord_realignment = 0, security_enable = 0
0294 */
0295 int ieee802154_start_req(struct sk_buff *skb, struct genl_info *info)
0296 {
0297     struct net_device *dev;
0298     struct ieee802154_addr addr;
0299 
0300     u8 channel, bcn_ord, sf_ord;
0301     u8 page;
0302     int pan_coord, blx, coord_realign;
0303     int ret = -EBUSY;
0304 
0305     if (!info->attrs[IEEE802154_ATTR_COORD_PAN_ID] ||
0306         !info->attrs[IEEE802154_ATTR_COORD_SHORT_ADDR] ||
0307         !info->attrs[IEEE802154_ATTR_CHANNEL] ||
0308         !info->attrs[IEEE802154_ATTR_BCN_ORD] ||
0309         !info->attrs[IEEE802154_ATTR_SF_ORD] ||
0310         !info->attrs[IEEE802154_ATTR_PAN_COORD] ||
0311         !info->attrs[IEEE802154_ATTR_BAT_EXT] ||
0312         !info->attrs[IEEE802154_ATTR_COORD_REALIGN]
0313      )
0314         return -EINVAL;
0315 
0316     dev = ieee802154_nl_get_dev(info);
0317     if (!dev)
0318         return -ENODEV;
0319 
0320     if (netif_running(dev))
0321         goto out;
0322 
0323     if (!ieee802154_mlme_ops(dev)->start_req) {
0324         ret = -EOPNOTSUPP;
0325         goto out;
0326     }
0327 
0328     addr.mode = IEEE802154_ADDR_SHORT;
0329     addr.short_addr = nla_get_shortaddr(
0330             info->attrs[IEEE802154_ATTR_COORD_SHORT_ADDR]);
0331     addr.pan_id = nla_get_shortaddr(
0332             info->attrs[IEEE802154_ATTR_COORD_PAN_ID]);
0333 
0334     channel = nla_get_u8(info->attrs[IEEE802154_ATTR_CHANNEL]);
0335     bcn_ord = nla_get_u8(info->attrs[IEEE802154_ATTR_BCN_ORD]);
0336     sf_ord = nla_get_u8(info->attrs[IEEE802154_ATTR_SF_ORD]);
0337     pan_coord = nla_get_u8(info->attrs[IEEE802154_ATTR_PAN_COORD]);
0338     blx = nla_get_u8(info->attrs[IEEE802154_ATTR_BAT_EXT]);
0339     coord_realign = nla_get_u8(info->attrs[IEEE802154_ATTR_COORD_REALIGN]);
0340 
0341     if (info->attrs[IEEE802154_ATTR_PAGE])
0342         page = nla_get_u8(info->attrs[IEEE802154_ATTR_PAGE]);
0343     else
0344         page = 0;
0345 
0346     if (addr.short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST)) {
0347         ieee802154_nl_start_confirm(dev, IEEE802154_NO_SHORT_ADDRESS);
0348         dev_put(dev);
0349         return -EINVAL;
0350     }
0351 
0352     rtnl_lock();
0353     ret = ieee802154_mlme_ops(dev)->start_req(dev, &addr, channel, page,
0354         bcn_ord, sf_ord, pan_coord, blx, coord_realign);
0355     rtnl_unlock();
0356 
0357     /* FIXME: add validation for unused parameters to be sane
0358      * for SoftMAC
0359      */
0360     ieee802154_nl_start_confirm(dev, IEEE802154_SUCCESS);
0361 
0362 out:
0363     dev_put(dev);
0364     return ret;
0365 }
0366 
0367 int ieee802154_scan_req(struct sk_buff *skb, struct genl_info *info)
0368 {
0369     struct net_device *dev;
0370     int ret = -EOPNOTSUPP;
0371     u8 type;
0372     u32 channels;
0373     u8 duration;
0374     u8 page;
0375 
0376     if (!info->attrs[IEEE802154_ATTR_SCAN_TYPE] ||
0377         !info->attrs[IEEE802154_ATTR_CHANNELS] ||
0378         !info->attrs[IEEE802154_ATTR_DURATION])
0379         return -EINVAL;
0380 
0381     dev = ieee802154_nl_get_dev(info);
0382     if (!dev)
0383         return -ENODEV;
0384     if (!ieee802154_mlme_ops(dev)->scan_req)
0385         goto out;
0386 
0387     type = nla_get_u8(info->attrs[IEEE802154_ATTR_SCAN_TYPE]);
0388     channels = nla_get_u32(info->attrs[IEEE802154_ATTR_CHANNELS]);
0389     duration = nla_get_u8(info->attrs[IEEE802154_ATTR_DURATION]);
0390 
0391     if (info->attrs[IEEE802154_ATTR_PAGE])
0392         page = nla_get_u8(info->attrs[IEEE802154_ATTR_PAGE]);
0393     else
0394         page = 0;
0395 
0396     ret = ieee802154_mlme_ops(dev)->scan_req(dev, type, channels,
0397                          page, duration);
0398 
0399 out:
0400     dev_put(dev);
0401     return ret;
0402 }
0403 
0404 int ieee802154_list_iface(struct sk_buff *skb, struct genl_info *info)
0405 {
0406     /* Request for interface name, index, type, IEEE address,
0407      * PAN Id, short address
0408      */
0409     struct sk_buff *msg;
0410     struct net_device *dev = NULL;
0411     int rc = -ENOBUFS;
0412 
0413     pr_debug("%s\n", __func__);
0414 
0415     dev = ieee802154_nl_get_dev(info);
0416     if (!dev)
0417         return -ENODEV;
0418 
0419     msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
0420     if (!msg)
0421         goto out_dev;
0422 
0423     rc = ieee802154_nl_fill_iface(msg, info->snd_portid, info->snd_seq,
0424                       0, dev);
0425     if (rc < 0)
0426         goto out_free;
0427 
0428     dev_put(dev);
0429 
0430     return genlmsg_reply(msg, info);
0431 out_free:
0432     nlmsg_free(msg);
0433 out_dev:
0434     dev_put(dev);
0435     return rc;
0436 }
0437 
0438 int ieee802154_dump_iface(struct sk_buff *skb, struct netlink_callback *cb)
0439 {
0440     struct net *net = sock_net(skb->sk);
0441     struct net_device *dev;
0442     int idx;
0443     int s_idx = cb->args[0];
0444 
0445     pr_debug("%s\n", __func__);
0446 
0447     idx = 0;
0448     for_each_netdev(net, dev) {
0449         if (idx < s_idx || dev->type != ARPHRD_IEEE802154)
0450             goto cont;
0451 
0452         if (ieee802154_nl_fill_iface(skb, NETLINK_CB(cb->skb).portid,
0453                          cb->nlh->nlmsg_seq,
0454                          NLM_F_MULTI, dev) < 0)
0455             break;
0456 cont:
0457         idx++;
0458     }
0459     cb->args[0] = idx;
0460 
0461     return skb->len;
0462 }
0463 
0464 int ieee802154_set_macparams(struct sk_buff *skb, struct genl_info *info)
0465 {
0466     struct net_device *dev = NULL;
0467     struct ieee802154_mlme_ops *ops;
0468     struct ieee802154_mac_params params;
0469     struct wpan_phy *phy;
0470     int rc = -EINVAL;
0471 
0472     pr_debug("%s\n", __func__);
0473 
0474     dev = ieee802154_nl_get_dev(info);
0475     if (!dev)
0476         return -ENODEV;
0477 
0478     ops = ieee802154_mlme_ops(dev);
0479 
0480     if (!ops->get_mac_params || !ops->set_mac_params) {
0481         rc = -EOPNOTSUPP;
0482         goto out;
0483     }
0484 
0485     if (netif_running(dev)) {
0486         rc = -EBUSY;
0487         goto out;
0488     }
0489 
0490     if (!info->attrs[IEEE802154_ATTR_LBT_ENABLED] &&
0491         !info->attrs[IEEE802154_ATTR_CCA_MODE] &&
0492         !info->attrs[IEEE802154_ATTR_CCA_ED_LEVEL] &&
0493         !info->attrs[IEEE802154_ATTR_CSMA_RETRIES] &&
0494         !info->attrs[IEEE802154_ATTR_CSMA_MIN_BE] &&
0495         !info->attrs[IEEE802154_ATTR_CSMA_MAX_BE] &&
0496         !info->attrs[IEEE802154_ATTR_FRAME_RETRIES])
0497         goto out;
0498 
0499     phy = dev->ieee802154_ptr->wpan_phy;
0500     get_device(&phy->dev);
0501 
0502     rtnl_lock();
0503     ops->get_mac_params(dev, &params);
0504 
0505     if (info->attrs[IEEE802154_ATTR_TXPOWER])
0506         params.transmit_power = nla_get_s8(info->attrs[IEEE802154_ATTR_TXPOWER]) * 100;
0507 
0508     if (info->attrs[IEEE802154_ATTR_LBT_ENABLED])
0509         params.lbt = nla_get_u8(info->attrs[IEEE802154_ATTR_LBT_ENABLED]);
0510 
0511     if (info->attrs[IEEE802154_ATTR_CCA_MODE])
0512         params.cca.mode = nla_get_u8(info->attrs[IEEE802154_ATTR_CCA_MODE]);
0513 
0514     if (info->attrs[IEEE802154_ATTR_CCA_ED_LEVEL])
0515         params.cca_ed_level = nla_get_s32(info->attrs[IEEE802154_ATTR_CCA_ED_LEVEL]) * 100;
0516 
0517     if (info->attrs[IEEE802154_ATTR_CSMA_RETRIES])
0518         params.csma_retries = nla_get_u8(info->attrs[IEEE802154_ATTR_CSMA_RETRIES]);
0519 
0520     if (info->attrs[IEEE802154_ATTR_CSMA_MIN_BE])
0521         params.min_be = nla_get_u8(info->attrs[IEEE802154_ATTR_CSMA_MIN_BE]);
0522 
0523     if (info->attrs[IEEE802154_ATTR_CSMA_MAX_BE])
0524         params.max_be = nla_get_u8(info->attrs[IEEE802154_ATTR_CSMA_MAX_BE]);
0525 
0526     if (info->attrs[IEEE802154_ATTR_FRAME_RETRIES])
0527         params.frame_retries = nla_get_s8(info->attrs[IEEE802154_ATTR_FRAME_RETRIES]);
0528 
0529     rc = ops->set_mac_params(dev, &params);
0530     rtnl_unlock();
0531 
0532     wpan_phy_put(phy);
0533     dev_put(dev);
0534 
0535     return 0;
0536 
0537 out:
0538     dev_put(dev);
0539     return rc;
0540 }
0541 
0542 static int
0543 ieee802154_llsec_parse_key_id(struct genl_info *info,
0544                   struct ieee802154_llsec_key_id *desc)
0545 {
0546     memset(desc, 0, sizeof(*desc));
0547 
0548     if (!info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE])
0549         return -EINVAL;
0550 
0551     desc->mode = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE]);
0552 
0553     if (desc->mode == IEEE802154_SCF_KEY_IMPLICIT) {
0554         if (!info->attrs[IEEE802154_ATTR_PAN_ID])
0555             return -EINVAL;
0556 
0557         desc->device_addr.pan_id = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_PAN_ID]);
0558 
0559         if (info->attrs[IEEE802154_ATTR_SHORT_ADDR]) {
0560             desc->device_addr.mode = IEEE802154_ADDR_SHORT;
0561             desc->device_addr.short_addr = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_SHORT_ADDR]);
0562         } else {
0563             if (!info->attrs[IEEE802154_ATTR_HW_ADDR])
0564                 return -EINVAL;
0565 
0566             desc->device_addr.mode = IEEE802154_ADDR_LONG;
0567             desc->device_addr.extended_addr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
0568         }
0569     }
0570 
0571     if (desc->mode != IEEE802154_SCF_KEY_IMPLICIT &&
0572         !info->attrs[IEEE802154_ATTR_LLSEC_KEY_ID])
0573         return -EINVAL;
0574 
0575     if (desc->mode == IEEE802154_SCF_KEY_SHORT_INDEX &&
0576         !info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_SHORT])
0577         return -EINVAL;
0578 
0579     if (desc->mode == IEEE802154_SCF_KEY_HW_INDEX &&
0580         !info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_EXTENDED])
0581         return -EINVAL;
0582 
0583     if (desc->mode != IEEE802154_SCF_KEY_IMPLICIT)
0584         desc->id = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_KEY_ID]);
0585 
0586     switch (desc->mode) {
0587     case IEEE802154_SCF_KEY_SHORT_INDEX:
0588     {
0589         u32 source = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_SHORT]);
0590 
0591         desc->short_source = cpu_to_le32(source);
0592         break;
0593     }
0594     case IEEE802154_SCF_KEY_HW_INDEX:
0595         desc->extended_source = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_LLSEC_KEY_SOURCE_EXTENDED]);
0596         break;
0597     }
0598 
0599     return 0;
0600 }
0601 
0602 static int
0603 ieee802154_llsec_fill_key_id(struct sk_buff *msg,
0604                  const struct ieee802154_llsec_key_id *desc)
0605 {
0606     if (nla_put_u8(msg, IEEE802154_ATTR_LLSEC_KEY_MODE, desc->mode))
0607         return -EMSGSIZE;
0608 
0609     if (desc->mode == IEEE802154_SCF_KEY_IMPLICIT) {
0610         if (nla_put_shortaddr(msg, IEEE802154_ATTR_PAN_ID,
0611                       desc->device_addr.pan_id))
0612             return -EMSGSIZE;
0613 
0614         if (desc->device_addr.mode == IEEE802154_ADDR_SHORT &&
0615             nla_put_shortaddr(msg, IEEE802154_ATTR_SHORT_ADDR,
0616                       desc->device_addr.short_addr))
0617             return -EMSGSIZE;
0618 
0619         if (desc->device_addr.mode == IEEE802154_ADDR_LONG &&
0620             nla_put_hwaddr(msg, IEEE802154_ATTR_HW_ADDR,
0621                    desc->device_addr.extended_addr,
0622                    IEEE802154_ATTR_PAD))
0623             return -EMSGSIZE;
0624     }
0625 
0626     if (desc->mode != IEEE802154_SCF_KEY_IMPLICIT &&
0627         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_KEY_ID, desc->id))
0628         return -EMSGSIZE;
0629 
0630     if (desc->mode == IEEE802154_SCF_KEY_SHORT_INDEX &&
0631         nla_put_u32(msg, IEEE802154_ATTR_LLSEC_KEY_SOURCE_SHORT,
0632             le32_to_cpu(desc->short_source)))
0633         return -EMSGSIZE;
0634 
0635     if (desc->mode == IEEE802154_SCF_KEY_HW_INDEX &&
0636         nla_put_hwaddr(msg, IEEE802154_ATTR_LLSEC_KEY_SOURCE_EXTENDED,
0637                desc->extended_source, IEEE802154_ATTR_PAD))
0638         return -EMSGSIZE;
0639 
0640     return 0;
0641 }
0642 
0643 int ieee802154_llsec_getparams(struct sk_buff *skb, struct genl_info *info)
0644 {
0645     struct sk_buff *msg;
0646     struct net_device *dev = NULL;
0647     int rc = -ENOBUFS;
0648     struct ieee802154_mlme_ops *ops;
0649     void *hdr;
0650     struct ieee802154_llsec_params params;
0651 
0652     pr_debug("%s\n", __func__);
0653 
0654     dev = ieee802154_nl_get_dev(info);
0655     if (!dev)
0656         return -ENODEV;
0657 
0658     ops = ieee802154_mlme_ops(dev);
0659     if (!ops->llsec) {
0660         rc = -EOPNOTSUPP;
0661         goto out_dev;
0662     }
0663 
0664     msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
0665     if (!msg)
0666         goto out_dev;
0667 
0668     hdr = genlmsg_put(msg, 0, info->snd_seq, &nl802154_family, 0,
0669               IEEE802154_LLSEC_GETPARAMS);
0670     if (!hdr)
0671         goto out_free;
0672 
0673     rc = ops->llsec->get_params(dev, &params);
0674     if (rc < 0)
0675         goto out_free;
0676 
0677     if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
0678         nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
0679         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_ENABLED, params.enabled) ||
0680         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_SECLEVEL, params.out_level) ||
0681         nla_put_u32(msg, IEEE802154_ATTR_LLSEC_FRAME_COUNTER,
0682             be32_to_cpu(params.frame_counter)) ||
0683         ieee802154_llsec_fill_key_id(msg, &params.out_key)) {
0684         rc = -ENOBUFS;
0685         goto out_free;
0686     }
0687 
0688     dev_put(dev);
0689 
0690     return ieee802154_nl_reply(msg, info);
0691 out_free:
0692     nlmsg_free(msg);
0693 out_dev:
0694     dev_put(dev);
0695     return rc;
0696 }
0697 
0698 int ieee802154_llsec_setparams(struct sk_buff *skb, struct genl_info *info)
0699 {
0700     struct net_device *dev = NULL;
0701     int rc = -EINVAL;
0702     struct ieee802154_mlme_ops *ops;
0703     struct ieee802154_llsec_params params;
0704     int changed = 0;
0705 
0706     pr_debug("%s\n", __func__);
0707 
0708     dev = ieee802154_nl_get_dev(info);
0709     if (!dev)
0710         return -ENODEV;
0711 
0712     if (!info->attrs[IEEE802154_ATTR_LLSEC_ENABLED] &&
0713         !info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE] &&
0714         !info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL])
0715         goto out;
0716 
0717     ops = ieee802154_mlme_ops(dev);
0718     if (!ops->llsec) {
0719         rc = -EOPNOTSUPP;
0720         goto out;
0721     }
0722 
0723     if (info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL] &&
0724         nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL]) > 7)
0725         goto out;
0726 
0727     if (info->attrs[IEEE802154_ATTR_LLSEC_ENABLED]) {
0728         params.enabled = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_ENABLED]);
0729         changed |= IEEE802154_LLSEC_PARAM_ENABLED;
0730     }
0731 
0732     if (info->attrs[IEEE802154_ATTR_LLSEC_KEY_MODE]) {
0733         if (ieee802154_llsec_parse_key_id(info, &params.out_key))
0734             goto out;
0735 
0736         changed |= IEEE802154_LLSEC_PARAM_OUT_KEY;
0737     }
0738 
0739     if (info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL]) {
0740         params.out_level = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_SECLEVEL]);
0741         changed |= IEEE802154_LLSEC_PARAM_OUT_LEVEL;
0742     }
0743 
0744     if (info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]) {
0745         u32 fc = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]);
0746 
0747         params.frame_counter = cpu_to_be32(fc);
0748         changed |= IEEE802154_LLSEC_PARAM_FRAME_COUNTER;
0749     }
0750 
0751     rc = ops->llsec->set_params(dev, &params, changed);
0752 
0753     dev_put(dev);
0754 
0755     return rc;
0756 out:
0757     dev_put(dev);
0758     return rc;
0759 }
0760 
0761 struct llsec_dump_data {
0762     struct sk_buff *skb;
0763     int s_idx, s_idx2;
0764     int portid;
0765     int nlmsg_seq;
0766     struct net_device *dev;
0767     struct ieee802154_mlme_ops *ops;
0768     struct ieee802154_llsec_table *table;
0769 };
0770 
0771 static int
0772 ieee802154_llsec_dump_table(struct sk_buff *skb, struct netlink_callback *cb,
0773                 int (*step)(struct llsec_dump_data *))
0774 {
0775     struct net *net = sock_net(skb->sk);
0776     struct net_device *dev;
0777     struct llsec_dump_data data;
0778     int idx = 0;
0779     int first_dev = cb->args[0];
0780     int rc;
0781 
0782     for_each_netdev(net, dev) {
0783         if (idx < first_dev || dev->type != ARPHRD_IEEE802154)
0784             goto skip;
0785 
0786         data.ops = ieee802154_mlme_ops(dev);
0787         if (!data.ops->llsec)
0788             goto skip;
0789 
0790         data.skb = skb;
0791         data.s_idx = cb->args[1];
0792         data.s_idx2 = cb->args[2];
0793         data.dev = dev;
0794         data.portid = NETLINK_CB(cb->skb).portid;
0795         data.nlmsg_seq = cb->nlh->nlmsg_seq;
0796 
0797         data.ops->llsec->lock_table(dev);
0798         data.ops->llsec->get_table(data.dev, &data.table);
0799         rc = step(&data);
0800         data.ops->llsec->unlock_table(dev);
0801 
0802         if (rc < 0)
0803             break;
0804 
0805 skip:
0806         idx++;
0807     }
0808     cb->args[0] = idx;
0809 
0810     return skb->len;
0811 }
0812 
0813 static int
0814 ieee802154_nl_llsec_change(struct sk_buff *skb, struct genl_info *info,
0815                int (*fn)(struct net_device*, struct genl_info*))
0816 {
0817     struct net_device *dev = NULL;
0818     int rc = -EINVAL;
0819 
0820     dev = ieee802154_nl_get_dev(info);
0821     if (!dev)
0822         return -ENODEV;
0823 
0824     if (!ieee802154_mlme_ops(dev)->llsec)
0825         rc = -EOPNOTSUPP;
0826     else
0827         rc = fn(dev, info);
0828 
0829     dev_put(dev);
0830     return rc;
0831 }
0832 
0833 static int
0834 ieee802154_llsec_parse_key(struct genl_info *info,
0835                struct ieee802154_llsec_key *key)
0836 {
0837     u8 frames;
0838     u32 commands[256 / 32];
0839 
0840     memset(key, 0, sizeof(*key));
0841 
0842     if (!info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_FRAME_TYPES] ||
0843         !info->attrs[IEEE802154_ATTR_LLSEC_KEY_BYTES])
0844         return -EINVAL;
0845 
0846     frames = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_FRAME_TYPES]);
0847     if ((frames & BIT(IEEE802154_FC_TYPE_MAC_CMD)) &&
0848         !info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS])
0849         return -EINVAL;
0850 
0851     if (info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS]) {
0852         nla_memcpy(commands,
0853                info->attrs[IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS],
0854                256 / 8);
0855 
0856         if (commands[0] || commands[1] || commands[2] || commands[3] ||
0857             commands[4] || commands[5] || commands[6] ||
0858             commands[7] >= BIT(IEEE802154_CMD_GTS_REQ + 1))
0859             return -EINVAL;
0860 
0861         key->cmd_frame_ids = commands[7];
0862     }
0863 
0864     key->frame_types = frames;
0865 
0866     nla_memcpy(key->key, info->attrs[IEEE802154_ATTR_LLSEC_KEY_BYTES],
0867            IEEE802154_LLSEC_KEY_SIZE);
0868 
0869     return 0;
0870 }
0871 
0872 static int llsec_add_key(struct net_device *dev, struct genl_info *info)
0873 {
0874     struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
0875     struct ieee802154_llsec_key key;
0876     struct ieee802154_llsec_key_id id;
0877 
0878     if (ieee802154_llsec_parse_key(info, &key) ||
0879         ieee802154_llsec_parse_key_id(info, &id))
0880         return -EINVAL;
0881 
0882     return ops->llsec->add_key(dev, &id, &key);
0883 }
0884 
0885 int ieee802154_llsec_add_key(struct sk_buff *skb, struct genl_info *info)
0886 {
0887     if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
0888         (NLM_F_CREATE | NLM_F_EXCL))
0889         return -EINVAL;
0890 
0891     return ieee802154_nl_llsec_change(skb, info, llsec_add_key);
0892 }
0893 
0894 static int llsec_remove_key(struct net_device *dev, struct genl_info *info)
0895 {
0896     struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
0897     struct ieee802154_llsec_key_id id;
0898 
0899     if (ieee802154_llsec_parse_key_id(info, &id))
0900         return -EINVAL;
0901 
0902     return ops->llsec->del_key(dev, &id);
0903 }
0904 
0905 int ieee802154_llsec_del_key(struct sk_buff *skb, struct genl_info *info)
0906 {
0907     return ieee802154_nl_llsec_change(skb, info, llsec_remove_key);
0908 }
0909 
0910 static int
0911 ieee802154_nl_fill_key(struct sk_buff *msg, u32 portid, u32 seq,
0912                const struct ieee802154_llsec_key_entry *key,
0913                const struct net_device *dev)
0914 {
0915     void *hdr;
0916     u32 commands[256 / 32];
0917 
0918     hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
0919               IEEE802154_LLSEC_LIST_KEY);
0920     if (!hdr)
0921         goto out;
0922 
0923     if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
0924         nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
0925         ieee802154_llsec_fill_key_id(msg, &key->id) ||
0926         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_KEY_USAGE_FRAME_TYPES,
0927                key->key->frame_types))
0928         goto nla_put_failure;
0929 
0930     if (key->key->frame_types & BIT(IEEE802154_FC_TYPE_MAC_CMD)) {
0931         memset(commands, 0, sizeof(commands));
0932         commands[7] = key->key->cmd_frame_ids;
0933         if (nla_put(msg, IEEE802154_ATTR_LLSEC_KEY_USAGE_COMMANDS,
0934                 sizeof(commands), commands))
0935             goto nla_put_failure;
0936     }
0937 
0938     if (nla_put(msg, IEEE802154_ATTR_LLSEC_KEY_BYTES,
0939             IEEE802154_LLSEC_KEY_SIZE, key->key->key))
0940         goto nla_put_failure;
0941 
0942     genlmsg_end(msg, hdr);
0943     return 0;
0944 
0945 nla_put_failure:
0946     genlmsg_cancel(msg, hdr);
0947 out:
0948     return -EMSGSIZE;
0949 }
0950 
0951 static int llsec_iter_keys(struct llsec_dump_data *data)
0952 {
0953     struct ieee802154_llsec_key_entry *pos;
0954     int rc = 0, idx = 0;
0955 
0956     list_for_each_entry(pos, &data->table->keys, list) {
0957         if (idx++ < data->s_idx)
0958             continue;
0959 
0960         if (ieee802154_nl_fill_key(data->skb, data->portid,
0961                        data->nlmsg_seq, pos, data->dev)) {
0962             rc = -EMSGSIZE;
0963             break;
0964         }
0965 
0966         data->s_idx++;
0967     }
0968 
0969     return rc;
0970 }
0971 
0972 int ieee802154_llsec_dump_keys(struct sk_buff *skb, struct netlink_callback *cb)
0973 {
0974     return ieee802154_llsec_dump_table(skb, cb, llsec_iter_keys);
0975 }
0976 
0977 static int
0978 llsec_parse_dev(struct genl_info *info,
0979         struct ieee802154_llsec_device *dev)
0980 {
0981     memset(dev, 0, sizeof(*dev));
0982 
0983     if (!info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER] ||
0984         !info->attrs[IEEE802154_ATTR_HW_ADDR] ||
0985         !info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE] ||
0986         !info->attrs[IEEE802154_ATTR_LLSEC_DEV_KEY_MODE] ||
0987         (!!info->attrs[IEEE802154_ATTR_PAN_ID] !=
0988          !!info->attrs[IEEE802154_ATTR_SHORT_ADDR]))
0989         return -EINVAL;
0990 
0991     if (info->attrs[IEEE802154_ATTR_PAN_ID]) {
0992         dev->pan_id = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_PAN_ID]);
0993         dev->short_addr = nla_get_shortaddr(info->attrs[IEEE802154_ATTR_SHORT_ADDR]);
0994     } else {
0995         dev->short_addr = cpu_to_le16(IEEE802154_ADDR_UNDEF);
0996     }
0997 
0998     dev->hwaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
0999     dev->frame_counter = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]);
1000     dev->seclevel_exempt = !!nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE]);
1001     dev->key_mode = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_DEV_KEY_MODE]);
1002 
1003     if (dev->key_mode >= __IEEE802154_LLSEC_DEVKEY_MAX)
1004         return -EINVAL;
1005 
1006     return 0;
1007 }
1008 
1009 static int llsec_add_dev(struct net_device *dev, struct genl_info *info)
1010 {
1011     struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1012     struct ieee802154_llsec_device desc;
1013 
1014     if (llsec_parse_dev(info, &desc))
1015         return -EINVAL;
1016 
1017     return ops->llsec->add_dev(dev, &desc);
1018 }
1019 
1020 int ieee802154_llsec_add_dev(struct sk_buff *skb, struct genl_info *info)
1021 {
1022     if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
1023         (NLM_F_CREATE | NLM_F_EXCL))
1024         return -EINVAL;
1025 
1026     return ieee802154_nl_llsec_change(skb, info, llsec_add_dev);
1027 }
1028 
1029 static int llsec_del_dev(struct net_device *dev, struct genl_info *info)
1030 {
1031     struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1032     __le64 devaddr;
1033 
1034     if (!info->attrs[IEEE802154_ATTR_HW_ADDR])
1035         return -EINVAL;
1036 
1037     devaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1038 
1039     return ops->llsec->del_dev(dev, devaddr);
1040 }
1041 
1042 int ieee802154_llsec_del_dev(struct sk_buff *skb, struct genl_info *info)
1043 {
1044     return ieee802154_nl_llsec_change(skb, info, llsec_del_dev);
1045 }
1046 
1047 static int
1048 ieee802154_nl_fill_dev(struct sk_buff *msg, u32 portid, u32 seq,
1049                const struct ieee802154_llsec_device *desc,
1050                const struct net_device *dev)
1051 {
1052     void *hdr;
1053 
1054     hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
1055               IEEE802154_LLSEC_LIST_DEV);
1056     if (!hdr)
1057         goto out;
1058 
1059     if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
1060         nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
1061         nla_put_shortaddr(msg, IEEE802154_ATTR_PAN_ID, desc->pan_id) ||
1062         nla_put_shortaddr(msg, IEEE802154_ATTR_SHORT_ADDR,
1063                   desc->short_addr) ||
1064         nla_put_hwaddr(msg, IEEE802154_ATTR_HW_ADDR, desc->hwaddr,
1065                IEEE802154_ATTR_PAD) ||
1066         nla_put_u32(msg, IEEE802154_ATTR_LLSEC_FRAME_COUNTER,
1067             desc->frame_counter) ||
1068         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_DEV_OVERRIDE,
1069                desc->seclevel_exempt) ||
1070         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_DEV_KEY_MODE, desc->key_mode))
1071         goto nla_put_failure;
1072 
1073     genlmsg_end(msg, hdr);
1074     return 0;
1075 
1076 nla_put_failure:
1077     genlmsg_cancel(msg, hdr);
1078 out:
1079     return -EMSGSIZE;
1080 }
1081 
1082 static int llsec_iter_devs(struct llsec_dump_data *data)
1083 {
1084     struct ieee802154_llsec_device *pos;
1085     int rc = 0, idx = 0;
1086 
1087     list_for_each_entry(pos, &data->table->devices, list) {
1088         if (idx++ < data->s_idx)
1089             continue;
1090 
1091         if (ieee802154_nl_fill_dev(data->skb, data->portid,
1092                        data->nlmsg_seq, pos, data->dev)) {
1093             rc = -EMSGSIZE;
1094             break;
1095         }
1096 
1097         data->s_idx++;
1098     }
1099 
1100     return rc;
1101 }
1102 
1103 int ieee802154_llsec_dump_devs(struct sk_buff *skb, struct netlink_callback *cb)
1104 {
1105     return ieee802154_llsec_dump_table(skb, cb, llsec_iter_devs);
1106 }
1107 
1108 static int llsec_add_devkey(struct net_device *dev, struct genl_info *info)
1109 {
1110     struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1111     struct ieee802154_llsec_device_key key;
1112     __le64 devaddr;
1113 
1114     if (!info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER] ||
1115         !info->attrs[IEEE802154_ATTR_HW_ADDR] ||
1116         ieee802154_llsec_parse_key_id(info, &key.key_id))
1117         return -EINVAL;
1118 
1119     devaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1120     key.frame_counter = nla_get_u32(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_COUNTER]);
1121 
1122     return ops->llsec->add_devkey(dev, devaddr, &key);
1123 }
1124 
1125 int ieee802154_llsec_add_devkey(struct sk_buff *skb, struct genl_info *info)
1126 {
1127     if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
1128         (NLM_F_CREATE | NLM_F_EXCL))
1129         return -EINVAL;
1130 
1131     return ieee802154_nl_llsec_change(skb, info, llsec_add_devkey);
1132 }
1133 
1134 static int llsec_del_devkey(struct net_device *dev, struct genl_info *info)
1135 {
1136     struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1137     struct ieee802154_llsec_device_key key;
1138     __le64 devaddr;
1139 
1140     if (!info->attrs[IEEE802154_ATTR_HW_ADDR] ||
1141         ieee802154_llsec_parse_key_id(info, &key.key_id))
1142         return -EINVAL;
1143 
1144     devaddr = nla_get_hwaddr(info->attrs[IEEE802154_ATTR_HW_ADDR]);
1145 
1146     return ops->llsec->del_devkey(dev, devaddr, &key);
1147 }
1148 
1149 int ieee802154_llsec_del_devkey(struct sk_buff *skb, struct genl_info *info)
1150 {
1151     return ieee802154_nl_llsec_change(skb, info, llsec_del_devkey);
1152 }
1153 
1154 static int
1155 ieee802154_nl_fill_devkey(struct sk_buff *msg, u32 portid, u32 seq,
1156               __le64 devaddr,
1157               const struct ieee802154_llsec_device_key *devkey,
1158               const struct net_device *dev)
1159 {
1160     void *hdr;
1161 
1162     hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
1163               IEEE802154_LLSEC_LIST_DEVKEY);
1164     if (!hdr)
1165         goto out;
1166 
1167     if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
1168         nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
1169         nla_put_hwaddr(msg, IEEE802154_ATTR_HW_ADDR, devaddr,
1170                IEEE802154_ATTR_PAD) ||
1171         nla_put_u32(msg, IEEE802154_ATTR_LLSEC_FRAME_COUNTER,
1172             devkey->frame_counter) ||
1173         ieee802154_llsec_fill_key_id(msg, &devkey->key_id))
1174         goto nla_put_failure;
1175 
1176     genlmsg_end(msg, hdr);
1177     return 0;
1178 
1179 nla_put_failure:
1180     genlmsg_cancel(msg, hdr);
1181 out:
1182     return -EMSGSIZE;
1183 }
1184 
1185 static int llsec_iter_devkeys(struct llsec_dump_data *data)
1186 {
1187     struct ieee802154_llsec_device *dpos;
1188     struct ieee802154_llsec_device_key *kpos;
1189     int idx = 0, idx2;
1190 
1191     list_for_each_entry(dpos, &data->table->devices, list) {
1192         if (idx++ < data->s_idx)
1193             continue;
1194 
1195         idx2 = 0;
1196 
1197         list_for_each_entry(kpos, &dpos->keys, list) {
1198             if (idx2++ < data->s_idx2)
1199                 continue;
1200 
1201             if (ieee802154_nl_fill_devkey(data->skb, data->portid,
1202                               data->nlmsg_seq,
1203                               dpos->hwaddr, kpos,
1204                               data->dev)) {
1205                 return -EMSGSIZE;
1206             }
1207 
1208             data->s_idx2++;
1209         }
1210 
1211         data->s_idx++;
1212     }
1213 
1214     return 0;
1215 }
1216 
1217 int ieee802154_llsec_dump_devkeys(struct sk_buff *skb,
1218                   struct netlink_callback *cb)
1219 {
1220     return ieee802154_llsec_dump_table(skb, cb, llsec_iter_devkeys);
1221 }
1222 
1223 static int
1224 llsec_parse_seclevel(struct genl_info *info,
1225              struct ieee802154_llsec_seclevel *sl)
1226 {
1227     memset(sl, 0, sizeof(*sl));
1228 
1229     if (!info->attrs[IEEE802154_ATTR_LLSEC_FRAME_TYPE] ||
1230         !info->attrs[IEEE802154_ATTR_LLSEC_SECLEVELS] ||
1231         !info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE])
1232         return -EINVAL;
1233 
1234     sl->frame_type = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_FRAME_TYPE]);
1235     if (sl->frame_type == IEEE802154_FC_TYPE_MAC_CMD) {
1236         if (!info->attrs[IEEE802154_ATTR_LLSEC_CMD_FRAME_ID])
1237             return -EINVAL;
1238 
1239         sl->cmd_frame_id = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_CMD_FRAME_ID]);
1240     }
1241 
1242     sl->sec_levels = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_SECLEVELS]);
1243     sl->device_override = nla_get_u8(info->attrs[IEEE802154_ATTR_LLSEC_DEV_OVERRIDE]);
1244 
1245     return 0;
1246 }
1247 
1248 static int llsec_add_seclevel(struct net_device *dev, struct genl_info *info)
1249 {
1250     struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1251     struct ieee802154_llsec_seclevel sl;
1252 
1253     if (llsec_parse_seclevel(info, &sl))
1254         return -EINVAL;
1255 
1256     return ops->llsec->add_seclevel(dev, &sl);
1257 }
1258 
1259 int ieee802154_llsec_add_seclevel(struct sk_buff *skb, struct genl_info *info)
1260 {
1261     if ((info->nlhdr->nlmsg_flags & (NLM_F_CREATE | NLM_F_EXCL)) !=
1262         (NLM_F_CREATE | NLM_F_EXCL))
1263         return -EINVAL;
1264 
1265     return ieee802154_nl_llsec_change(skb, info, llsec_add_seclevel);
1266 }
1267 
1268 static int llsec_del_seclevel(struct net_device *dev, struct genl_info *info)
1269 {
1270     struct ieee802154_mlme_ops *ops = ieee802154_mlme_ops(dev);
1271     struct ieee802154_llsec_seclevel sl;
1272 
1273     if (llsec_parse_seclevel(info, &sl))
1274         return -EINVAL;
1275 
1276     return ops->llsec->del_seclevel(dev, &sl);
1277 }
1278 
1279 int ieee802154_llsec_del_seclevel(struct sk_buff *skb, struct genl_info *info)
1280 {
1281     return ieee802154_nl_llsec_change(skb, info, llsec_del_seclevel);
1282 }
1283 
1284 static int
1285 ieee802154_nl_fill_seclevel(struct sk_buff *msg, u32 portid, u32 seq,
1286                 const struct ieee802154_llsec_seclevel *sl,
1287                 const struct net_device *dev)
1288 {
1289     void *hdr;
1290 
1291     hdr = genlmsg_put(msg, 0, seq, &nl802154_family, NLM_F_MULTI,
1292               IEEE802154_LLSEC_LIST_SECLEVEL);
1293     if (!hdr)
1294         goto out;
1295 
1296     if (nla_put_string(msg, IEEE802154_ATTR_DEV_NAME, dev->name) ||
1297         nla_put_u32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex) ||
1298         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_FRAME_TYPE, sl->frame_type) ||
1299         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_SECLEVELS, sl->sec_levels) ||
1300         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_DEV_OVERRIDE,
1301                sl->device_override))
1302         goto nla_put_failure;
1303 
1304     if (sl->frame_type == IEEE802154_FC_TYPE_MAC_CMD &&
1305         nla_put_u8(msg, IEEE802154_ATTR_LLSEC_CMD_FRAME_ID,
1306                sl->cmd_frame_id))
1307         goto nla_put_failure;
1308 
1309     genlmsg_end(msg, hdr);
1310     return 0;
1311 
1312 nla_put_failure:
1313     genlmsg_cancel(msg, hdr);
1314 out:
1315     return -EMSGSIZE;
1316 }
1317 
1318 static int llsec_iter_seclevels(struct llsec_dump_data *data)
1319 {
1320     struct ieee802154_llsec_seclevel *pos;
1321     int rc = 0, idx = 0;
1322 
1323     list_for_each_entry(pos, &data->table->security_levels, list) {
1324         if (idx++ < data->s_idx)
1325             continue;
1326 
1327         if (ieee802154_nl_fill_seclevel(data->skb, data->portid,
1328                         data->nlmsg_seq, pos,
1329                         data->dev)) {
1330             rc = -EMSGSIZE;
1331             break;
1332         }
1333 
1334         data->s_idx++;
1335     }
1336 
1337     return rc;
1338 }
1339 
1340 int ieee802154_llsec_dump_seclevels(struct sk_buff *skb,
1341                     struct netlink_callback *cb)
1342 {
1343     return ieee802154_llsec_dump_table(skb, cb, llsec_iter_seclevels);
1344 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!