1 // SPDX-License-Identifier: GPL-2.0-only
2 
3 #include "netlink.h"
4 #include "common.h"
5 #include "bitset.h"
6 
7 #define EEE_MODES_COUNT \
8 	(sizeof_field(struct ethtool_eee, supported) * BITS_PER_BYTE)
9 
10 struct eee_req_info {
11 	struct ethnl_req_info		base;
12 };
13 
14 struct eee_reply_data {
15 	struct ethnl_reply_data		base;
16 	struct ethtool_eee		eee;
17 };
18 
19 #define EEE_REPDATA(__reply_base) \
20 	container_of(__reply_base, struct eee_reply_data, base)
21 
22 const struct nla_policy ethnl_eee_get_policy[] = {
23 	[ETHTOOL_A_EEE_HEADER]		=
24 		NLA_POLICY_NESTED(ethnl_header_policy),
25 };
26 
eee_prepare_data(const struct ethnl_req_info * req_base,struct ethnl_reply_data * reply_base,struct genl_info * info)27 static int eee_prepare_data(const struct ethnl_req_info *req_base,
28 			    struct ethnl_reply_data *reply_base,
29 			    struct genl_info *info)
30 {
31 	struct eee_reply_data *data = EEE_REPDATA(reply_base);
32 	struct net_device *dev = reply_base->dev;
33 	int ret;
34 
35 	if (!dev->ethtool_ops->get_eee)
36 		return -EOPNOTSUPP;
37 	ret = ethnl_ops_begin(dev);
38 	if (ret < 0)
39 		return ret;
40 	ret = dev->ethtool_ops->get_eee(dev, &data->eee);
41 	ethnl_ops_complete(dev);
42 
43 	return ret;
44 }
45 
eee_reply_size(const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)46 static int eee_reply_size(const struct ethnl_req_info *req_base,
47 			  const struct ethnl_reply_data *reply_base)
48 {
49 	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
50 	const struct eee_reply_data *data = EEE_REPDATA(reply_base);
51 	const struct ethtool_eee *eee = &data->eee;
52 	int len = 0;
53 	int ret;
54 
55 	BUILD_BUG_ON(sizeof(eee->advertised) * BITS_PER_BYTE !=
56 		     EEE_MODES_COUNT);
57 	BUILD_BUG_ON(sizeof(eee->lp_advertised) * BITS_PER_BYTE !=
58 		     EEE_MODES_COUNT);
59 
60 	/* MODES_OURS */
61 	ret = ethnl_bitset32_size(&eee->advertised, &eee->supported,
62 				  EEE_MODES_COUNT, link_mode_names, compact);
63 	if (ret < 0)
64 		return ret;
65 	len += ret;
66 	/* MODES_PEERS */
67 	ret = ethnl_bitset32_size(&eee->lp_advertised, NULL,
68 				  EEE_MODES_COUNT, link_mode_names, compact);
69 	if (ret < 0)
70 		return ret;
71 	len += ret;
72 
73 	len += nla_total_size(sizeof(u8)) +	/* _EEE_ACTIVE */
74 	       nla_total_size(sizeof(u8)) +	/* _EEE_ENABLED */
75 	       nla_total_size(sizeof(u8)) +	/* _EEE_TX_LPI_ENABLED */
76 	       nla_total_size(sizeof(u32));	/* _EEE_TX_LPI_TIMER */
77 
78 	return len;
79 }
80 
eee_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)81 static int eee_fill_reply(struct sk_buff *skb,
82 			  const struct ethnl_req_info *req_base,
83 			  const struct ethnl_reply_data *reply_base)
84 {
85 	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
86 	const struct eee_reply_data *data = EEE_REPDATA(reply_base);
87 	const struct ethtool_eee *eee = &data->eee;
88 	int ret;
89 
90 	ret = ethnl_put_bitset32(skb, ETHTOOL_A_EEE_MODES_OURS,
91 				 &eee->advertised, &eee->supported,
92 				 EEE_MODES_COUNT, link_mode_names, compact);
93 	if (ret < 0)
94 		return ret;
95 	ret = ethnl_put_bitset32(skb, ETHTOOL_A_EEE_MODES_PEER,
96 				 &eee->lp_advertised, NULL, EEE_MODES_COUNT,
97 				 link_mode_names, compact);
98 	if (ret < 0)
99 		return ret;
100 
101 	if (nla_put_u8(skb, ETHTOOL_A_EEE_ACTIVE, !!eee->eee_active) ||
102 	    nla_put_u8(skb, ETHTOOL_A_EEE_ENABLED, !!eee->eee_enabled) ||
103 	    nla_put_u8(skb, ETHTOOL_A_EEE_TX_LPI_ENABLED,
104 		       !!eee->tx_lpi_enabled) ||
105 	    nla_put_u32(skb, ETHTOOL_A_EEE_TX_LPI_TIMER, eee->tx_lpi_timer))
106 		return -EMSGSIZE;
107 
108 	return 0;
109 }
110 
111 const struct ethnl_request_ops ethnl_eee_request_ops = {
112 	.request_cmd		= ETHTOOL_MSG_EEE_GET,
113 	.reply_cmd		= ETHTOOL_MSG_EEE_GET_REPLY,
114 	.hdr_attr		= ETHTOOL_A_EEE_HEADER,
115 	.req_info_size		= sizeof(struct eee_req_info),
116 	.reply_data_size	= sizeof(struct eee_reply_data),
117 
118 	.prepare_data		= eee_prepare_data,
119 	.reply_size		= eee_reply_size,
120 	.fill_reply		= eee_fill_reply,
121 };
122 
123 /* EEE_SET */
124 
125 const struct nla_policy ethnl_eee_set_policy[] = {
126 	[ETHTOOL_A_EEE_HEADER]		=
127 		NLA_POLICY_NESTED(ethnl_header_policy),
128 	[ETHTOOL_A_EEE_MODES_OURS]	= { .type = NLA_NESTED },
129 	[ETHTOOL_A_EEE_ENABLED]		= { .type = NLA_U8 },
130 	[ETHTOOL_A_EEE_TX_LPI_ENABLED]	= { .type = NLA_U8 },
131 	[ETHTOOL_A_EEE_TX_LPI_TIMER]	= { .type = NLA_U32 },
132 };
133 
ethnl_set_eee(struct sk_buff * skb,struct genl_info * info)134 int ethnl_set_eee(struct sk_buff *skb, struct genl_info *info)
135 {
136 	struct ethnl_req_info req_info = {};
137 	struct nlattr **tb = info->attrs;
138 	const struct ethtool_ops *ops;
139 	struct ethtool_eee eee = {};
140 	struct net_device *dev;
141 	bool mod = false;
142 	int ret;
143 
144 	ret = ethnl_parse_header_dev_get(&req_info,
145 					 tb[ETHTOOL_A_EEE_HEADER],
146 					 genl_info_net(info), info->extack,
147 					 true);
148 	if (ret < 0)
149 		return ret;
150 	dev = req_info.dev;
151 	ops = dev->ethtool_ops;
152 	ret = -EOPNOTSUPP;
153 	if (!ops->get_eee || !ops->set_eee)
154 		goto out_dev;
155 
156 	rtnl_lock();
157 	ret = ethnl_ops_begin(dev);
158 	if (ret < 0)
159 		goto out_rtnl;
160 	ret = ops->get_eee(dev, &eee);
161 	if (ret < 0)
162 		goto out_ops;
163 
164 	ret = ethnl_update_bitset32(&eee.advertised, EEE_MODES_COUNT,
165 				    tb[ETHTOOL_A_EEE_MODES_OURS],
166 				    link_mode_names, info->extack, &mod);
167 	if (ret < 0)
168 		goto out_ops;
169 	ethnl_update_bool32(&eee.eee_enabled, tb[ETHTOOL_A_EEE_ENABLED], &mod);
170 	ethnl_update_bool32(&eee.tx_lpi_enabled,
171 			    tb[ETHTOOL_A_EEE_TX_LPI_ENABLED], &mod);
172 	ethnl_update_u32(&eee.tx_lpi_timer, tb[ETHTOOL_A_EEE_TX_LPI_TIMER],
173 			 &mod);
174 	ret = 0;
175 	if (!mod)
176 		goto out_ops;
177 
178 	ret = dev->ethtool_ops->set_eee(dev, &eee);
179 	if (ret < 0)
180 		goto out_ops;
181 	ethtool_notify(dev, ETHTOOL_MSG_EEE_NTF, NULL);
182 
183 out_ops:
184 	ethnl_ops_complete(dev);
185 out_rtnl:
186 	rtnl_unlock();
187 out_dev:
188 	dev_put(dev);
189 	return ret;
190 }
191