1// SPDX-License-Identifier: GPL-2.0-only
  2
  3#include <linux/ethtool.h>
  4
  5#include "netlink.h"
  6#include "common.h"
  7#include "bitset.h"
  8
  9struct module_req_info {
 10	struct ethnl_req_info base;
 11};
 12
 13struct module_reply_data {
 14	struct ethnl_reply_data	base;
 15	struct ethtool_module_power_mode_params power;
 16};
 17
 18#define MODULE_REPDATA(__reply_base) \
 19	container_of(__reply_base, struct module_reply_data, base)
 20
 21/* MODULE_GET */
 22
 23const struct nla_policy ethnl_module_get_policy[ETHTOOL_A_MODULE_HEADER + 1] = {
 24	[ETHTOOL_A_MODULE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
 25};
 26
 27static int module_get_power_mode(struct net_device *dev,
 28				 struct module_reply_data *data,
 29				 struct netlink_ext_ack *extack)
 30{
 31	const struct ethtool_ops *ops = dev->ethtool_ops;
 32
 33	if (!ops->get_module_power_mode)
 34		return 0;
 35
 36	return ops->get_module_power_mode(dev, &data->power, extack);
 37}
 38
 39static int module_prepare_data(const struct ethnl_req_info *req_base,
 40			       struct ethnl_reply_data *reply_base,
 41			       struct genl_info *info)
 42{
 43	struct module_reply_data *data = MODULE_REPDATA(reply_base);
 44	struct netlink_ext_ack *extack = info ? info->extack : NULL;
 45	struct net_device *dev = reply_base->dev;
 46	int ret;
 47
 48	ret = ethnl_ops_begin(dev);
 49	if (ret < 0)
 50		return ret;
 51
 52	ret = module_get_power_mode(dev, data, extack);
 53	if (ret < 0)
 54		goto out_complete;
 55
 56out_complete:
 57	ethnl_ops_complete(dev);
 58	return ret;
 59}
 60
 61static int module_reply_size(const struct ethnl_req_info *req_base,
 62			     const struct ethnl_reply_data *reply_base)
 63{
 64	struct module_reply_data *data = MODULE_REPDATA(reply_base);
 65	int len = 0;
 66
 67	if (data->power.policy)
 68		len += nla_total_size(sizeof(u8));	/* _MODULE_POWER_MODE_POLICY */
 69
 70	if (data->power.mode)
 71		len += nla_total_size(sizeof(u8));	/* _MODULE_POWER_MODE */
 72
 73	return len;
 74}
 75
 76static int module_fill_reply(struct sk_buff *skb,
 77			     const struct ethnl_req_info *req_base,
 78			     const struct ethnl_reply_data *reply_base)
 79{
 80	const struct module_reply_data *data = MODULE_REPDATA(reply_base);
 81
 82	if (data->power.policy &&
 83	    nla_put_u8(skb, ETHTOOL_A_MODULE_POWER_MODE_POLICY,
 84		       data->power.policy))
 85		return -EMSGSIZE;
 86
 87	if (data->power.mode &&
 88	    nla_put_u8(skb, ETHTOOL_A_MODULE_POWER_MODE, data->power.mode))
 89		return -EMSGSIZE;
 90
 91	return 0;
 92}
 93
 94const struct ethnl_request_ops ethnl_module_request_ops = {
 95	.request_cmd		= ETHTOOL_MSG_MODULE_GET,
 96	.reply_cmd		= ETHTOOL_MSG_MODULE_GET_REPLY,
 97	.hdr_attr		= ETHTOOL_A_MODULE_HEADER,
 98	.req_info_size		= sizeof(struct module_req_info),
 99	.reply_data_size	= sizeof(struct module_reply_data),
100
101	.prepare_data		= module_prepare_data,
102	.reply_size		= module_reply_size,
103	.fill_reply		= module_fill_reply,
104};
105
106/* MODULE_SET */
107
108const struct nla_policy ethnl_module_set_policy[ETHTOOL_A_MODULE_POWER_MODE_POLICY + 1] = {
109	[ETHTOOL_A_MODULE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
110	[ETHTOOL_A_MODULE_POWER_MODE_POLICY] =
111		NLA_POLICY_RANGE(NLA_U8, ETHTOOL_MODULE_POWER_MODE_POLICY_HIGH,
112				 ETHTOOL_MODULE_POWER_MODE_POLICY_AUTO),
113};
114
115static int module_set_power_mode(struct net_device *dev, struct nlattr **tb,
116				 bool *p_mod, struct netlink_ext_ack *extack)
117{
118	struct ethtool_module_power_mode_params power = {};
119	struct ethtool_module_power_mode_params power_new;
120	const struct ethtool_ops *ops = dev->ethtool_ops;
121	int ret;
122
123	if (!tb[ETHTOOL_A_MODULE_POWER_MODE_POLICY])
124		return 0;
125
126	if (!ops->get_module_power_mode || !ops->set_module_power_mode) {
127		NL_SET_ERR_MSG_ATTR(extack,
128				    tb[ETHTOOL_A_MODULE_POWER_MODE_POLICY],
129				    "Setting power mode policy is not supported by this device");
130		return -EOPNOTSUPP;
131	}
132
133	power_new.policy = nla_get_u8(tb[ETHTOOL_A_MODULE_POWER_MODE_POLICY]);
134	ret = ops->get_module_power_mode(dev, &power, extack);
135	if (ret < 0)
136		return ret;
137
138	if (power_new.policy == power.policy)
139		return 0;
140	*p_mod = true;
141
142	return ops->set_module_power_mode(dev, &power_new, extack);
143}
144
145int ethnl_set_module(struct sk_buff *skb, struct genl_info *info)
146{
147	struct ethnl_req_info req_info = {};
148	struct nlattr **tb = info->attrs;
149	struct net_device *dev;
150	bool mod = false;
151	int ret;
152
153	ret = ethnl_parse_header_dev_get(&req_info, tb[ETHTOOL_A_MODULE_HEADER],
154					 genl_info_net(info), info->extack,
155					 true);
156	if (ret < 0)
157		return ret;
158	dev = req_info.dev;
159
160	rtnl_lock();
161	ret = ethnl_ops_begin(dev);
162	if (ret < 0)
163		goto out_rtnl;
164
165	ret = module_set_power_mode(dev, tb, &mod, info->extack);
166	if (ret < 0)
167		goto out_ops;
168
169	if (!mod)
170		goto out_ops;
171
172	ethtool_notify(dev, ETHTOOL_MSG_MODULE_NTF, NULL);
173
174out_ops:
175	ethnl_ops_complete(dev);
176out_rtnl:
177	rtnl_unlock();
178	ethnl_parse_header_dev_put(&req_info);
179	return ret;
180}