From ab0bd561724bf3c09aa80e76ca0a187c6880bc5c Mon Sep 17 00:00:00 2001
From: Alexander Aring <alex.aring@gmail.com>
Date: Wed, 12 Nov 2014 03:36:55 +0100
Subject: [PATCH] ieee820154: add channel set support

This patch adds page and channel setting support to nl802154 framework.

Signed-off-by: Alexander Aring <alex.aring@gmail.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>
---
 include/net/cfg802154.h   |  1 +
 net/ieee802154/nl802154.c | 28 ++++++++++++++++++++++++++++
 net/ieee802154/rdev-ops.h |  7 +++++++
 net/mac802154/cfg.c       | 24 ++++++++++++++++++++++++
 4 files changed, 60 insertions(+)

diff --git a/include/net/cfg802154.h b/include/net/cfg802154.h
index 8a3edc5edad1..391fdb37208d 100644
--- a/include/net/cfg802154.h
+++ b/include/net/cfg802154.h
@@ -37,6 +37,7 @@ struct cfg802154_ops {
 							   int type);
 	void	(*del_virtual_intf_deprecated)(struct wpan_phy *wpan_phy,
 					       struct net_device *dev);
+	int	(*set_channel)(struct wpan_phy *wpan_phy, u8 page, u8 channel);
 };
 
 struct wpan_phy {
diff --git a/net/ieee802154/nl802154.c b/net/ieee802154/nl802154.c
index 46df7dca92d9..d8ef2c8a182f 100644
--- a/net/ieee802154/nl802154.c
+++ b/net/ieee802154/nl802154.c
@@ -23,6 +23,7 @@
 #include <net/sock.h>
 
 #include "nl802154.h"
+#include "rdev-ops.h"
 #include "core.h"
 
 static int nl802154_pre_doit(const struct genl_ops *ops, struct sk_buff *skb,
@@ -550,6 +551,25 @@ static int nl802154_get_interface(struct sk_buff *skb, struct genl_info *info)
 	return genlmsg_reply(msg, info);
 }
 
+static int nl802154_set_channel(struct sk_buff *skb, struct genl_info *info)
+{
+	struct cfg802154_registered_device *rdev = info->user_ptr[0];
+	u8 channel, page;
+
+	if (!info->attrs[NL802154_ATTR_PAGE] ||
+	    !info->attrs[NL802154_ATTR_CHANNEL])
+		return -EINVAL;
+
+	page = nla_get_u8(info->attrs[NL802154_ATTR_PAGE]);
+	channel = nla_get_u8(info->attrs[NL802154_ATTR_CHANNEL]);
+
+	/* check 802.15.4 constraints */
+	if (page >= WPAN_NUM_PAGES || channel >= WPAN_NUM_CHANNELS)
+		return -EINVAL;
+
+	return rdev_set_channel(rdev, page, channel);
+}
+
 #define NL802154_FLAG_NEED_WPAN_PHY	0x01
 #define NL802154_FLAG_NEED_NETDEV	0x02
 #define NL802154_FLAG_NEED_RTNL		0x04
@@ -660,6 +680,14 @@ static const struct genl_ops nl802154_ops[] = {
 		.internal_flags = NL802154_FLAG_NEED_WPAN_DEV |
 				  NL802154_FLAG_NEED_RTNL,
 	},
+	{
+		.cmd = NL802154_CMD_SET_CHANNEL,
+		.doit = nl802154_set_channel,
+		.policy = nl802154_policy,
+		.flags = GENL_ADMIN_PERM,
+		.internal_flags = NL802154_FLAG_NEED_WPAN_PHY |
+				  NL802154_FLAG_NEED_RTNL,
+	},
 };
 
 /* initialisation/exit functions */
diff --git a/net/ieee802154/rdev-ops.h b/net/ieee802154/rdev-ops.h
index ac8824ec168c..8a3b0eb4e026 100644
--- a/net/ieee802154/rdev-ops.h
+++ b/net/ieee802154/rdev-ops.h
@@ -20,4 +20,11 @@ rdev_del_virtual_intf_deprecated(struct cfg802154_registered_device *rdev,
 	rdev->ops->del_virtual_intf_deprecated(&rdev->wpan_phy, dev);
 }
 
+static inline int
+rdev_set_channel(struct cfg802154_registered_device *rdev, const u8 page,
+		 const u8 channel)
+{
+	return rdev->ops->set_channel(&rdev->wpan_phy, page, channel);
+}
+
 #endif /* __CFG802154_RDEV_OPS */
diff --git a/net/mac802154/cfg.c b/net/mac802154/cfg.c
index d2c4e8f89720..9d5b1895c752 100644
--- a/net/mac802154/cfg.c
+++ b/net/mac802154/cfg.c
@@ -17,6 +17,7 @@
 #include <net/cfg802154.h>
 
 #include "ieee802154_i.h"
+#include "driver-ops.h"
 #include "cfg.h"
 
 static struct net_device *
@@ -41,7 +42,30 @@ static void ieee802154_del_iface_deprecated(struct wpan_phy *wpan_phy,
 	ieee802154_if_remove(sdata);
 }
 
+static int
+ieee802154_set_channel(struct wpan_phy *wpan_phy, const u8 page,
+		       const u8 channel)
+{
+	struct ieee802154_local *local = wpan_phy_priv(wpan_phy);
+	int ret;
+
+	ASSERT_RTNL();
+
+	/* check if phy support this setting */
+	if (!(wpan_phy->channels_supported[page] & BIT(channel)))
+		return -EINVAL;
+
+	ret = drv_set_channel(local, page, channel);
+	if (!ret) {
+		wpan_phy->current_page = page;
+		wpan_phy->current_channel = channel;
+	}
+
+	return ret;
+}
+
 const struct cfg802154_ops mac802154_config_ops = {
 	.add_virtual_intf_deprecated = ieee802154_add_iface_deprecated,
 	.del_virtual_intf_deprecated = ieee802154_del_iface_deprecated,
+	.set_channel = ieee802154_set_channel,
 };
-- 
2.39.2