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

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