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.lost, 190 ETHTOOL_A_TS_STAT_TX_LOST) || 191 tsinfo_put_stat(skb, stats->tx_stats.err, 192 ETHTOOL_A_TS_STAT_TX_ERR)) 193 goto err_cancel; 194 195 nla_nest_end(skb, nest); 196 return 0; 197 198 err_cancel: 199 nla_nest_cancel(skb, nest); 200 return -EMSGSIZE; 201 } 202 203 static int tsinfo_fill_reply(struct sk_buff *skb, 204 const struct ethnl_req_info *req_base, 205 const struct ethnl_reply_data *reply_base) 206 { 207 const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base); 208 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; 209 const struct kernel_ethtool_ts_info *ts_info = &data->ts_info; 210 int ret; 211 212 if (ts_info->so_timestamping) { 213 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TIMESTAMPING, 214 &ts_info->so_timestamping, NULL, 215 __SOF_TIMESTAMPING_CNT, 216 sof_timestamping_names, compact); 217 if (ret < 0) 218 return ret; 219 } 220 if (ts_info->tx_types) { 221 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TX_TYPES, 222 &ts_info->tx_types, NULL, 223 __HWTSTAMP_TX_CNT, 224 ts_tx_type_names, compact); 225 if (ret < 0) 226 return ret; 227 } 228 if (ts_info->rx_filters) { 229 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_RX_FILTERS, 230 &ts_info->rx_filters, NULL, 231 __HWTSTAMP_FILTER_CNT, 232 ts_rx_filter_names, compact); 233 if (ret < 0) 234 return ret; 235 } 236 if (ts_info->phc_index >= 0) { 237 struct nlattr *nest; 238 239 ret = nla_put_u32(skb, ETHTOOL_A_TSINFO_PHC_INDEX, 240 ts_info->phc_index); 241 if (ret) 242 return -EMSGSIZE; 243 244 nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER); 245 if (!nest) 246 return -EMSGSIZE; 247 248 if (nla_put_u32(skb, ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX, 249 ts_info->phc_index) || 250 nla_put_u32(skb, 251 ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER, 252 ts_info->phc_qualifier)) { 253 nla_nest_cancel(skb, nest); 254 return -EMSGSIZE; 255 } 256 257 nla_nest_end(skb, nest); 258 } 259 if (req_base->flags & ETHTOOL_FLAG_STATS && 260 tsinfo_put_stats(skb, &data->stats)) 261 return -EMSGSIZE; 262 263 return 0; 264 } 265 266 struct ethnl_tsinfo_dump_ctx { 267 struct tsinfo_req_info *req_info; 268 struct tsinfo_reply_data *reply_data; 269 unsigned long pos_ifindex; 270 bool netdev_dump_done; 271 unsigned long pos_phyindex; 272 enum hwtstamp_provider_qualifier pos_phcqualifier; 273 }; 274 275 static void *ethnl_tsinfo_prepare_dump(struct sk_buff *skb, 276 struct net_device *dev, 277 struct tsinfo_reply_data *reply_data, 278 struct netlink_callback *cb) 279 { 280 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx; 281 void *ehdr = NULL; 282 283 ehdr = ethnl_dump_put(skb, cb, 284 ETHTOOL_MSG_TSINFO_GET_REPLY); 285 if (!ehdr) 286 return ERR_PTR(-EMSGSIZE); 287 288 reply_data = ctx->reply_data; 289 memset(reply_data, 0, sizeof(*reply_data)); 290 reply_data->base.dev = dev; 291 memset(&reply_data->ts_info, 0, sizeof(reply_data->ts_info)); 292 293 return ehdr; 294 } 295 296 static int ethnl_tsinfo_end_dump(struct sk_buff *skb, 297 struct net_device *dev, 298 struct tsinfo_req_info *req_info, 299 struct tsinfo_reply_data *reply_data, 300 void *ehdr) 301 { 302 int ret; 303 304 reply_data->ts_info.so_timestamping |= SOF_TIMESTAMPING_RX_SOFTWARE | 305 SOF_TIMESTAMPING_SOFTWARE; 306 307 ret = ethnl_fill_reply_header(skb, dev, ETHTOOL_A_TSINFO_HEADER); 308 if (ret < 0) 309 return ret; 310 311 ret = tsinfo_fill_reply(skb, &req_info->base, &reply_data->base); 312 if (ret < 0) 313 return ret; 314 315 reply_data->base.dev = NULL; 316 genlmsg_end(skb, ehdr); 317 318 return ret; 319 } 320 321 static int ethnl_tsinfo_dump_one_phydev(struct sk_buff *skb, 322 struct net_device *dev, 323 struct phy_device *phydev, 324 struct netlink_callback *cb) 325 { 326 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx; 327 struct tsinfo_reply_data *reply_data; 328 struct tsinfo_req_info *req_info; 329 void *ehdr = NULL; 330 int ret = 0; 331 332 if (!phy_has_tsinfo(phydev)) 333 return -EOPNOTSUPP; 334 335 reply_data = ctx->reply_data; 336 req_info = ctx->req_info; 337 ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb); 338 if (IS_ERR(ehdr)) 339 return PTR_ERR(ehdr); 340 341 ret = phy_ts_info(phydev, &reply_data->ts_info); 342 if (ret < 0) 343 goto err; 344 345 ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data, ehdr); 346 if (ret < 0) 347 goto err; 348 349 return ret; 350 err: 351 genlmsg_cancel(skb, ehdr); 352 return ret; 353 } 354 355 static int ethnl_tsinfo_dump_one_netdev(struct sk_buff *skb, 356 struct net_device *dev, 357 struct netlink_callback *cb) 358 { 359 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx; 360 const struct ethtool_ops *ops = dev->ethtool_ops; 361 struct tsinfo_reply_data *reply_data; 362 struct tsinfo_req_info *req_info; 363 void *ehdr = NULL; 364 int ret = 0; 365 366 if (!ops->get_ts_info) 367 return -EOPNOTSUPP; 368 369 reply_data = ctx->reply_data; 370 req_info = ctx->req_info; 371 for (; ctx->pos_phcqualifier < HWTSTAMP_PROVIDER_QUALIFIER_CNT; 372 ctx->pos_phcqualifier++) { 373 if (!net_support_hwtstamp_qualifier(dev, 374 ctx->pos_phcqualifier)) 375 continue; 376 377 ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb); 378 if (IS_ERR(ehdr)) { 379 ret = PTR_ERR(ehdr); 380 goto err; 381 } 382 383 reply_data->ts_info.phc_qualifier = ctx->pos_phcqualifier; 384 ret = ops->get_ts_info(dev, &reply_data->ts_info); 385 if (ret < 0) 386 goto err; 387 388 ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data, 389 ehdr); 390 if (ret < 0) 391 goto err; 392 } 393 394 return ret; 395 396 err: 397 genlmsg_cancel(skb, ehdr); 398 return ret; 399 } 400 401 static int ethnl_tsinfo_dump_one_net_topo(struct sk_buff *skb, 402 struct net_device *dev, 403 struct netlink_callback *cb) 404 { 405 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx; 406 struct phy_device_node *pdn; 407 int ret = 0; 408 409 if (!ctx->netdev_dump_done) { 410 ret = ethnl_tsinfo_dump_one_netdev(skb, dev, cb); 411 if (ret < 0 && ret != -EOPNOTSUPP) 412 return ret; 413 ctx->netdev_dump_done = true; 414 } 415 416 if (!dev->link_topo) { 417 if (phy_has_tsinfo(dev->phydev)) { 418 ret = ethnl_tsinfo_dump_one_phydev(skb, dev, 419 dev->phydev, cb); 420 if (ret < 0 && ret != -EOPNOTSUPP) 421 return ret; 422 } 423 424 return 0; 425 } 426 427 xa_for_each_start(&dev->link_topo->phys, ctx->pos_phyindex, pdn, 428 ctx->pos_phyindex) { 429 if (phy_has_tsinfo(pdn->phy)) { 430 ret = ethnl_tsinfo_dump_one_phydev(skb, dev, 431 pdn->phy, cb); 432 if (ret < 0 && ret != -EOPNOTSUPP) 433 return ret; 434 } 435 } 436 437 return ret; 438 } 439 440 int ethnl_tsinfo_dumpit(struct sk_buff *skb, struct netlink_callback *cb) 441 { 442 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx; 443 struct net *net = sock_net(skb->sk); 444 struct net_device *dev; 445 int ret = 0; 446 447 rtnl_lock(); 448 if (ctx->req_info->base.dev) { 449 ret = ethnl_tsinfo_dump_one_net_topo(skb, 450 ctx->req_info->base.dev, 451 cb); 452 } else { 453 for_each_netdev_dump(net, dev, ctx->pos_ifindex) { 454 ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb); 455 if (ret < 0 && ret != -EOPNOTSUPP) 456 break; 457 ctx->pos_phyindex = 0; 458 ctx->netdev_dump_done = false; 459 ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE; 460 } 461 } 462 rtnl_unlock(); 463 464 return ret; 465 } 466 467 int ethnl_tsinfo_start(struct netlink_callback *cb) 468 { 469 const struct genl_dumpit_info *info = genl_dumpit_info(cb); 470 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx; 471 struct nlattr **tb = info->info.attrs; 472 struct tsinfo_reply_data *reply_data; 473 struct tsinfo_req_info *req_info; 474 int ret; 475 476 BUILD_BUG_ON(sizeof(*ctx) > sizeof(cb->ctx)); 477 478 req_info = kzalloc(sizeof(*req_info), GFP_KERNEL); 479 if (!req_info) 480 return -ENOMEM; 481 reply_data = kzalloc(sizeof(*reply_data), GFP_KERNEL); 482 if (!reply_data) { 483 ret = -ENOMEM; 484 goto free_req_info; 485 } 486 487 ret = ethnl_parse_header_dev_get(&req_info->base, 488 tb[ETHTOOL_A_TSINFO_HEADER], 489 sock_net(cb->skb->sk), cb->extack, 490 false); 491 if (ret < 0) 492 goto free_reply_data; 493 494 ctx->req_info = req_info; 495 ctx->reply_data = reply_data; 496 ctx->pos_ifindex = 0; 497 ctx->pos_phyindex = 0; 498 ctx->netdev_dump_done = false; 499 ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE; 500 501 return 0; 502 503 free_reply_data: 504 kfree(reply_data); 505 free_req_info: 506 kfree(req_info); 507 508 return ret; 509 } 510 511 int ethnl_tsinfo_done(struct netlink_callback *cb) 512 { 513 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx; 514 struct tsinfo_req_info *req_info = ctx->req_info; 515 516 ethnl_parse_header_dev_put(&req_info->base); 517 kfree(ctx->reply_data); 518 kfree(ctx->req_info); 519 520 return 0; 521 } 522 523 const struct ethnl_request_ops ethnl_tsinfo_request_ops = { 524 .request_cmd = ETHTOOL_MSG_TSINFO_GET, 525 .reply_cmd = ETHTOOL_MSG_TSINFO_GET_REPLY, 526 .hdr_attr = ETHTOOL_A_TSINFO_HEADER, 527 .req_info_size = sizeof(struct tsinfo_req_info), 528 .reply_data_size = sizeof(struct tsinfo_reply_data), 529 530 .parse_request = tsinfo_parse_request, 531 .prepare_data = tsinfo_prepare_data, 532 .reply_size = tsinfo_reply_size, 533 .fill_reply = tsinfo_fill_reply, 534 }; 535