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};
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
27const 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
31static void
32ethtool_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
47static int
48ethtool_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(src: link_modes, nbits: __ETHTOOL_LINK_MODE_MASK_NBITS))
66 return -EINVAL;
67
68 return 0;
69}
70
71static void
72fec_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
93static int fec_prepare_data(const struct ethnl_req_info *req_base,
94 struct ethnl_reply_data *reply_base,
95 const 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(stats: (u64 *)&stats, n: sizeof(stats) / 8);
116 dev->ethtool_ops->get_fec_stats(dev, &stats);
117
118 fec_stats_recalc(grp: &data->corr, stats: &stats.corrected_blocks);
119 fec_stats_recalc(grp: &data->uncorr, stats: &stats.uncorrectable_blocks);
120 fec_stats_recalc(grp: &data->corr_bits, stats: &stats.corrected_bits);
121 }
122
123 WARN_ON_ONCE(fec.reserved);
124
125 ethtool_fec_to_link_modes(fec: fec.fec, link_modes: data->fec_link_modes,
126 fec_auto: &data->fec_auto);
127
128 ethtool_fec_to_link_modes(fec: fec.active_fec, link_modes: active_fec_modes, NULL);
129 data->active_fec = find_first_bit(addr: active_fec_modes,
130 size: __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
137out_complete:
138 ethnl_ops_complete(dev);
139 return ret;
140}
141
142static 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(val: data->fec_link_modes, NULL,
151 nbits: __ETHTOOL_LINK_MODE_MASK_NBITS,
152 names: link_mode_names, compact);
153 if (ret < 0)
154 return ret;
155 len += ret;
156
157 len += nla_total_size(payload: sizeof(u8)) + /* _FEC_AUTO */
158 nla_total_size(payload: sizeof(u32)); /* _FEC_ACTIVE */
159
160 if (req_base->flags & ETHTOOL_FLAG_STATS)
161 len += 3 * nla_total_size_64bit(payload: sizeof(u64) *
162 (1 + ETHTOOL_MAX_LANES));
163
164 return len;
165}
166
167static 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, attrtype: ETHTOOL_A_FEC_STATS);
172 if (!nest)
173 return -EMSGSIZE;
174
175 if (nla_put_64bit(skb, attrtype: ETHTOOL_A_FEC_STAT_CORRECTED,
176 attrlen: sizeof(u64) * data->corr.cnt,
177 data: data->corr.stats, padattr: ETHTOOL_A_FEC_STAT_PAD) ||
178 nla_put_64bit(skb, attrtype: ETHTOOL_A_FEC_STAT_UNCORR,
179 attrlen: sizeof(u64) * data->uncorr.cnt,
180 data: data->uncorr.stats, padattr: ETHTOOL_A_FEC_STAT_PAD) ||
181 nla_put_64bit(skb, attrtype: ETHTOOL_A_FEC_STAT_CORR_BITS,
182 attrlen: sizeof(u64) * data->corr_bits.cnt,
183 data: data->corr_bits.stats, padattr: ETHTOOL_A_FEC_STAT_PAD))
184 goto err_cancel;
185
186 nla_nest_end(skb, start: nest);
187 return 0;
188
189err_cancel:
190 nla_nest_cancel(skb, start: nest);
191 return -EMSGSIZE;
192}
193
194static 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, attrtype: ETHTOOL_A_FEC_MODES,
203 val: data->fec_link_modes, NULL,
204 nbits: __ETHTOOL_LINK_MODE_MASK_NBITS,
205 names: link_mode_names, compact);
206 if (ret < 0)
207 return ret;
208
209 if (nla_put_u8(skb, attrtype: ETHTOOL_A_FEC_AUTO, value: data->fec_auto) ||
210 (data->active_fec &&
211 nla_put_u32(skb, attrtype: ETHTOOL_A_FEC_ACTIVE, value: 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/* FEC_SET */
221
222const struct nla_policy ethnl_fec_set_policy[ETHTOOL_A_FEC_AUTO + 1] = {
223 [ETHTOOL_A_FEC_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
224 [ETHTOOL_A_FEC_MODES] = { .type = NLA_NESTED },
225 [ETHTOOL_A_FEC_AUTO] = NLA_POLICY_MAX(NLA_U8, 1),
226};
227
228static int
229ethnl_set_fec_validate(struct ethnl_req_info *req_info, struct genl_info *info)
230{
231 const struct ethtool_ops *ops = req_info->dev->ethtool_ops;
232
233 return ops->get_fecparam && ops->set_fecparam ? 1 : -EOPNOTSUPP;
234}
235
236static int
237ethnl_set_fec(struct ethnl_req_info *req_info, struct genl_info *info)
238{
239 __ETHTOOL_DECLARE_LINK_MODE_MASK(fec_link_modes) = {};
240 struct net_device *dev = req_info->dev;
241 struct nlattr **tb = info->attrs;
242 struct ethtool_fecparam fec = {};
243 bool mod = false;
244 u8 fec_auto;
245 int ret;
246
247 ret = dev->ethtool_ops->get_fecparam(dev, &fec);
248 if (ret < 0)
249 return ret;
250
251 ethtool_fec_to_link_modes(fec: fec.fec, link_modes: fec_link_modes, fec_auto: &fec_auto);
252
253 ret = ethnl_update_bitset(bitmap: fec_link_modes,
254 nbits: __ETHTOOL_LINK_MODE_MASK_NBITS,
255 attr: tb[ETHTOOL_A_FEC_MODES],
256 names: link_mode_names, extack: info->extack, mod: &mod);
257 if (ret < 0)
258 return ret;
259 ethnl_update_u8(dst: &fec_auto, attr: tb[ETHTOOL_A_FEC_AUTO], mod: &mod);
260 if (!mod)
261 return 0;
262
263 ret = ethtool_link_modes_to_fecparam(fec: &fec, link_modes: fec_link_modes, fec_auto);
264 if (ret) {
265 NL_SET_ERR_MSG_ATTR(info->extack, tb[ETHTOOL_A_FEC_MODES],
266 "invalid FEC modes requested");
267 return ret;
268 }
269 if (!fec.fec) {
270 NL_SET_ERR_MSG_ATTR(info->extack, tb[ETHTOOL_A_FEC_MODES],
271 "no FEC modes set");
272 return -EINVAL;
273 }
274
275 ret = dev->ethtool_ops->set_fecparam(dev, &fec);
276 return ret < 0 ? ret : 1;
277}
278
279const struct ethnl_request_ops ethnl_fec_request_ops = {
280 .request_cmd = ETHTOOL_MSG_FEC_GET,
281 .reply_cmd = ETHTOOL_MSG_FEC_GET_REPLY,
282 .hdr_attr = ETHTOOL_A_FEC_HEADER,
283 .req_info_size = sizeof(struct fec_req_info),
284 .reply_data_size = sizeof(struct fec_reply_data),
285
286 .prepare_data = fec_prepare_data,
287 .reply_size = fec_reply_size,
288 .fill_reply = fec_fill_reply,
289
290 .set_validate = ethnl_set_fec_validate,
291 .set = ethnl_set_fec,
292 .set_ntf_cmd = ETHTOOL_MSG_FEC_NTF,
293};
294

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