xref: /openbmc/linux/net/ethtool/fec.c (revision e5bd61e8)
1 // SPDX-License-Identifier: GPL-2.0-only
2 
3 #include "netlink.h"
4 #include "common.h"
5 #include "bitset.h"
6 
7 struct fec_req_info {
8 	struct ethnl_req_info		base;
9 };
10 
11 struct fec_reply_data {
12 	struct ethnl_reply_data		base;
13 	__ETHTOOL_DECLARE_LINK_MODE_MASK(fec_link_modes);
14 	u32 active_fec;
15 	u8 fec_auto;
16 	struct fec_stat_grp {
17 		u64 stats[1 + ETHTOOL_MAX_LANES];
18 		u8 cnt;
19 	} corr, uncorr, corr_bits;
20 };
21 
22 #define FEC_REPDATA(__reply_base) \
23 	container_of(__reply_base, struct fec_reply_data, base)
24 
25 #define ETHTOOL_FEC_MASK	((ETHTOOL_FEC_LLRS << 1) - 1)
26 
27 const struct nla_policy ethnl_fec_get_policy[ETHTOOL_A_FEC_HEADER + 1] = {
28 	[ETHTOOL_A_FEC_HEADER]	= NLA_POLICY_NESTED(ethnl_header_policy_stats),
29 };
30 
31 static void
32 ethtool_fec_to_link_modes(u32 fec, unsigned long *link_modes, u8 *fec_auto)
33 {
34 	if (fec_auto)
35 		*fec_auto = !!(fec & ETHTOOL_FEC_AUTO);
36 
37 	if (fec & ETHTOOL_FEC_OFF)
38 		__set_bit(ETHTOOL_LINK_MODE_FEC_NONE_BIT, link_modes);
39 	if (fec & ETHTOOL_FEC_RS)
40 		__set_bit(ETHTOOL_LINK_MODE_FEC_RS_BIT, link_modes);
41 	if (fec & ETHTOOL_FEC_BASER)
42 		__set_bit(ETHTOOL_LINK_MODE_FEC_BASER_BIT, link_modes);
43 	if (fec & ETHTOOL_FEC_LLRS)
44 		__set_bit(ETHTOOL_LINK_MODE_FEC_LLRS_BIT, link_modes);
45 }
46 
47 static int
48 ethtool_link_modes_to_fecparam(struct ethtool_fecparam *fec,
49 			       unsigned long *link_modes, u8 fec_auto)
50 {
51 	memset(fec, 0, sizeof(*fec));
52 
53 	if (fec_auto)
54 		fec->fec |= ETHTOOL_FEC_AUTO;
55 
56 	if (__test_and_clear_bit(ETHTOOL_LINK_MODE_FEC_NONE_BIT, link_modes))
57 		fec->fec |= ETHTOOL_FEC_OFF;
58 	if (__test_and_clear_bit(ETHTOOL_LINK_MODE_FEC_RS_BIT, link_modes))
59 		fec->fec |= ETHTOOL_FEC_RS;
60 	if (__test_and_clear_bit(ETHTOOL_LINK_MODE_FEC_BASER_BIT, link_modes))
61 		fec->fec |= ETHTOOL_FEC_BASER;
62 	if (__test_and_clear_bit(ETHTOOL_LINK_MODE_FEC_LLRS_BIT, link_modes))
63 		fec->fec |= ETHTOOL_FEC_LLRS;
64 
65 	if (!bitmap_empty(link_modes, __ETHTOOL_LINK_MODE_MASK_NBITS))
66 		return -EINVAL;
67 
68 	return 0;
69 }
70 
71 static void
72 fec_stats_recalc(struct fec_stat_grp *grp, struct ethtool_fec_stat *stats)
73 {
74 	int i;
75 
76 	if (stats->lanes[0] == ETHTOOL_STAT_NOT_SET) {
77 		grp->stats[0] = stats->total;
78 		grp->cnt = stats->total != ETHTOOL_STAT_NOT_SET;
79 		return;
80 	}
81 
82 	grp->cnt = 1;
83 	grp->stats[0] = 0;
84 	for (i = 0; i < ETHTOOL_MAX_LANES; i++) {
85 		if (stats->lanes[i] == ETHTOOL_STAT_NOT_SET)
86 			break;
87 
88 		grp->stats[0] += stats->lanes[i];
89 		grp->stats[grp->cnt++] = stats->lanes[i];
90 	}
91 }
92 
93 static int fec_prepare_data(const struct ethnl_req_info *req_base,
94 			    struct ethnl_reply_data *reply_base,
95 			    struct genl_info *info)
96 {
97 	__ETHTOOL_DECLARE_LINK_MODE_MASK(active_fec_modes) = {};
98 	struct fec_reply_data *data = FEC_REPDATA(reply_base);
99 	struct net_device *dev = reply_base->dev;
100 	struct ethtool_fecparam fec = {};
101 	int ret;
102 
103 	if (!dev->ethtool_ops->get_fecparam)
104 		return -EOPNOTSUPP;
105 	ret = ethnl_ops_begin(dev);
106 	if (ret < 0)
107 		return ret;
108 	ret = dev->ethtool_ops->get_fecparam(dev, &fec);
109 	if (ret)
110 		goto out_complete;
111 	if (req_base->flags & ETHTOOL_FLAG_STATS &&
112 	    dev->ethtool_ops->get_fec_stats) {
113 		struct ethtool_fec_stats stats;
114 
115 		ethtool_stats_init((u64 *)&stats, sizeof(stats) / 8);
116 		dev->ethtool_ops->get_fec_stats(dev, &stats);
117 
118 		fec_stats_recalc(&data->corr, &stats.corrected_blocks);
119 		fec_stats_recalc(&data->uncorr, &stats.uncorrectable_blocks);
120 		fec_stats_recalc(&data->corr_bits, &stats.corrected_bits);
121 	}
122 
123 	WARN_ON_ONCE(fec.reserved);
124 
125 	ethtool_fec_to_link_modes(fec.fec, data->fec_link_modes,
126 				  &data->fec_auto);
127 
128 	ethtool_fec_to_link_modes(fec.active_fec, active_fec_modes, NULL);
129 	data->active_fec = find_first_bit(active_fec_modes,
130 					  __ETHTOOL_LINK_MODE_MASK_NBITS);
131 	/* Don't report attr if no FEC mode set. Note that
132 	 * ethtool_fecparam_to_link_modes() ignores NONE and AUTO.
133 	 */
134 	if (data->active_fec == __ETHTOOL_LINK_MODE_MASK_NBITS)
135 		data->active_fec = 0;
136 
137 out_complete:
138 	ethnl_ops_complete(dev);
139 	return ret;
140 }
141 
142 static int fec_reply_size(const struct ethnl_req_info *req_base,
143 			  const struct ethnl_reply_data *reply_base)
144 {
145 	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
146 	const struct fec_reply_data *data = FEC_REPDATA(reply_base);
147 	int len = 0;
148 	int ret;
149 
150 	ret = ethnl_bitset_size(data->fec_link_modes, NULL,
151 				__ETHTOOL_LINK_MODE_MASK_NBITS,
152 				link_mode_names, compact);
153 	if (ret < 0)
154 		return ret;
155 	len += ret;
156 
157 	len += nla_total_size(sizeof(u8)) +	/* _FEC_AUTO */
158 	       nla_total_size(sizeof(u32));	/* _FEC_ACTIVE */
159 
160 	if (req_base->flags & ETHTOOL_FLAG_STATS)
161 		len += 3 * nla_total_size_64bit(sizeof(u64) *
162 						(1 + ETHTOOL_MAX_LANES));
163 
164 	return len;
165 }
166 
167 static int fec_put_stats(struct sk_buff *skb, const struct fec_reply_data *data)
168 {
169 	struct nlattr *nest;
170 
171 	nest = nla_nest_start(skb, ETHTOOL_A_FEC_STATS);
172 	if (!nest)
173 		return -EMSGSIZE;
174 
175 	if (nla_put_64bit(skb, ETHTOOL_A_FEC_STAT_CORRECTED,
176 			  sizeof(u64) * data->corr.cnt,
177 			  data->corr.stats, ETHTOOL_A_FEC_STAT_PAD) ||
178 	    nla_put_64bit(skb, ETHTOOL_A_FEC_STAT_UNCORR,
179 			  sizeof(u64) * data->uncorr.cnt,
180 			  data->uncorr.stats, ETHTOOL_A_FEC_STAT_PAD) ||
181 	    nla_put_64bit(skb, ETHTOOL_A_FEC_STAT_CORR_BITS,
182 			  sizeof(u64) * data->corr_bits.cnt,
183 			  data->corr_bits.stats, ETHTOOL_A_FEC_STAT_PAD))
184 		goto err_cancel;
185 
186 	nla_nest_end(skb, nest);
187 	return 0;
188 
189 err_cancel:
190 	nla_nest_cancel(skb, nest);
191 	return -EMSGSIZE;
192 }
193 
194 static int fec_fill_reply(struct sk_buff *skb,
195 			  const struct ethnl_req_info *req_base,
196 			  const struct ethnl_reply_data *reply_base)
197 {
198 	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
199 	const struct fec_reply_data *data = FEC_REPDATA(reply_base);
200 	int ret;
201 
202 	ret = ethnl_put_bitset(skb, ETHTOOL_A_FEC_MODES,
203 			       data->fec_link_modes, NULL,
204 			       __ETHTOOL_LINK_MODE_MASK_NBITS,
205 			       link_mode_names, compact);
206 	if (ret < 0)
207 		return ret;
208 
209 	if (nla_put_u8(skb, ETHTOOL_A_FEC_AUTO, data->fec_auto) ||
210 	    (data->active_fec &&
211 	     nla_put_u32(skb, ETHTOOL_A_FEC_ACTIVE, data->active_fec)))
212 		return -EMSGSIZE;
213 
214 	if (req_base->flags & ETHTOOL_FLAG_STATS && fec_put_stats(skb, data))
215 		return -EMSGSIZE;
216 
217 	return 0;
218 }
219 
220 const struct ethnl_request_ops ethnl_fec_request_ops = {
221 	.request_cmd		= ETHTOOL_MSG_FEC_GET,
222 	.reply_cmd		= ETHTOOL_MSG_FEC_GET_REPLY,
223 	.hdr_attr		= ETHTOOL_A_FEC_HEADER,
224 	.req_info_size		= sizeof(struct fec_req_info),
225 	.reply_data_size	= sizeof(struct fec_reply_data),
226 
227 	.prepare_data		= fec_prepare_data,
228 	.reply_size		= fec_reply_size,
229 	.fill_reply		= fec_fill_reply,
230 };
231 
232 /* FEC_SET */
233 
234 const struct nla_policy ethnl_fec_set_policy[ETHTOOL_A_FEC_AUTO + 1] = {
235 	[ETHTOOL_A_FEC_HEADER]	= NLA_POLICY_NESTED(ethnl_header_policy),
236 	[ETHTOOL_A_FEC_MODES]	= { .type = NLA_NESTED },
237 	[ETHTOOL_A_FEC_AUTO]	= NLA_POLICY_MAX(NLA_U8, 1),
238 };
239 
240 int ethnl_set_fec(struct sk_buff *skb, struct genl_info *info)
241 {
242 	__ETHTOOL_DECLARE_LINK_MODE_MASK(fec_link_modes) = {};
243 	struct ethnl_req_info req_info = {};
244 	struct nlattr **tb = info->attrs;
245 	struct ethtool_fecparam fec = {};
246 	const struct ethtool_ops *ops;
247 	struct net_device *dev;
248 	bool mod = false;
249 	u8 fec_auto;
250 	int ret;
251 
252 	ret = ethnl_parse_header_dev_get(&req_info, tb[ETHTOOL_A_FEC_HEADER],
253 					 genl_info_net(info), info->extack,
254 					 true);
255 	if (ret < 0)
256 		return ret;
257 	dev = req_info.dev;
258 	ops = dev->ethtool_ops;
259 	ret = -EOPNOTSUPP;
260 	if (!ops->get_fecparam || !ops->set_fecparam)
261 		goto out_dev;
262 
263 	rtnl_lock();
264 	ret = ethnl_ops_begin(dev);
265 	if (ret < 0)
266 		goto out_rtnl;
267 	ret = ops->get_fecparam(dev, &fec);
268 	if (ret < 0)
269 		goto out_ops;
270 
271 	ethtool_fec_to_link_modes(fec.fec, fec_link_modes, &fec_auto);
272 
273 	ret = ethnl_update_bitset(fec_link_modes,
274 				  __ETHTOOL_LINK_MODE_MASK_NBITS,
275 				  tb[ETHTOOL_A_FEC_MODES],
276 				  link_mode_names, info->extack, &mod);
277 	if (ret < 0)
278 		goto out_ops;
279 	ethnl_update_u8(&fec_auto, tb[ETHTOOL_A_FEC_AUTO], &mod);
280 
281 	ret = 0;
282 	if (!mod)
283 		goto out_ops;
284 
285 	ret = ethtool_link_modes_to_fecparam(&fec, fec_link_modes, fec_auto);
286 	if (ret) {
287 		NL_SET_ERR_MSG_ATTR(info->extack, tb[ETHTOOL_A_FEC_MODES],
288 				    "invalid FEC modes requested");
289 		goto out_ops;
290 	}
291 	if (!fec.fec) {
292 		ret = -EINVAL;
293 		NL_SET_ERR_MSG_ATTR(info->extack, tb[ETHTOOL_A_FEC_MODES],
294 				    "no FEC modes set");
295 		goto out_ops;
296 	}
297 
298 	ret = dev->ethtool_ops->set_fecparam(dev, &fec);
299 	if (ret < 0)
300 		goto out_ops;
301 	ethtool_notify(dev, ETHTOOL_MSG_FEC_NTF, NULL);
302 
303 out_ops:
304 	ethnl_ops_complete(dev);
305 out_rtnl:
306 	rtnl_unlock();
307 out_dev:
308 	dev_put(dev);
309 	return ret;
310 }
311