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 (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
tsinfo_put_stat(struct sk_buff * skb,u64 val,u16 attrtype)176 static 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
tsinfo_put_stats(struct sk_buff * skb,const struct ethtool_ts_stats * stats)185 static 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
207 err_cancel:
208 nla_nest_cancel(skb, nest);
209 return -EMSGSIZE;
210 }
211
tsinfo_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)212 static 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
285 struct 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
ethnl_tsinfo_prepare_dump(struct sk_buff * skb,struct net_device * dev,struct tsinfo_reply_data * reply_data,struct netlink_callback * cb)294 static 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
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)316 static 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
ethnl_tsinfo_dump_one_phydev(struct sk_buff * skb,struct net_device * dev,struct phy_device * phydev,struct netlink_callback * cb)341 static 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;
375 err:
376 genlmsg_cancel(skb, ehdr);
377 return ret;
378 }
379
ethnl_tsinfo_dump_one_netdev(struct sk_buff * skb,struct net_device * dev,struct netlink_callback * cb)380 static 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
423 err:
424 genlmsg_cancel(skb, ehdr);
425 return ret;
426 }
427
ethnl_tsinfo_dump_one_net_topo(struct sk_buff * skb,struct net_device * dev,struct netlink_callback * cb)428 static 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
ethnl_tsinfo_dumpit(struct sk_buff * skb,struct netlink_callback * cb)467 int 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
ethnl_tsinfo_start(struct netlink_callback * cb)497 int 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
533 free_reply_data:
534 kfree(reply_data);
535 free_req_info:
536 kfree(req_info);
537
538 return ret;
539 }
540
ethnl_tsinfo_done(struct netlink_callback * cb)541 int 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
553 const 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 };
565