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