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 15 struct tsinfo_req_info { 16 struct ethnl_req_info base; 17 struct hwtstamp_provider_desc hwprov_desc; 18 }; 19 20 struct 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 35 const 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 42 int 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 73 static int 74 tsinfo_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 96 static 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 129 static 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 184 static 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 193 static 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 215 err_cancel: 216 nla_nest_cancel(skb, nest); 217 return -EMSGSIZE; 218 } 219 220 static 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 293 struct 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 302 static 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 324 static 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 349 static 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; 383 err: 384 genlmsg_cancel(skb, ehdr); 385 return ret; 386 } 387 388 static 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 429 err: 430 genlmsg_cancel(skb, ehdr); 431 return ret; 432 } 433 434 static 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 473 int 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 500 int 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 542 err_dev_put: 543 ethnl_parse_header_dev_put(&req_info->base); 544 free_reply_data: 545 kfree(reply_data); 546 free_req_info: 547 kfree(req_info); 548 549 return ret; 550 } 551 552 int 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 564 const 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 }; 576