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