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 memset(&reply_data->ts_info, 0, sizeof(reply_data->ts_info)); 295 296 return ehdr; 297 } 298 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 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 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 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 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 dev = ctx->req_info->base.dev; 453 netdev_lock_ops(dev); 454 ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb); 455 netdev_unlock_ops(dev); 456 } else { 457 for_each_netdev_dump(net, dev, ctx->pos_ifindex) { 458 netdev_lock_ops(dev); 459 ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb); 460 netdev_unlock_ops(dev); 461 if (ret < 0 && ret != -EOPNOTSUPP) 462 break; 463 ctx->pos_phyindex = 0; 464 ctx->netdev_dump_done = false; 465 ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE; 466 } 467 } 468 rtnl_unlock(); 469 470 return ret; 471 } 472 473 int ethnl_tsinfo_start(struct netlink_callback *cb) 474 { 475 const struct genl_dumpit_info *info = genl_dumpit_info(cb); 476 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx; 477 struct nlattr **tb = info->info.attrs; 478 struct tsinfo_reply_data *reply_data; 479 struct tsinfo_req_info *req_info; 480 int ret; 481 482 BUILD_BUG_ON(sizeof(*ctx) > sizeof(cb->ctx)); 483 484 req_info = kzalloc(sizeof(*req_info), GFP_KERNEL); 485 if (!req_info) 486 return -ENOMEM; 487 reply_data = kzalloc(sizeof(*reply_data), GFP_KERNEL); 488 if (!reply_data) { 489 ret = -ENOMEM; 490 goto free_req_info; 491 } 492 493 ret = ethnl_parse_header_dev_get(&req_info->base, 494 tb[ETHTOOL_A_TSINFO_HEADER], 495 sock_net(cb->skb->sk), cb->extack, 496 false); 497 if (ret < 0) 498 goto free_reply_data; 499 500 ctx->req_info = req_info; 501 ctx->reply_data = reply_data; 502 ctx->pos_ifindex = 0; 503 ctx->pos_phyindex = 0; 504 ctx->netdev_dump_done = false; 505 ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE; 506 507 return 0; 508 509 free_reply_data: 510 kfree(reply_data); 511 free_req_info: 512 kfree(req_info); 513 514 return ret; 515 } 516 517 int ethnl_tsinfo_done(struct netlink_callback *cb) 518 { 519 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx; 520 struct tsinfo_req_info *req_info = ctx->req_info; 521 522 ethnl_parse_header_dev_put(&req_info->base); 523 kfree(ctx->reply_data); 524 kfree(ctx->req_info); 525 526 return 0; 527 } 528 529 const struct ethnl_request_ops ethnl_tsinfo_request_ops = { 530 .request_cmd = ETHTOOL_MSG_TSINFO_GET, 531 .reply_cmd = ETHTOOL_MSG_TSINFO_GET_REPLY, 532 .hdr_attr = ETHTOOL_A_TSINFO_HEADER, 533 .req_info_size = sizeof(struct tsinfo_req_info), 534 .reply_data_size = sizeof(struct tsinfo_reply_data), 535 536 .parse_request = tsinfo_parse_request, 537 .prepare_data = tsinfo_prepare_data, 538 .reply_size = tsinfo_reply_size, 539 .fill_reply = tsinfo_fill_reply, 540 }; 541