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