mse.c

  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, &i, PHY_MSE_CAP_CHANNEL_A,
 74					PHY_MSE_CHANNEL_A);
 75	if (ret)
 76		return ret;
 77	ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_B,
 78					PHY_MSE_CHANNEL_B);
 79	if (ret)
 80		return ret;
 81	ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_C,
 82					PHY_MSE_CHANNEL_C);
 83	if (ret)
 84		return ret;
 85	ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_D,
 86					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, &i,
 98					PHY_MSE_CAP_WORST_CHANNEL,
 99					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, &i, PHY_MSE_CAP_LINK,
111					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_base, info->attrs,
129				      ETHTOOL_A_MSE_HEADER, info->extack);
130	if (IS_ERR(phydev))
131		return PTR_ERR(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(&phydev->lock);
159	ethnl_ops_complete(dev);
160	if (ret)
161		kfree(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(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(0);
181	if (data->capability.supported_caps & PHY_MSE_CAP_AVG)
182		/* ETHTOOL_A_MSE_CAPABILITIES_MAX_AVERAGE_MSE */
183		len += nla_total_size(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(sizeof(u64));
188	/* ETHTOOL_A_MSE_CAPABILITIES_REFRESH_RATE_PS */
189	len += nla_total_size(sizeof(u64));
190	/* ETHTOOL_A_MSE_CAPABILITIES_NUM_SYMBOLS */
191	len += nla_total_size(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(0);
200
201		if (data->capability.supported_caps & PHY_MSE_CAP_AVG)
202			snapshot_len += nla_total_size(sizeof(u64));
203		if (data->capability.supported_caps & PHY_MSE_CAP_PEAK)
204			snapshot_len += nla_total_size(sizeof(u64));
205		if (data->capability.supported_caps & PHY_MSE_CAP_WORST_PEAK)
206			snapshot_len += nla_total_size(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, 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				   ETHTOOL_A_MSE_CAPABILITIES_MAX_AVERAGE_MSE,
250				   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, ETHTOOL_A_MSE_CAPABILITIES_MAX_PEAK_MSE,
258				   data->capability.max_peak_mse);
259		if (ret < 0)
260			goto nla_put_nest_failure;
261	}
262
263	ret = nla_put_uint(skb, ETHTOOL_A_MSE_CAPABILITIES_REFRESH_RATE_PS,
264			   data->capability.refresh_rate_ps);
265	if (ret < 0)
266		goto nla_put_nest_failure;
267
268	ret = nla_put_uint(skb, ETHTOOL_A_MSE_CAPABILITIES_NUM_SYMBOLS,
269			   data->capability.num_symbols);
270	if (ret < 0)
271		goto nla_put_nest_failure;
272
273	nla_nest_end(skb, 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(s->channel);
280		if (chan_attr < 0)
281			return chan_attr;
282
283		nest = nla_nest_start(skb, 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					   ETHTOOL_A_MSE_SNAPSHOT_AVERAGE_MSE,
290					   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, ETHTOOL_A_MSE_SNAPSHOT_PEAK_MSE,
296					   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					   ETHTOOL_A_MSE_SNAPSHOT_WORST_PEAK_MSE,
303					   s->snapshot.worst_peak_mse);
304			if (ret)
305				goto nla_put_nest_failure;
306		}
307
308		nla_nest_end(skb, nest);
309	}
310
311	return 0;
312
313nla_put_nest_failure:
314	nla_nest_cancel(skb, 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};