fec.c

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