projects/linux·blob master · refs

575 lines · 14.6 KiB

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