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(src: link_modes, nbits: __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(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 | |
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(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 | |
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, 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 | |
189 | err_cancel: |
190 | nla_nest_cancel(skb, start: 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, 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 | |
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_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 | |
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 | |