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 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, 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 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 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 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 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 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 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 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 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 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 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 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 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 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