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