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(src: link_modes, nbits: __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(stats: (u64 *)&stats, n: sizeof(stats) / 8);
117 ethtool_stats_init(stats: (u64 *)data->fec_stat_hist.values,
118 n: 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(grp: &data->corr, stats: &stats.corrected_blocks);
123 fec_stats_recalc(grp: &data->uncorr, stats: &stats.uncorrectable_blocks);
124 fec_stats_recalc(grp: &data->corr_bits, stats: &stats.corrected_bits);
125 }
126
127 WARN_ON_ONCE(fec.reserved);
128
129 ethtool_fec_to_link_modes(fec: fec.fec, link_modes: data->fec_link_modes,
130 fec_auto: &data->fec_auto);
131
132 ethtool_fec_to_link_modes(fec: fec.active_fec, link_modes: active_fec_modes, NULL);
133 data->active_fec = find_first_bit(addr: active_fec_modes,
134 size: __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(val: data->fec_link_modes, NULL,
155 nbits: __ETHTOOL_LINK_MODE_MASK_NBITS,
156 names: link_mode_names, compact);
157 if (ret < 0)
158 return ret;
159 len += ret;
160
161 len += nla_total_size(payload: sizeof(u8)) + /* _FEC_AUTO */
162 nla_total_size(payload: sizeof(u32)); /* _FEC_ACTIVE */
163
164 if (req_base->flags & ETHTOOL_FLAG_STATS) {
165 len += 3 * nla_total_size_64bit(payload: sizeof(u64) *
166 (1 + ETHTOOL_MAX_LANES));
167 /* add FEC bins information */
168 len += (nla_total_size(payload: 0) + /* _A_FEC_HIST */
169 nla_total_size(payload: 4) + /* _A_FEC_HIST_BIN_LOW */
170 nla_total_size(payload: 4) + /* _A_FEC_HIST_BIN_HI */
171 /* _A_FEC_HIST_BIN_VAL + per-lane values */
172 nla_total_size_64bit(payload: sizeof(u64)) +
173 nla_total_size_64bit(payload: 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, attrtype: ETHTOOL_A_FEC_STAT_HIST);
201 if (!nest)
202 return -EMSGSIZE;
203
204 if (nla_put_u32(skb, attrtype: ETHTOOL_A_FEC_HIST_BIN_LOW,
205 value: ranges[i].low) ||
206 nla_put_u32(skb, attrtype: ETHTOOL_A_FEC_HIST_BIN_HIGH,
207 value: 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, attrtype: ETHTOOL_A_FEC_HIST_BIN_VAL,
216 value: values[i].sum == ETHTOOL_STAT_NOT_SET ?
217 sum : values[i].sum))
218 goto err_cancel_hist;
219 if (j && nla_put_64bit(skb, attrtype: ETHTOOL_A_FEC_HIST_BIN_VAL_PER_LANE,
220 attrlen: sizeof(u64) * j,
221 data: values[i].per_lane,
222 padattr: ETHTOOL_A_FEC_HIST_PAD))
223 goto err_cancel_hist;
224
225 nla_nest_end(skb, start: nest);
226 }
227
228 return 0;
229
230err_cancel_hist:
231 nla_nest_cancel(skb, start: 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, attrtype: ETHTOOL_A_FEC_STATS);
240 if (!nest)
241 return -EMSGSIZE;
242
243 if (nla_put_64bit(skb, attrtype: ETHTOOL_A_FEC_STAT_CORRECTED,
244 attrlen: sizeof(u64) * data->corr.cnt,
245 data: data->corr.stats, padattr: ETHTOOL_A_FEC_STAT_PAD) ||
246 nla_put_64bit(skb, attrtype: ETHTOOL_A_FEC_STAT_UNCORR,
247 attrlen: sizeof(u64) * data->uncorr.cnt,
248 data: data->uncorr.stats, padattr: ETHTOOL_A_FEC_STAT_PAD) ||
249 nla_put_64bit(skb, attrtype: ETHTOOL_A_FEC_STAT_CORR_BITS,
250 attrlen: sizeof(u64) * data->corr_bits.cnt,
251 data: data->corr_bits.stats, padattr: ETHTOOL_A_FEC_STAT_PAD))
252 goto err_cancel;
253
254 if (fec_put_hist(skb, hist: &data->fec_stat_hist))
255 goto err_cancel;
256
257 nla_nest_end(skb, start: nest);
258 return 0;
259
260err_cancel:
261 nla_nest_cancel(skb, start: 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, attrtype: ETHTOOL_A_FEC_MODES,
274 val: data->fec_link_modes, NULL,
275 nbits: __ETHTOOL_LINK_MODE_MASK_NBITS,
276 names: link_mode_names, compact);
277 if (ret < 0)
278 return ret;
279
280 if (nla_put_u8(skb, attrtype: ETHTOOL_A_FEC_AUTO, value: data->fec_auto) ||
281 (data->active_fec &&
282 nla_put_u32(skb, attrtype: ETHTOOL_A_FEC_ACTIVE, value: 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_link_modes, fec_auto: &fec_auto);
323
324 ret = ethnl_update_bitset(bitmap: fec_link_modes,
325 nbits: __ETHTOOL_LINK_MODE_MASK_NBITS,
326 attr: tb[ETHTOOL_A_FEC_MODES],
327 names: link_mode_names, extack: info->extack, mod: &mod);
328 if (ret < 0)
329 return ret;
330 ethnl_update_u8(dst: &fec_auto, attr: tb[ETHTOOL_A_FEC_AUTO], mod: &mod);
331 if (!mod)
332 return 0;
333
334 ret = ethtool_link_modes_to_fecparam(fec: &fec, link_modes: 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};
365

source code of linux/net/ethtool/fec.c