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 
14 struct tsinfo_req_info {
15 	struct ethnl_req_info		base;
16 	struct hwtstamp_provider_desc	hwprov_desc;
17 };
18 
19 struct 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 
34 const 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 
ts_parse_hwtst_provider(const struct nlattr * nest,struct hwtstamp_provider_desc * hwprov_desc,struct netlink_ext_ack * extack,bool * mod)41 int 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 
72 static int
tsinfo_parse_request(struct ethnl_req_info * req_base,struct nlattr ** tb,struct netlink_ext_ack * extack)73 tsinfo_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 
tsinfo_prepare_data(const struct ethnl_req_info * req_base,struct ethnl_reply_data * reply_base,const struct genl_info * info)88 static 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 
tsinfo_reply_size(const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)121 static 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 (req_base->flags & ETHTOOL_FLAG_STATS)
164 		len += nla_total_size(0) + /* _TSINFO_STATS */
165 		       nla_total_size_64bit(sizeof(u64)) * ETHTOOL_TS_STAT_CNT;
166 
167 	return len;
168 }
169 
tsinfo_put_stat(struct sk_buff * skb,u64 val,u16 attrtype)170 static int tsinfo_put_stat(struct sk_buff *skb, u64 val, u16 attrtype)
171 {
172 	if (val == ETHTOOL_STAT_NOT_SET)
173 		return 0;
174 	if (nla_put_uint(skb, attrtype, val))
175 		return -EMSGSIZE;
176 	return 0;
177 }
178 
tsinfo_put_stats(struct sk_buff * skb,const struct ethtool_ts_stats * stats)179 static int tsinfo_put_stats(struct sk_buff *skb,
180 			    const struct ethtool_ts_stats *stats)
181 {
182 	struct nlattr *nest;
183 
184 	nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_STATS);
185 	if (!nest)
186 		return -EMSGSIZE;
187 
188 	if (tsinfo_put_stat(skb, stats->tx_stats.pkts,
189 			    ETHTOOL_A_TS_STAT_TX_PKTS) ||
190 	    tsinfo_put_stat(skb, stats->tx_stats.onestep_pkts_unconfirmed,
191 			    ETHTOOL_A_TS_STAT_TX_ONESTEP_PKTS_UNCONFIRMED) ||
192 	    tsinfo_put_stat(skb, stats->tx_stats.lost,
193 			    ETHTOOL_A_TS_STAT_TX_LOST) ||
194 	    tsinfo_put_stat(skb, stats->tx_stats.err,
195 			    ETHTOOL_A_TS_STAT_TX_ERR))
196 		goto err_cancel;
197 
198 	nla_nest_end(skb, nest);
199 	return 0;
200 
201 err_cancel:
202 	nla_nest_cancel(skb, nest);
203 	return -EMSGSIZE;
204 }
205 
tsinfo_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)206 static int tsinfo_fill_reply(struct sk_buff *skb,
207 			     const struct ethnl_req_info *req_base,
208 			     const struct ethnl_reply_data *reply_base)
209 {
210 	const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
211 	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
212 	const struct kernel_ethtool_ts_info *ts_info = &data->ts_info;
213 	int ret;
214 
215 	if (ts_info->so_timestamping) {
216 		ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TIMESTAMPING,
217 					 &ts_info->so_timestamping, NULL,
218 					 __SOF_TIMESTAMPING_CNT,
219 					 sof_timestamping_names, compact);
220 		if (ret < 0)
221 			return ret;
222 	}
223 	if (ts_info->tx_types) {
224 		ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TX_TYPES,
225 					 &ts_info->tx_types, NULL,
226 					 __HWTSTAMP_TX_CNT,
227 					 ts_tx_type_names, compact);
228 		if (ret < 0)
229 			return ret;
230 	}
231 	if (ts_info->rx_filters) {
232 		ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_RX_FILTERS,
233 					 &ts_info->rx_filters, NULL,
234 					 __HWTSTAMP_FILTER_CNT,
235 					 ts_rx_filter_names, compact);
236 		if (ret < 0)
237 			return ret;
238 	}
239 	if (ts_info->phc_index >= 0) {
240 		struct nlattr *nest;
241 
242 		ret = nla_put_u32(skb, ETHTOOL_A_TSINFO_PHC_INDEX,
243 				  ts_info->phc_index);
244 		if (ret)
245 			return -EMSGSIZE;
246 
247 		nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER);
248 		if (!nest)
249 			return -EMSGSIZE;
250 
251 		if (nla_put_u32(skb, ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX,
252 				ts_info->phc_index) ||
253 		    nla_put_u32(skb,
254 				ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER,
255 				ts_info->phc_qualifier)) {
256 			nla_nest_cancel(skb, nest);
257 			return -EMSGSIZE;
258 		}
259 
260 		nla_nest_end(skb, nest);
261 	}
262 	if (req_base->flags & ETHTOOL_FLAG_STATS &&
263 	    tsinfo_put_stats(skb, &data->stats))
264 		return -EMSGSIZE;
265 
266 	return 0;
267 }
268 
269 struct ethnl_tsinfo_dump_ctx {
270 	struct tsinfo_req_info		*req_info;
271 	struct tsinfo_reply_data	*reply_data;
272 	unsigned long			pos_ifindex;
273 	bool				netdev_dump_done;
274 	unsigned long			pos_phyindex;
275 	enum hwtstamp_provider_qualifier pos_phcqualifier;
276 };
277 
ethnl_tsinfo_prepare_dump(struct sk_buff * skb,struct net_device * dev,struct tsinfo_reply_data * reply_data,struct netlink_callback * cb)278 static void *ethnl_tsinfo_prepare_dump(struct sk_buff *skb,
279 				       struct net_device *dev,
280 				       struct tsinfo_reply_data *reply_data,
281 				       struct netlink_callback *cb)
282 {
283 	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
284 	void *ehdr = NULL;
285 
286 	ehdr = ethnl_dump_put(skb, cb,
287 			      ETHTOOL_MSG_TSINFO_GET_REPLY);
288 	if (!ehdr)
289 		return ERR_PTR(-EMSGSIZE);
290 
291 	reply_data = ctx->reply_data;
292 	memset(reply_data, 0, sizeof(*reply_data));
293 	reply_data->base.dev = dev;
294 	reply_data->ts_info.cmd = ETHTOOL_GET_TS_INFO;
295 	reply_data->ts_info.phc_index = -1;
296 
297 	return ehdr;
298 }
299 
ethnl_tsinfo_end_dump(struct sk_buff * skb,struct net_device * dev,struct tsinfo_req_info * req_info,struct tsinfo_reply_data * reply_data,void * ehdr)300 static int ethnl_tsinfo_end_dump(struct sk_buff *skb,
301 				 struct net_device *dev,
302 				 struct tsinfo_req_info *req_info,
303 				 struct tsinfo_reply_data *reply_data,
304 				 void *ehdr)
305 {
306 	int ret;
307 
308 	reply_data->ts_info.so_timestamping |= SOF_TIMESTAMPING_RX_SOFTWARE |
309 					       SOF_TIMESTAMPING_SOFTWARE;
310 
311 	ret = ethnl_fill_reply_header(skb, dev, ETHTOOL_A_TSINFO_HEADER);
312 	if (ret < 0)
313 		return ret;
314 
315 	ret = tsinfo_fill_reply(skb, &req_info->base, &reply_data->base);
316 	if (ret < 0)
317 		return ret;
318 
319 	reply_data->base.dev = NULL;
320 	genlmsg_end(skb, ehdr);
321 
322 	return ret;
323 }
324 
ethnl_tsinfo_dump_one_phydev(struct sk_buff * skb,struct net_device * dev,struct phy_device * phydev,struct netlink_callback * cb)325 static int ethnl_tsinfo_dump_one_phydev(struct sk_buff *skb,
326 					struct net_device *dev,
327 					struct phy_device *phydev,
328 					struct netlink_callback *cb)
329 {
330 	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
331 	struct tsinfo_reply_data *reply_data;
332 	struct tsinfo_req_info *req_info;
333 	void *ehdr = NULL;
334 	int ret = 0;
335 
336 	if (!phy_has_tsinfo(phydev))
337 		return -EOPNOTSUPP;
338 
339 	reply_data = ctx->reply_data;
340 	req_info = ctx->req_info;
341 	ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb);
342 	if (IS_ERR(ehdr))
343 		return PTR_ERR(ehdr);
344 
345 	ret = phy_ts_info(phydev, &reply_data->ts_info);
346 	if (ret < 0)
347 		goto err;
348 
349 	ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data, ehdr);
350 	if (ret < 0)
351 		goto err;
352 
353 	return ret;
354 err:
355 	genlmsg_cancel(skb, ehdr);
356 	return ret;
357 }
358 
ethnl_tsinfo_dump_one_netdev(struct sk_buff * skb,struct net_device * dev,struct netlink_callback * cb)359 static int ethnl_tsinfo_dump_one_netdev(struct sk_buff *skb,
360 					struct net_device *dev,
361 					struct netlink_callback *cb)
362 {
363 	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
364 	const struct ethtool_ops *ops = dev->ethtool_ops;
365 	struct tsinfo_reply_data *reply_data;
366 	struct tsinfo_req_info *req_info;
367 	void *ehdr = NULL;
368 	int ret = 0;
369 
370 	if (!ops->get_ts_info)
371 		return -EOPNOTSUPP;
372 
373 	reply_data = ctx->reply_data;
374 	req_info = ctx->req_info;
375 	for (; ctx->pos_phcqualifier < HWTSTAMP_PROVIDER_QUALIFIER_CNT;
376 	     ctx->pos_phcqualifier++) {
377 		if (!net_support_hwtstamp_qualifier(dev,
378 						    ctx->pos_phcqualifier))
379 			continue;
380 
381 		ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb);
382 		if (IS_ERR(ehdr)) {
383 			ret = PTR_ERR(ehdr);
384 			goto err;
385 		}
386 
387 		reply_data->ts_info.phc_qualifier = ctx->pos_phcqualifier;
388 		ret = ops->get_ts_info(dev, &reply_data->ts_info);
389 		if (ret < 0)
390 			goto err;
391 
392 		ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data,
393 					    ehdr);
394 		if (ret < 0)
395 			goto err;
396 	}
397 
398 	return ret;
399 
400 err:
401 	genlmsg_cancel(skb, ehdr);
402 	return ret;
403 }
404 
ethnl_tsinfo_dump_one_net_topo(struct sk_buff * skb,struct net_device * dev,struct netlink_callback * cb)405 static int ethnl_tsinfo_dump_one_net_topo(struct sk_buff *skb,
406 					  struct net_device *dev,
407 					  struct netlink_callback *cb)
408 {
409 	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
410 	struct phy_device_node *pdn;
411 	int ret = 0;
412 
413 	if (!ctx->netdev_dump_done) {
414 		ret = ethnl_tsinfo_dump_one_netdev(skb, dev, cb);
415 		if (ret < 0 && ret != -EOPNOTSUPP)
416 			return ret;
417 		ctx->netdev_dump_done = true;
418 	}
419 
420 	if (!dev->link_topo) {
421 		if (phy_has_tsinfo(dev->phydev)) {
422 			ret = ethnl_tsinfo_dump_one_phydev(skb, dev,
423 							   dev->phydev, cb);
424 			if (ret < 0 && ret != -EOPNOTSUPP)
425 				return ret;
426 		}
427 
428 		return 0;
429 	}
430 
431 	xa_for_each_start(&dev->link_topo->phys, ctx->pos_phyindex, pdn,
432 			  ctx->pos_phyindex) {
433 		if (phy_has_tsinfo(pdn->phy)) {
434 			ret = ethnl_tsinfo_dump_one_phydev(skb, dev,
435 							   pdn->phy, cb);
436 			if (ret < 0 && ret != -EOPNOTSUPP)
437 				return ret;
438 		}
439 	}
440 
441 	return ret;
442 }
443 
ethnl_tsinfo_dumpit(struct sk_buff * skb,struct netlink_callback * cb)444 int ethnl_tsinfo_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
445 {
446 	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
447 	struct net *net = sock_net(skb->sk);
448 	struct net_device *dev;
449 	int ret = 0;
450 
451 	rtnl_lock();
452 	if (ctx->req_info->base.dev) {
453 		dev = ctx->req_info->base.dev;
454 		netdev_lock_ops(dev);
455 		ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb);
456 		netdev_unlock_ops(dev);
457 	} else {
458 		for_each_netdev_dump(net, dev, ctx->pos_ifindex) {
459 			netdev_lock_ops(dev);
460 			ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb);
461 			netdev_unlock_ops(dev);
462 			if (ret < 0 && ret != -EOPNOTSUPP)
463 				break;
464 			ctx->pos_phyindex = 0;
465 			ctx->netdev_dump_done = false;
466 			ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE;
467 		}
468 	}
469 	rtnl_unlock();
470 
471 	return ret;
472 }
473 
ethnl_tsinfo_start(struct netlink_callback * cb)474 int ethnl_tsinfo_start(struct netlink_callback *cb)
475 {
476 	const struct genl_dumpit_info *info = genl_dumpit_info(cb);
477 	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
478 	struct nlattr **tb = info->info.attrs;
479 	struct tsinfo_reply_data *reply_data;
480 	struct tsinfo_req_info *req_info;
481 	int ret;
482 
483 	BUILD_BUG_ON(sizeof(*ctx) > sizeof(cb->ctx));
484 
485 	req_info = kzalloc(sizeof(*req_info), GFP_KERNEL);
486 	if (!req_info)
487 		return -ENOMEM;
488 	reply_data = kzalloc(sizeof(*reply_data), GFP_KERNEL);
489 	if (!reply_data) {
490 		ret = -ENOMEM;
491 		goto free_req_info;
492 	}
493 
494 	ret = ethnl_parse_header_dev_get(&req_info->base,
495 					 tb[ETHTOOL_A_TSINFO_HEADER],
496 					 sock_net(cb->skb->sk), cb->extack,
497 					 false);
498 	if (ret < 0)
499 		goto free_reply_data;
500 
501 	ctx->req_info = req_info;
502 	ctx->reply_data = reply_data;
503 	ctx->pos_ifindex = 0;
504 	ctx->pos_phyindex = 0;
505 	ctx->netdev_dump_done = false;
506 	ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE;
507 
508 	return 0;
509 
510 free_reply_data:
511 	kfree(reply_data);
512 free_req_info:
513 	kfree(req_info);
514 
515 	return ret;
516 }
517 
ethnl_tsinfo_done(struct netlink_callback * cb)518 int ethnl_tsinfo_done(struct netlink_callback *cb)
519 {
520 	struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
521 	struct tsinfo_req_info *req_info = ctx->req_info;
522 
523 	ethnl_parse_header_dev_put(&req_info->base);
524 	kfree(ctx->reply_data);
525 	kfree(ctx->req_info);
526 
527 	return 0;
528 }
529 
530 const struct ethnl_request_ops ethnl_tsinfo_request_ops = {
531 	.request_cmd		= ETHTOOL_MSG_TSINFO_GET,
532 	.reply_cmd		= ETHTOOL_MSG_TSINFO_GET_REPLY,
533 	.hdr_attr		= ETHTOOL_A_TSINFO_HEADER,
534 	.req_info_size		= sizeof(struct tsinfo_req_info),
535 	.reply_data_size	= sizeof(struct tsinfo_reply_data),
536 
537 	.parse_request		= tsinfo_parse_request,
538 	.prepare_data		= tsinfo_prepare_data,
539 	.reply_size		= tsinfo_reply_size,
540 	.fill_reply		= tsinfo_fill_reply,
541 };
542