1 files changed, 164 insertions, 8 deletions

diff --git a/net/ieee802154/netlink.c b/net/ieee802154/netlink.c
index 27eda9fdf3c2..2106ecbf0308 100644
--- a/net/ieee802154/netlink.c
+++ b/net/ieee802154/netlink.c
@@ -19,6 +19,7 @@
  * Written by:
  * Sergey Lapin <slapin@ossfans.org>
  * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
+ * Maxim Osipov <maxim.osipov@siemens.com>
  */
 
 #include <linux/kernel.h>
@@ -26,10 +27,12 @@
 #include <linux/netdevice.h>
 #include <net/netlink.h>
 #include <net/genetlink.h>
+#include <net/sock.h>
 #include <linux/nl802154.h>
-#include <net/ieee802154/af_ieee802154.h>
-#include <net/ieee802154/nl802154.h>
-#include <net/ieee802154/netdevice.h>
+#include <net/af_ieee802154.h>
+#include <net/nl802154.h>
+#include <net/ieee802154.h>
+#include <net/ieee802154_netdev.h>
 
 static unsigned int ieee802154_seq_num;
 
@@ -73,7 +76,7 @@ static int ieee802154_nl_finish(struct sk_buff *msg)
         /* XXX: nlh is right at the start of msg */
         void *hdr = genlmsg_data(NLMSG_DATA(msg->data));
 
-        if (!genlmsg_end(msg, hdr))
+        if (genlmsg_end(msg, hdr) < 0)
                 goto out;
 
         return genlmsg_multicast(msg, 0, ieee802154_coord_mcgrp.id,
@@ -229,7 +232,7 @@ nla_put_failure:
 EXPORT_SYMBOL(ieee802154_nl_beacon_indic);
 
 int ieee802154_nl_scan_confirm(struct net_device *dev,
-                u8 status, u8 scan_type, u32 unscanned,
+                u8 status, u8 scan_type, u32 unscanned, u8 page,
                 u8 *edl/* , struct list_head *pan_desc_list */)
 {
         struct sk_buff *msg;
@@ -248,6 +251,7 @@ int ieee802154_nl_scan_confirm(struct net_device *dev,
         NLA_PUT_U8(msg, IEEE802154_ATTR_STATUS, status);
         NLA_PUT_U8(msg, IEEE802154_ATTR_SCAN_TYPE, scan_type);
         NLA_PUT_U32(msg, IEEE802154_ATTR_CHANNELS, unscanned);
+        NLA_PUT_U8(msg, IEEE802154_ATTR_PAGE, page);
 
         if (edl)
                 NLA_PUT(msg, IEEE802154_ATTR_ED_LIST, 27, edl);
@@ -260,6 +264,60 @@ nla_put_failure:
 }
 EXPORT_SYMBOL(ieee802154_nl_scan_confirm);
 
+int ieee802154_nl_start_confirm(struct net_device *dev, u8 status)
+{
+        struct sk_buff *msg;
+
+        pr_debug("%s\n", __func__);
+
+        msg = ieee802154_nl_create(0, IEEE802154_START_CONF);
+        if (!msg)
+                return -ENOBUFS;
+
+        NLA_PUT_STRING(msg, IEEE802154_ATTR_DEV_NAME, dev->name);
+        NLA_PUT_U32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex);
+        NLA_PUT(msg, IEEE802154_ATTR_HW_ADDR, IEEE802154_ADDR_LEN,
+                        dev->dev_addr);
+
+        NLA_PUT_U8(msg, IEEE802154_ATTR_STATUS, status);
+
+        return ieee802154_nl_finish(msg);
+
+nla_put_failure:
+        nlmsg_free(msg);
+        return -ENOBUFS;
+}
+EXPORT_SYMBOL(ieee802154_nl_start_confirm);
+
+static int ieee802154_nl_fill_iface(struct sk_buff *msg, u32 pid,
+        u32 seq, int flags, struct net_device *dev)
+{
+        void *hdr;
+
+        pr_debug("%s\n", __func__);
+
+        hdr = genlmsg_put(msg, 0, seq, &ieee802154_coordinator_family, flags,
+                IEEE802154_LIST_IFACE);
+        if (!hdr)
+                goto out;
+
+        NLA_PUT_STRING(msg, IEEE802154_ATTR_DEV_NAME, dev->name);
+        NLA_PUT_U32(msg, IEEE802154_ATTR_DEV_INDEX, dev->ifindex);
+
+        NLA_PUT(msg, IEEE802154_ATTR_HW_ADDR, IEEE802154_ADDR_LEN,
+                dev->dev_addr);
+        NLA_PUT_U16(msg, IEEE802154_ATTR_SHORT_ADDR,
+                ieee802154_mlme_ops(dev)->get_short_addr(dev));
+        NLA_PUT_U16(msg, IEEE802154_ATTR_PAN_ID,
+                ieee802154_mlme_ops(dev)->get_pan_id(dev));
+        return genlmsg_end(msg, hdr);
+
+nla_put_failure:
+        genlmsg_cancel(msg, hdr);
+out:
+        return -EMSGSIZE;
+}
+
 /* Requests from userspace */
 static struct net_device *ieee802154_nl_get_dev(struct genl_info *info)
 {
@@ -272,7 +330,7 @@ static struct net_device *ieee802154_nl_get_dev(struct genl_info *info)
                 dev = dev_get_by_name(&init_net, name);
         } else if (info->attrs[IEEE802154_ATTR_DEV_INDEX])
                 dev = dev_get_by_index(&init_net,
-                                nla_get_u32(info->attrs[IEEE802154_ATTR_DEV_INDEX]));
+                        nla_get_u32(info->attrs[IEEE802154_ATTR_DEV_INDEX]));
         else
                 return NULL;
 
@@ -292,6 +350,7 @@ static int ieee802154_associate_req(struct sk_buff *skb,
 {
         struct net_device *dev;
         struct ieee802154_addr addr;
+        u8 page;
         int ret = -EINVAL;
 
         if (!info->attrs[IEEE802154_ATTR_CHANNEL] ||
@@ -317,8 +376,14 @@ static int ieee802154_associate_req(struct sk_buff *skb,
         }
         addr.pan_id = nla_get_u16(info->attrs[IEEE802154_ATTR_COORD_PAN_ID]);
 
+        if (info->attrs[IEEE802154_ATTR_PAGE])
+                page = nla_get_u8(info->attrs[IEEE802154_ATTR_PAGE]);
+        else
+                page = 0;
+
         ret = ieee802154_mlme_ops(dev)->assoc_req(dev, &addr,
                         nla_get_u8(info->attrs[IEEE802154_ATTR_CHANNEL]),
+                        page,
                         nla_get_u8(info->attrs[IEEE802154_ATTR_CAPABILITY]));
 
         dev_put(dev);
@@ -401,6 +466,7 @@ static int ieee802154_start_req(struct sk_buff *skb, struct genl_info *info)
         struct ieee802154_addr addr;
 
         u8 channel, bcn_ord, sf_ord;
+        u8 page;
         int pan_coord, blx, coord_realign;
         int ret;
 
@@ -431,7 +497,19 @@ static int ieee802154_start_req(struct sk_buff *skb, struct genl_info *info)
         blx = nla_get_u8(info->attrs[IEEE802154_ATTR_BAT_EXT]);
         coord_realign = nla_get_u8(info->attrs[IEEE802154_ATTR_COORD_REALIGN]);
 
-        ret = ieee802154_mlme_ops(dev)->start_req(dev, &addr, channel,
+        if (info->attrs[IEEE802154_ATTR_PAGE])
+                page = nla_get_u8(info->attrs[IEEE802154_ATTR_PAGE]);
+        else
+                page = 0;
+
+
+        if (addr.short_addr == IEEE802154_ADDR_BROADCAST) {
+                ieee802154_nl_start_confirm(dev, IEEE802154_NO_SHORT_ADDRESS);
+                dev_put(dev);
+                return -EINVAL;
+        }
+
+        ret = ieee802154_mlme_ops(dev)->start_req(dev, &addr, channel, page,
                 bcn_ord, sf_ord, pan_coord, blx, coord_realign);
 
         dev_put(dev);
@@ -445,6 +523,7 @@ static int ieee802154_scan_req(struct sk_buff *skb, struct genl_info *info)
         u8 type;
         u32 channels;
         u8 duration;
+        u8 page;
 
         if (!info->attrs[IEEE802154_ATTR_SCAN_TYPE] ||
             !info->attrs[IEEE802154_ATTR_CHANNELS] ||
@@ -459,13 +538,80 @@ static int ieee802154_scan_req(struct sk_buff *skb, struct genl_info *info)
         channels = nla_get_u32(info->attrs[IEEE802154_ATTR_CHANNELS]);
         duration = nla_get_u8(info->attrs[IEEE802154_ATTR_DURATION]);
 
-        ret = ieee802154_mlme_ops(dev)->scan_req(dev, type, channels,
+        if (info->attrs[IEEE802154_ATTR_PAGE])
+                page = nla_get_u8(info->attrs[IEEE802154_ATTR_PAGE]);
+        else
+                page = 0;
+
+
+        ret = ieee802154_mlme_ops(dev)->scan_req(dev, type, channels, page,
                         duration);
 
         dev_put(dev);
         return ret;
 }
 
+static int ieee802154_list_iface(struct sk_buff *skb,
+        struct genl_info *info)
+{
+        /* Request for interface name, index, type, IEEE address,
+           PAN Id, short address */
+        struct sk_buff *msg;
+        struct net_device *dev = NULL;
+        int rc = -ENOBUFS;
+
+        pr_debug("%s\n", __func__);
+
+        dev = ieee802154_nl_get_dev(info);
+        if (!dev)
+                return -ENODEV;
+
+        msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+        if (!msg)
+                goto out_dev;
+
+        rc = ieee802154_nl_fill_iface(msg, info->snd_pid, info->snd_seq,
+                        0, dev);
+        if (rc < 0)
+                goto out_free;
+
+        dev_put(dev);
+
+        return genlmsg_unicast(&init_net, msg, info->snd_pid);
+out_free:
+        nlmsg_free(msg);
+out_dev:
+        dev_put(dev);
+        return rc;
+
+}
+
+static int ieee802154_dump_iface(struct sk_buff *skb,
+        struct netlink_callback *cb)
+{
+        struct net *net = sock_net(skb->sk);
+        struct net_device *dev;
+        int idx;
+        int s_idx = cb->args[0];
+
+        pr_debug("%s\n", __func__);
+
+        idx = 0;
+        for_each_netdev(net, dev) {
+                if (idx < s_idx || (dev->type != ARPHRD_IEEE802154))
+                        goto cont;
+
+                if (ieee802154_nl_fill_iface(skb, NETLINK_CB(cb->skb).pid,
+                        cb->nlh->nlmsg_seq, NLM_F_MULTI, dev) < 0)
+                        break;
+cont:
+                idx++;
+        }
+        cb->args[0] = idx;
+
+        return skb->len;
+}
+
 #define IEEE802154_OP(_cmd, _func)                      \
         {                                               \
                 .cmd    = _cmd,                         \
@@ -475,12 +621,22 @@ static int ieee802154_scan_req(struct sk_buff *skb, struct genl_info *info)
                 .flags  = GENL_ADMIN_PERM,              \
         }
 
+#define IEEE802154_DUMP(_cmd, _func, _dump)             \
+        {                                               \
+                .cmd    = _cmd,                         \
+                .policy = ieee802154_policy,            \
+                .doit   = _func,                        \
+                .dumpit = _dump,                        \
+        }
+
 static struct genl_ops ieee802154_coordinator_ops[] = {
         IEEE802154_OP(IEEE802154_ASSOCIATE_REQ, ieee802154_associate_req),
         IEEE802154_OP(IEEE802154_ASSOCIATE_RESP, ieee802154_associate_resp),
         IEEE802154_OP(IEEE802154_DISASSOCIATE_REQ, ieee802154_disassociate_req),
         IEEE802154_OP(IEEE802154_SCAN_REQ, ieee802154_scan_req),
         IEEE802154_OP(IEEE802154_START_REQ, ieee802154_start_req),
+        IEEE802154_DUMP(IEEE802154_LIST_IFACE, ieee802154_list_iface,
+                                                        ieee802154_dump_iface),
 };
 
 static int __init ieee802154_nl_init(void)