1
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
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));
69
70 if (data->power.mode)
71 len += nla_total_size(sizeof(u8));
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
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}
181
