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
ethtool_fec_to_link_modes(u32 fec,unsigned long * link_modes,u8 * fec_auto)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
ethtool_link_modes_to_fecparam(struct ethtool_fecparam * fec,unsigned long * link_modes,u8 fec_auto)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
fec_stats_recalc(struct fec_stat_grp * grp,struct ethtool_fec_stat * stats)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
fec_prepare_data(const struct ethnl_req_info * req_base,struct ethnl_reply_data * reply_base,struct genl_info * info)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
fec_reply_size(const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)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
fec_put_stats(struct sk_buff * skb,const struct fec_reply_data * data)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
fec_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)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
ethnl_set_fec(struct sk_buff * skb,struct genl_info * info)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