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