1 // SPDX-License-Identifier: GPL-2.0-only
2
3 #include <linux/ethtool.h>
4 #include <linux/phy.h>
5 #include <linux/slab.h>
6
7 #include "netlink.h"
8 #include "common.h"
9
10 /* Channels A-D only; WORST and LINK are exclusive alternatives */
11 #define PHY_MSE_CHANNEL_COUNT 4
12
13 struct mse_req_info {
14 struct ethnl_req_info base;
15 };
16
17 struct mse_snapshot_entry {
18 struct phy_mse_snapshot snapshot;
19 int channel;
20 };
21
22 struct mse_reply_data {
23 struct ethnl_reply_data base;
24 struct phy_mse_capability capability;
25 struct mse_snapshot_entry *snapshots;
26 unsigned int num_snapshots;
27 };
28
29 static struct mse_reply_data *
mse_repdata(const struct ethnl_reply_data * reply_base)30 mse_repdata(const struct ethnl_reply_data *reply_base)
31 {
32 return container_of(reply_base, struct mse_reply_data, base);
33 }
34
35 const struct nla_policy ethnl_mse_get_policy[] = {
36 [ETHTOOL_A_MSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy_phy),
37 };
38
get_snapshot_if_supported(struct phy_device * phydev,struct mse_reply_data * data,unsigned int * idx,u32 cap_bit,enum phy_mse_channel channel)39 static int get_snapshot_if_supported(struct phy_device *phydev,
40 struct mse_reply_data *data,
41 unsigned int *idx, u32 cap_bit,
42 enum phy_mse_channel channel)
43 {
44 int ret;
45
46 if (data->capability.supported_caps & cap_bit) {
47 ret = phydev->drv->get_mse_snapshot(phydev, channel,
48 &data->snapshots[*idx].snapshot);
49 if (ret)
50 return ret;
51 data->snapshots[*idx].channel = channel;
52 (*idx)++;
53 }
54
55 return 0;
56 }
57
mse_get_channels(struct phy_device * phydev,struct mse_reply_data * data)58 static int mse_get_channels(struct phy_device *phydev,
59 struct mse_reply_data *data)
60 {
61 unsigned int i = 0;
62 int ret;
63
64 if (!data->capability.supported_caps)
65 return 0;
66
67 data->snapshots = kzalloc_objs(*data->snapshots, PHY_MSE_CHANNEL_COUNT);
68 if (!data->snapshots)
69 return -ENOMEM;
70
71 /* Priority 1: Individual channels */
72 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_A,
73 PHY_MSE_CHANNEL_A);
74 if (ret)
75 return ret;
76 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_B,
77 PHY_MSE_CHANNEL_B);
78 if (ret)
79 return ret;
80 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_C,
81 PHY_MSE_CHANNEL_C);
82 if (ret)
83 return ret;
84 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_D,
85 PHY_MSE_CHANNEL_D);
86 if (ret)
87 return ret;
88
89 /* If any individual channels were found, we are done. */
90 if (i > 0) {
91 data->num_snapshots = i;
92 return 0;
93 }
94
95 /* Priority 2: Worst channel, if no individual channels supported. */
96 ret = get_snapshot_if_supported(phydev, data, &i,
97 PHY_MSE_CAP_WORST_CHANNEL,
98 PHY_MSE_CHANNEL_WORST);
99 if (ret)
100 return ret;
101
102 /* If worst channel was found, we are done. */
103 if (i > 0) {
104 data->num_snapshots = i;
105 return 0;
106 }
107
108 /* Priority 3: Link-wide, if nothing else is supported. */
109 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_LINK,
110 PHY_MSE_CHANNEL_LINK);
111 if (ret)
112 return ret;
113
114 data->num_snapshots = i;
115 return 0;
116 }
117
mse_prepare_data(const struct ethnl_req_info * req_base,struct ethnl_reply_data * reply_base,const struct genl_info * info)118 static int mse_prepare_data(const struct ethnl_req_info *req_base,
119 struct ethnl_reply_data *reply_base,
120 const struct genl_info *info)
121 {
122 struct mse_reply_data *data = mse_repdata(reply_base);
123 struct net_device *dev = reply_base->dev;
124 struct phy_device *phydev;
125 int ret;
126
127 phydev = ethnl_req_get_phydev(req_base, info->attrs,
128 ETHTOOL_A_MSE_HEADER, info->extack);
129 if (IS_ERR(phydev))
130 return PTR_ERR(phydev);
131 if (!phydev)
132 return -EOPNOTSUPP;
133
134 ret = ethnl_ops_begin(dev);
135 if (ret)
136 return ret;
137
138 mutex_lock(&phydev->lock);
139
140 if (!phydev->drv || !phydev->drv->get_mse_capability ||
141 !phydev->drv->get_mse_snapshot) {
142 ret = -EOPNOTSUPP;
143 goto out_unlock;
144 }
145 if (!phydev->link) {
146 ret = -ENETDOWN;
147 goto out_unlock;
148 }
149
150 ret = phydev->drv->get_mse_capability(phydev, &data->capability);
151 if (ret)
152 goto out_unlock;
153
154 ret = mse_get_channels(phydev, data);
155
156 out_unlock:
157 mutex_unlock(&phydev->lock);
158 ethnl_ops_complete(dev);
159 if (ret)
160 kfree(data->snapshots);
161 return ret;
162 }
163
mse_cleanup_data(struct ethnl_reply_data * reply_base)164 static void mse_cleanup_data(struct ethnl_reply_data *reply_base)
165 {
166 struct mse_reply_data *data = mse_repdata(reply_base);
167
168 kfree(data->snapshots);
169 }
170
mse_reply_size(const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)171 static int mse_reply_size(const struct ethnl_req_info *req_base,
172 const struct ethnl_reply_data *reply_base)
173 {
174 const struct mse_reply_data *data = mse_repdata(reply_base);
175 size_t len = 0;
176 unsigned int i;
177
178 /* ETHTOOL_A_MSE_CAPABILITIES */
179 len += nla_total_size(0);
180 if (data->capability.supported_caps & PHY_MSE_CAP_AVG)
181 /* ETHTOOL_A_MSE_CAPABILITIES_MAX_AVERAGE_MSE */
182 len += nla_total_size(sizeof(u64));
183 if (data->capability.supported_caps & (PHY_MSE_CAP_PEAK |
184 PHY_MSE_CAP_WORST_PEAK))
185 /* ETHTOOL_A_MSE_CAPABILITIES_MAX_PEAK_MSE */
186 len += nla_total_size(sizeof(u64));
187 /* ETHTOOL_A_MSE_CAPABILITIES_REFRESH_RATE_PS */
188 len += nla_total_size(sizeof(u64));
189 /* ETHTOOL_A_MSE_CAPABILITIES_NUM_SYMBOLS */
190 len += nla_total_size(sizeof(u64));
191
192 for (i = 0; i < data->num_snapshots; i++) {
193 size_t snapshot_len = 0;
194
195 /* Per-channel nest (e.g., ETHTOOL_A_MSE_CHANNEL_A / _B / _C /
196 * _D / _WORST_CHANNEL / _LINK)
197 */
198 snapshot_len += nla_total_size(0);
199
200 if (data->capability.supported_caps & PHY_MSE_CAP_AVG)
201 snapshot_len += nla_total_size(sizeof(u64));
202 if (data->capability.supported_caps & PHY_MSE_CAP_PEAK)
203 snapshot_len += nla_total_size(sizeof(u64));
204 if (data->capability.supported_caps & PHY_MSE_CAP_WORST_PEAK)
205 snapshot_len += nla_total_size(sizeof(u64));
206
207 len += snapshot_len;
208 }
209
210 return len;
211 }
212
mse_channel_to_attr(int ch)213 static int mse_channel_to_attr(int ch)
214 {
215 switch (ch) {
216 case PHY_MSE_CHANNEL_A:
217 return ETHTOOL_A_MSE_CHANNEL_A;
218 case PHY_MSE_CHANNEL_B:
219 return ETHTOOL_A_MSE_CHANNEL_B;
220 case PHY_MSE_CHANNEL_C:
221 return ETHTOOL_A_MSE_CHANNEL_C;
222 case PHY_MSE_CHANNEL_D:
223 return ETHTOOL_A_MSE_CHANNEL_D;
224 case PHY_MSE_CHANNEL_WORST:
225 return ETHTOOL_A_MSE_WORST_CHANNEL;
226 case PHY_MSE_CHANNEL_LINK:
227 return ETHTOOL_A_MSE_LINK;
228 default:
229 return -EINVAL;
230 }
231 }
232
mse_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)233 static int mse_fill_reply(struct sk_buff *skb,
234 const struct ethnl_req_info *req_base,
235 const struct ethnl_reply_data *reply_base)
236 {
237 const struct mse_reply_data *data = mse_repdata(reply_base);
238 struct nlattr *nest;
239 unsigned int i;
240 int ret;
241
242 nest = nla_nest_start(skb, ETHTOOL_A_MSE_CAPABILITIES);
243 if (!nest)
244 return -EMSGSIZE;
245
246 if (data->capability.supported_caps & PHY_MSE_CAP_AVG) {
247 ret = nla_put_uint(skb,
248 ETHTOOL_A_MSE_CAPABILITIES_MAX_AVERAGE_MSE,
249 data->capability.max_average_mse);
250 if (ret < 0)
251 goto nla_put_nest_failure;
252 }
253
254 if (data->capability.supported_caps & (PHY_MSE_CAP_PEAK |
255 PHY_MSE_CAP_WORST_PEAK)) {
256 ret = nla_put_uint(skb, ETHTOOL_A_MSE_CAPABILITIES_MAX_PEAK_MSE,
257 data->capability.max_peak_mse);
258 if (ret < 0)
259 goto nla_put_nest_failure;
260 }
261
262 ret = nla_put_uint(skb, ETHTOOL_A_MSE_CAPABILITIES_REFRESH_RATE_PS,
263 data->capability.refresh_rate_ps);
264 if (ret < 0)
265 goto nla_put_nest_failure;
266
267 ret = nla_put_uint(skb, ETHTOOL_A_MSE_CAPABILITIES_NUM_SYMBOLS,
268 data->capability.num_symbols);
269 if (ret < 0)
270 goto nla_put_nest_failure;
271
272 nla_nest_end(skb, nest);
273
274 for (i = 0; i < data->num_snapshots; i++) {
275 const struct mse_snapshot_entry *s = &data->snapshots[i];
276 int chan_attr;
277
278 chan_attr = mse_channel_to_attr(s->channel);
279 if (chan_attr < 0)
280 return chan_attr;
281
282 nest = nla_nest_start(skb, chan_attr);
283 if (!nest)
284 return -EMSGSIZE;
285
286 if (data->capability.supported_caps & PHY_MSE_CAP_AVG) {
287 ret = nla_put_uint(skb,
288 ETHTOOL_A_MSE_SNAPSHOT_AVERAGE_MSE,
289 s->snapshot.average_mse);
290 if (ret)
291 goto nla_put_nest_failure;
292 }
293 if (data->capability.supported_caps & PHY_MSE_CAP_PEAK) {
294 ret = nla_put_uint(skb, ETHTOOL_A_MSE_SNAPSHOT_PEAK_MSE,
295 s->snapshot.peak_mse);
296 if (ret)
297 goto nla_put_nest_failure;
298 }
299 if (data->capability.supported_caps & PHY_MSE_CAP_WORST_PEAK) {
300 ret = nla_put_uint(skb,
301 ETHTOOL_A_MSE_SNAPSHOT_WORST_PEAK_MSE,
302 s->snapshot.worst_peak_mse);
303 if (ret)
304 goto nla_put_nest_failure;
305 }
306
307 nla_nest_end(skb, nest);
308 }
309
310 return 0;
311
312 nla_put_nest_failure:
313 nla_nest_cancel(skb, nest);
314 return ret;
315 }
316
317 const struct ethnl_request_ops ethnl_mse_request_ops = {
318 .request_cmd = ETHTOOL_MSG_MSE_GET,
319 .reply_cmd = ETHTOOL_MSG_MSE_GET_REPLY,
320 .hdr_attr = ETHTOOL_A_MSE_HEADER,
321 .req_info_size = sizeof(struct mse_req_info),
322 .reply_data_size = sizeof(struct mse_reply_data),
323
324 .prepare_data = mse_prepare_data,
325 .cleanup_data = mse_cleanup_data,
326 .reply_size = mse_reply_size,
327 .fill_reply = mse_fill_reply,
328 };
329