tsinfo.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2
  3#include <linux/net_tstamp.h>
  4#include <linux/phy.h>
  5#include <linux/phy_link_topology.h>
  6#include <linux/ptp_clock_kernel.h>
  7#include <net/netdev_lock.h>
  8
  9#include "netlink.h"
 10#include "common.h"
 11#include "bitset.h"
 12#include "ts.h"
 13
 14struct tsinfo_req_info {
 15	struct ethnl_req_info		base;
 16	struct hwtstamp_provider_desc	hwprov_desc;
 17};
 18
 19struct tsinfo_reply_data {
 20	struct ethnl_reply_data		base;
 21	struct kernel_ethtool_ts_info	ts_info;
 22	struct ethtool_ts_stats		stats;
 23};
 24
 25#define TSINFO_REQINFO(__req_base) \
 26	container_of(__req_base, struct tsinfo_req_info, base)
 27
 28#define TSINFO_REPDATA(__reply_base) \
 29	container_of(__reply_base, struct tsinfo_reply_data, base)
 30
 31#define ETHTOOL_TS_STAT_CNT \
 32	(__ETHTOOL_A_TS_STAT_CNT - (ETHTOOL_A_TS_STAT_UNSPEC + 1))
 33
 34const struct nla_policy ethnl_tsinfo_get_policy[ETHTOOL_A_TSINFO_MAX + 1] = {
 35	[ETHTOOL_A_TSINFO_HEADER]		=
 36		NLA_POLICY_NESTED(ethnl_header_policy_stats),
 37	[ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER] =
 38		NLA_POLICY_NESTED(ethnl_ts_hwtst_prov_policy),
 39};
 40
 41int ts_parse_hwtst_provider(const struct nlattr *nest,
 42			    struct hwtstamp_provider_desc *hwprov_desc,
 43			    struct netlink_ext_ack *extack,
 44			    bool *mod)
 45{
 46	struct nlattr *tb[ARRAY_SIZE(ethnl_ts_hwtst_prov_policy)];
 47	int ret;
 48
 49	ret = nla_parse_nested(tb,
 50			       ARRAY_SIZE(ethnl_ts_hwtst_prov_policy) - 1,
 51			       nest,
 52			       ethnl_ts_hwtst_prov_policy, extack);
 53	if (ret < 0)
 54		return ret;
 55
 56	if (NL_REQ_ATTR_CHECK(extack, nest, tb,
 57			      ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX) ||
 58	    NL_REQ_ATTR_CHECK(extack, nest, tb,
 59			      ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER))
 60		return -EINVAL;
 61
 62	ethnl_update_u32(&hwprov_desc->index,
 63			 tb[ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX],
 64			 mod);
 65	ethnl_update_u32(&hwprov_desc->qualifier,
 66			 tb[ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER],
 67			 mod);
 68
 69	return 0;
 70}
 71
 72static int
 73tsinfo_parse_request(struct ethnl_req_info *req_base, struct nlattr **tb,
 74		     struct netlink_ext_ack *extack)
 75{
 76	struct tsinfo_req_info *req = TSINFO_REQINFO(req_base);
 77	bool mod = false;
 78
 79	req->hwprov_desc.index = -1;
 80
 81	if (!tb[ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER])
 82		return 0;
 83
 84	return ts_parse_hwtst_provider(tb[ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER],
 85				       &req->hwprov_desc, extack, &mod);
 86}
 87
 88static int tsinfo_prepare_data(const struct ethnl_req_info *req_base,
 89			       struct ethnl_reply_data *reply_base,
 90			       const struct genl_info *info)
 91{
 92	struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
 93	struct tsinfo_req_info *req = TSINFO_REQINFO(req_base);
 94	struct net_device *dev = reply_base->dev;
 95	int ret;
 96
 97	ret = ethnl_ops_begin(dev);
 98	if (ret < 0)
 99		return ret;
100
101	if (req->hwprov_desc.index != -1) {
102		ret = ethtool_get_ts_info_by_phc(dev, &data->ts_info,
103						 &req->hwprov_desc);
104		ethnl_ops_complete(dev);
105		return ret;
106	}
107
108	if (req_base->flags & ETHTOOL_FLAG_STATS) {
109		ethtool_stats_init((u64 *)&data->stats,
110				   sizeof(data->stats) / sizeof(u64));
111		if (dev->ethtool_ops->get_ts_stats)
112			dev->ethtool_ops->get_ts_stats(dev, &data->stats);
113	}
114
115	ret = __ethtool_get_ts_info(dev, &data->ts_info);
116	ethnl_ops_complete(dev);
117
118	return ret;
119}
120
121static int tsinfo_reply_size(const struct ethnl_req_info *req_base,
122			     const struct ethnl_reply_data *reply_base)
123{
124	const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
125	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
126	const struct kernel_ethtool_ts_info *ts_info = &data->ts_info;
127	int len = 0;
128	int ret;
129
130	BUILD_BUG_ON(__SOF_TIMESTAMPING_CNT > 32);
131	BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32);
132	BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32);
133
134	if (ts_info->so_timestamping) {
135		ret = ethnl_bitset32_size(&ts_info->so_timestamping, NULL,
136					  __SOF_TIMESTAMPING_CNT,
137					  sof_timestamping_names, compact);
138		if (ret < 0)
139			return ret;
140		len += ret;	/* _TSINFO_TIMESTAMPING */
141	}
142	if (ts_info->tx_types) {
143		ret = ethnl_bitset32_size(&ts_info->tx_types, NULL,
144					  __HWTSTAMP_TX_CNT,
145					  ts_tx_type_names, compact);
146		if (ret < 0)
147			return ret;
148		len += ret;	/* _TSINFO_TX_TYPES */
149	}
150	if (ts_info->rx_filters) {
151		ret = ethnl_bitset32_size(&ts_info->rx_filters, NULL,
152					  __HWTSTAMP_FILTER_CNT,
153					  ts_rx_filter_names, compact);
154		if (ret < 0)
155			return ret;
156		len += ret;	/* _TSINFO_RX_FILTERS */
157	}
158	if (ts_info->phc_index >= 0) {
159		len += nla_total_size(sizeof(u32));	/* _TSINFO_PHC_INDEX */
160		/* _TSINFO_HWTSTAMP_PROVIDER */
161		len += nla_total_size(0) + 2 * nla_total_size(sizeof(u32));
162	}
163	if (ts_info->phc_source) {
164		len += nla_total_size(sizeof(u32));	/* _TSINFO_HWTSTAMP_SOURCE */
165		if (ts_info->phc_phyindex)
166			/* _TSINFO_HWTSTAMP_PHYINDEX */
167			len += nla_total_size(sizeof(u32));
168	}
169	if (req_base->flags & ETHTOOL_FLAG_STATS)
170		len += nla_total_size(0) + /* _TSINFO_STATS */
171		       nla_total_size_64bit(sizeof(u64)) * ETHTOOL_TS_STAT_CNT;
172
173	return len;
174}
175
176static int tsinfo_put_stat(struct sk_buff *skb, u64 val, u16 attrtype)
177{
178	if (val == ETHTOOL_STAT_NOT_SET)
179		return 0;
180	if (nla_put_uint(skb, attrtype, val))
181		return -EMSGSIZE;
182	return 0;
183}
184
185static int tsinfo_put_stats(struct sk_buff *skb,
186			    const struct ethtool_ts_stats *stats)
187{
188	struct nlattr *nest;
189
190	nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_STATS);
191	if (!nest)
192		return -EMSGSIZE;
193
194	if (tsinfo_put_stat(skb, stats->tx_stats.pkts,
195			    ETHTOOL_A_TS_STAT_TX_PKTS) ||
196	    tsinfo_put_stat(skb, stats->tx_stats.onestep_pkts_unconfirmed,
197			    ETHTOOL_A_TS_STAT_TX_ONESTEP_PKTS_UNCONFIRMED) ||
198	    tsinfo_put_stat(skb, stats->tx_stats.lost,
199			    ETHTOOL_A_TS_STAT_TX_LOST) ||
200	    tsinfo_put_stat(skb, stats->tx_stats.err,
201			    ETHTOOL_A_TS_STAT_TX_ERR))
202		goto err_cancel;
203
204	nla_nest_end(skb, nest);
205	return 0;
206
207err_cancel:
208	nla_nest_cancel(skb, nest);
209	return -EMSGSIZE;
210}
211
212static int tsinfo_fill_reply(struct sk_buff *skb,
213			     const struct ethnl_req_info *req_base,
214			     const struct ethnl_reply_data *reply_base)
215{
216	const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
217	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
218	const struct kernel_ethtool_ts_info *ts_info = &data->ts_info;
219	int ret;
220
221	if (ts_info->so_timestamping) {
222		ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TIMESTAMPING,
223					 &ts_info->so_timestamping, NULL,
224					 __SOF_TIMESTAMPING_CNT,
225					 sof_timestamping_names, compact);
226		if (ret < 0)
227			return ret;
228	}
229	if (ts_info->tx_types) {
230		ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TX_TYPES,
231					 &ts_info->tx_types, NULL,
232					 __HWTSTAMP_TX_CNT,
233					 ts_tx_type_names, compact);
234		if (ret < 0)
235			return ret;
236	}
237	if (ts_info->rx_filters) {
238		ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_RX_FILTERS,
239					 &ts_info->rx_filters, NULL,
240					 __HWTSTAMP_FILTER_CNT,
241					 ts_rx_filter_names, compact);
242		if (ret < 0)
243			return ret;
244	}
245	if (ts_info->phc_index >= 0) {
246		struct nlattr *nest;
247
248		ret = nla_put_u32(skb, ETHTOOL_A_TSINFO_PHC_INDEX,
249				  ts_info->phc_index);
250		if (ret)
251			return -EMSGSIZE;
252
253		nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER);
254		if (!nest)
255			return -EMSGSIZE;
256
257		if (nla_put_u32(skb, ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX,
258				ts_info->phc_index) ||
259		    nla_put_u32(skb,
260				ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER,
261				ts_info->phc_qualifier)) {
262			nla_nest_cancel(skb, nest);
263			return -EMSGSIZE;
264		}
265
266		nla_nest_end(skb, nest);
267	}
268	if (ts_info->phc_source) {
269		if (nla_put_u32(skb, ETHTOOL_A_TSINFO_HWTSTAMP_SOURCE,
270				ts_info->phc_source))
271			return -EMSGSIZE;
272
273		if (ts_info->phc_phyindex &&
274		    nla_put_u32(skb, ETHTOOL_A_TSINFO_HWTSTAMP_PHYINDEX,
275				ts_info->phc_phyindex))
276			return -EMSGSIZE;
277	}
278	if (req_base->flags & ETHTOOL_FLAG_STATS &&
279	    tsinfo_put_stats(skb, &data->stats))
280		return -EMSGSIZE;
281
282	return 0;
283}
284
285struct ethnl_tsinfo_dump_ctx {
286	struct tsinfo_req_info		*req_info;
287	struct tsinfo_reply_data	*reply_data;
288	unsigned long			pos_ifindex;
289	bool				netdev_dump_done;
290	unsigned long			pos_phyindex;
291	enum hwtstamp_provider_qualifier pos_phcqualifier;
292};
293
294static void *ethnl_tsinfo_prepare_dump(struct sk_buff *skb,
295				       struct net_device *dev,
296				       struct tsinfo_reply_data *reply_data,
297				       struct netlink_callback *cb)
298{
299	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
300	void *ehdr = NULL;
301
302	ehdr = ethnl_dump_put(skb, cb,
303			      ETHTOOL_MSG_TSINFO_GET_REPLY);
304	if (!ehdr)
305		return ERR_PTR(-EMSGSIZE);
306
307	reply_data = ctx->reply_data;
308	memset(reply_data, 0, sizeof(*reply_data));
309	reply_data->base.dev = dev;
310	reply_data->ts_info.cmd = ETHTOOL_GET_TS_INFO;
311	reply_data->ts_info.phc_index = -1;
312
313	return ehdr;
314}
315
316static int ethnl_tsinfo_end_dump(struct sk_buff *skb,
317				 struct net_device *dev,
318				 struct tsinfo_req_info *req_info,
319				 struct tsinfo_reply_data *reply_data,
320				 void *ehdr)
321{
322	int ret;
323
324	reply_data->ts_info.so_timestamping |= SOF_TIMESTAMPING_RX_SOFTWARE |
325					       SOF_TIMESTAMPING_SOFTWARE;
326
327	ret = ethnl_fill_reply_header(skb, dev, ETHTOOL_A_TSINFO_HEADER);
328	if (ret < 0)
329		return ret;
330
331	ret = tsinfo_fill_reply(skb, &req_info->base, &reply_data->base);
332	if (ret < 0)
333		return ret;
334
335	reply_data->base.dev = NULL;
336	genlmsg_end(skb, ehdr);
337
338	return ret;
339}
340
341static int ethnl_tsinfo_dump_one_phydev(struct sk_buff *skb,
342					struct net_device *dev,
343					struct phy_device *phydev,
344					struct netlink_callback *cb)
345{
346	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
347	struct tsinfo_reply_data *reply_data;
348	struct tsinfo_req_info *req_info;
349	void *ehdr = NULL;
350	int ret = 0;
351
352	if (!phy_has_tsinfo(phydev))
353		return -EOPNOTSUPP;
354
355	reply_data = ctx->reply_data;
356	req_info = ctx->req_info;
357	ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb);
358	if (IS_ERR(ehdr))
359		return PTR_ERR(ehdr);
360
361	ret = phy_ts_info(phydev, &reply_data->ts_info);
362	if (ret < 0)
363		goto err;
364
365	if (reply_data->ts_info.phc_index >= 0) {
366		reply_data->ts_info.phc_source = HWTSTAMP_SOURCE_PHYLIB;
367		reply_data->ts_info.phc_phyindex = phydev->phyindex;
368	}
369
370	ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data, ehdr);
371	if (ret < 0)
372		goto err;
373
374	return ret;
375err:
376	genlmsg_cancel(skb, ehdr);
377	return ret;
378}
379
380static int ethnl_tsinfo_dump_one_netdev(struct sk_buff *skb,
381					struct net_device *dev,
382					struct netlink_callback *cb)
383{
384	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
385	const struct ethtool_ops *ops = dev->ethtool_ops;
386	struct tsinfo_reply_data *reply_data;
387	struct tsinfo_req_info *req_info;
388	void *ehdr = NULL;
389	int ret = 0;
390
391	if (!ops->get_ts_info)
392		return -EOPNOTSUPP;
393
394	reply_data = ctx->reply_data;
395	req_info = ctx->req_info;
396	for (; ctx->pos_phcqualifier < HWTSTAMP_PROVIDER_QUALIFIER_CNT;
397	     ctx->pos_phcqualifier++) {
398		if (!net_support_hwtstamp_qualifier(dev,
399						    ctx->pos_phcqualifier))
400			continue;
401
402		ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb);
403		if (IS_ERR(ehdr)) {
404			ret = PTR_ERR(ehdr);
405			goto err;
406		}
407
408		reply_data->ts_info.phc_qualifier = ctx->pos_phcqualifier;
409		ret = ops->get_ts_info(dev, &reply_data->ts_info);
410		if (ret < 0)
411			goto err;
412
413		if (reply_data->ts_info.phc_index >= 0)
414			reply_data->ts_info.phc_source = HWTSTAMP_SOURCE_NETDEV;
415		ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data,
416					    ehdr);
417		if (ret < 0)
418			goto err;
419	}
420
421	return ret;
422
423err:
424	genlmsg_cancel(skb, ehdr);
425	return ret;
426}
427
428static int ethnl_tsinfo_dump_one_net_topo(struct sk_buff *skb,
429					  struct net_device *dev,
430					  struct netlink_callback *cb)
431{
432	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
433	struct phy_device_node *pdn;
434	int ret = 0;
435
436	if (!ctx->netdev_dump_done) {
437		ret = ethnl_tsinfo_dump_one_netdev(skb, dev, cb);
438		if (ret < 0 && ret != -EOPNOTSUPP)
439			return ret;
440		ctx->netdev_dump_done = true;
441	}
442
443	if (!dev->link_topo) {
444		if (phy_has_tsinfo(dev->phydev)) {
445			ret = ethnl_tsinfo_dump_one_phydev(skb, dev,
446							   dev->phydev, cb);
447			if (ret < 0 && ret != -EOPNOTSUPP)
448				return ret;
449		}
450
451		return 0;
452	}
453
454	xa_for_each_start(&dev->link_topo->phys, ctx->pos_phyindex, pdn,
455			  ctx->pos_phyindex) {
456		if (phy_has_tsinfo(pdn->phy)) {
457			ret = ethnl_tsinfo_dump_one_phydev(skb, dev,
458							   pdn->phy, cb);
459			if (ret < 0 && ret != -EOPNOTSUPP)
460				return ret;
461		}
462	}
463
464	return ret;
465}
466
467int ethnl_tsinfo_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
468{
469	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
470	struct net *net = sock_net(skb->sk);
471	struct net_device *dev;
472	int ret = 0;
473
474	rtnl_lock();
475	if (ctx->req_info->base.dev) {
476		dev = ctx->req_info->base.dev;
477		netdev_lock_ops(dev);
478		ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb);
479		netdev_unlock_ops(dev);
480	} else {
481		for_each_netdev_dump(net, dev, ctx->pos_ifindex) {
482			netdev_lock_ops(dev);
483			ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb);
484			netdev_unlock_ops(dev);
485			if (ret < 0 && ret != -EOPNOTSUPP)
486				break;
487			ctx->pos_phyindex = 0;
488			ctx->netdev_dump_done = false;
489			ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE;
490		}
491	}
492	rtnl_unlock();
493
494	return ret;
495}
496
497int ethnl_tsinfo_start(struct netlink_callback *cb)
498{
499	const struct genl_dumpit_info *info = genl_dumpit_info(cb);
500	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
501	struct nlattr **tb = info->info.attrs;
502	struct tsinfo_reply_data *reply_data;
503	struct tsinfo_req_info *req_info;
504	int ret;
505
506	BUILD_BUG_ON(sizeof(*ctx) > sizeof(cb->ctx));
507
508	req_info = kzalloc(sizeof(*req_info), GFP_KERNEL);
509	if (!req_info)
510		return -ENOMEM;
511	reply_data = kzalloc(sizeof(*reply_data), GFP_KERNEL);
512	if (!reply_data) {
513		ret = -ENOMEM;
514		goto free_req_info;
515	}
516
517	ret = ethnl_parse_header_dev_get(&req_info->base,
518					 tb[ETHTOOL_A_TSINFO_HEADER],
519					 sock_net(cb->skb->sk), cb->extack,
520					 false);
521	if (ret < 0)
522		goto free_reply_data;
523
524	ctx->req_info = req_info;
525	ctx->reply_data = reply_data;
526	ctx->pos_ifindex = 0;
527	ctx->pos_phyindex = 0;
528	ctx->netdev_dump_done = false;
529	ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE;
530
531	return 0;
532
533free_reply_data:
534	kfree(reply_data);
535free_req_info:
536	kfree(req_info);
537
538	return ret;
539}
540
541int ethnl_tsinfo_done(struct netlink_callback *cb)
542{
543	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
544	struct tsinfo_req_info *req_info = ctx->req_info;
545
546	ethnl_parse_header_dev_put(&req_info->base);
547	kfree(ctx->reply_data);
548	kfree(ctx->req_info);
549
550	return 0;
551}
552
553const struct ethnl_request_ops ethnl_tsinfo_request_ops = {
554	.request_cmd		= ETHTOOL_MSG_TSINFO_GET,
555	.reply_cmd		= ETHTOOL_MSG_TSINFO_GET_REPLY,
556	.hdr_attr		= ETHTOOL_A_TSINFO_HEADER,
557	.req_info_size		= sizeof(struct tsinfo_req_info),
558	.reply_data_size	= sizeof(struct tsinfo_reply_data),
559
560	.parse_request		= tsinfo_parse_request,
561	.prepare_data		= tsinfo_prepare_data,
562	.reply_size		= tsinfo_reply_size,
563	.fill_reply		= tsinfo_fill_reply,
564};