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 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((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 /* FEC_SET */
221
222 const 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
228 static int
229 ethnl_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
236 static int
237 ethnl_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_auto);
252
253 ret = ethnl_update_bitset(fec_link_modes,
254 __ETHTOOL_LINK_MODE_MASK_NBITS,
255 tb[ETHTOOL_A_FEC_MODES],
256 link_mode_names, info->extack, &mod);
257 if (ret < 0)
258 return ret;
259 ethnl_update_u8(&fec_auto, tb[ETHTOOL_A_FEC_AUTO], &mod);
260 if (!mod)
261 return 0;
262
263 ret = ethtool_link_modes_to_fecparam(&fec, 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
279 const 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
Linux® is a registered trademark of Linus Torvalds in the United States and other countries.
TOMOYO® is a registered trademark of NTT DATA CORPORATION.