Quelle  tsinfo.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0-only

#include <linux/net_tstamp.h>
#include <linux/phy.h>
#include <linux/phy_link_topology.h>
#include <linux/ptp_clock_kernel.h>
#include <net/netdev_lock.h>

#include "netlink.h"
#include "common.h"
#include "bitset.h"
#include "ts.h"

struct tsinfo_req_info {
 struct ethnl_req_info  base;
 struct hwtstamp_provider_desc hwprov_desc;
};

struct tsinfo_reply_data {
 struct ethnl_reply_data  base;
 struct kernel_ethtool_ts_info ts_info;
 struct ethtool_ts_stats  stats;
};

#define TSINFO_REQINFO(__req_base) \
 container_of(__req_base, struct tsinfo_req_info, base)

#define TSINFO_REPDATA(__reply_base) \
 container_of(__reply_base, struct tsinfo_reply_data, base)

#define ETHTOOL_TS_STAT_CNT \
 (__ETHTOOL_A_TS_STAT_CNT - (ETHTOOL_A_TS_STAT_UNSPEC + 1))

const struct nla_policy ethnl_tsinfo_get_policy[ETHTOOL_A_TSINFO_MAX + 1] = {
 [ETHTOOL_A_TSINFO_HEADER]  =
  NLA_POLICY_NESTED(ethnl_header_policy_stats),
 [ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER] =
  NLA_POLICY_NESTED(ethnl_ts_hwtst_prov_policy),
};

int ts_parse_hwtst_provider(const struct nlattr *nest,
       struct hwtstamp_provider_desc *hwprov_desc,
       struct netlink_ext_ack *extack,
       bool *mod)
{
 struct nlattr *tb[ARRAY_SIZE(ethnl_ts_hwtst_prov_policy)];
 int ret;

 ret = nla_parse_nested(tb,
          ARRAY_SIZE(ethnl_ts_hwtst_prov_policy) - 1,
          nest,
          ethnl_ts_hwtst_prov_policy, extack);
 if (ret < 0)
  return ret;

 if (NL_REQ_ATTR_CHECK(extack, nest, tb,
         ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX) ||
     NL_REQ_ATTR_CHECK(extack, nest, tb,
         ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER))
  return -EINVAL;

 ethnl_update_u32(&hwprov_desc->index,
    tb[ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX],
    mod);
 ethnl_update_u32(&hwprov_desc->qualifier,
    tb[ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER],
    mod);

 return 0;
}

static int
tsinfo_parse_request(struct ethnl_req_info *req_base, struct nlattr **tb,
       struct netlink_ext_ack *extack)
{
 struct tsinfo_req_info *req = TSINFO_REQINFO(req_base);
 bool mod = false;

 req->hwprov_desc.index = -1;

 if (!tb[ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER])
  return 0;

 return ts_parse_hwtst_provider(tb[ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER],
           &req->hwprov_desc, extack, &mod);
}

static int tsinfo_prepare_data(const struct ethnl_req_info *req_base,
          struct ethnl_reply_data *reply_base,
          const struct genl_info *info)
{
 struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
 struct tsinfo_req_info *req = TSINFO_REQINFO(req_base);
 struct net_device *dev = reply_base->dev;
 int ret;

 ret = ethnl_ops_begin(dev);
 if (ret < 0)
  return ret;

 if (req->hwprov_desc.index != -1) {
  ret = ethtool_get_ts_info_by_phc(dev, &data->ts_info,
       &req->hwprov_desc);
  ethnl_ops_complete(dev);
  return ret;
 }

 if (req_base->flags & ETHTOOL_FLAG_STATS) {
  ethtool_stats_init((u64 *)&data->stats,
       sizeof(data->stats) / sizeof(u64));
  if (dev->ethtool_ops->get_ts_stats)
   dev->ethtool_ops->get_ts_stats(dev, &data->stats);
 }

 ret = __ethtool_get_ts_info(dev, &data->ts_info);
 ethnl_ops_complete(dev);

 return ret;
}

static int tsinfo_reply_size(const struct ethnl_req_info *req_base,
        const struct ethnl_reply_data *reply_base)
{
 const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
 const struct kernel_ethtool_ts_info *ts_info = &data->ts_info;
 int len = 0;
 int ret;

 BUILD_BUG_ON(__SOF_TIMESTAMPING_CNT > 32);
 BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32);
 BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32);

 if (ts_info->so_timestamping) {
  ret = ethnl_bitset32_size(&ts_info->so_timestamping, NULL,
       __SOF_TIMESTAMPING_CNT,
       sof_timestamping_names, compact);
  if (ret < 0)
   return ret;
  len += ret; /* _TSINFO_TIMESTAMPING */
 }
 if (ts_info->tx_types) {
  ret = ethnl_bitset32_size(&ts_info->tx_types, NULL,
       __HWTSTAMP_TX_CNT,
       ts_tx_type_names, compact);
  if (ret < 0)
   return ret;
  len += ret; /* _TSINFO_TX_TYPES */
 }
 if (ts_info->rx_filters) {
  ret = ethnl_bitset32_size(&ts_info->rx_filters, NULL,
       __HWTSTAMP_FILTER_CNT,
       ts_rx_filter_names, compact);
  if (ret < 0)
   return ret;
  len += ret; /* _TSINFO_RX_FILTERS */
 }
 if (ts_info->phc_index >= 0) {
  len += nla_total_size(sizeof(u32)); /* _TSINFO_PHC_INDEX */
  /* _TSINFO_HWTSTAMP_PROVIDER */
  len += nla_total_size(0) + 2 * nla_total_size(sizeof(u32));
 }
 if (ts_info->phc_source) {
  len += nla_total_size(sizeof(u32)); /* _TSINFO_HWTSTAMP_SOURCE */
  if (ts_info->phc_phyindex)
   /* _TSINFO_HWTSTAMP_PHYINDEX */
   len += nla_total_size(sizeof(u32));
 }
 if (req_base->flags & ETHTOOL_FLAG_STATS)
  len += nla_total_size(0) + /* _TSINFO_STATS */
         nla_total_size_64bit(sizeof(u64)) * ETHTOOL_TS_STAT_CNT;

 return len;
}

static int tsinfo_put_stat(struct sk_buff *skb, u64 val, u16 attrtype)
{
 if (val == ETHTOOL_STAT_NOT_SET)
  return 0;
 if (nla_put_uint(skb, attrtype, val))
  return -EMSGSIZE;
 return 0;
}

static int tsinfo_put_stats(struct sk_buff *skb,
       const struct ethtool_ts_stats *stats)
{
 struct nlattr *nest;

 nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_STATS);
 if (!nest)
  return -EMSGSIZE;

 if (tsinfo_put_stat(skb, stats->tx_stats.pkts,
       ETHTOOL_A_TS_STAT_TX_PKTS) ||
     tsinfo_put_stat(skb, stats->tx_stats.onestep_pkts_unconfirmed,
       ETHTOOL_A_TS_STAT_TX_ONESTEP_PKTS_UNCONFIRMED) ||
     tsinfo_put_stat(skb, stats->tx_stats.lost,
       ETHTOOL_A_TS_STAT_TX_LOST) ||
     tsinfo_put_stat(skb, stats->tx_stats.err,
       ETHTOOL_A_TS_STAT_TX_ERR))
  goto err_cancel;

 nla_nest_end(skb, nest);
 return 0;

err_cancel:
 nla_nest_cancel(skb, nest);
 return -EMSGSIZE;
}

static int tsinfo_fill_reply(struct sk_buff *skb,
        const struct ethnl_req_info *req_base,
        const struct ethnl_reply_data *reply_base)
{
 const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
 const struct kernel_ethtool_ts_info *ts_info = &data->ts_info;
 int ret;

 if (ts_info->so_timestamping) {
  ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TIMESTAMPING,
      &ts_info->so_timestamping, NULL,
      __SOF_TIMESTAMPING_CNT,
      sof_timestamping_names, compact);
  if (ret < 0)
   return ret;
 }
 if (ts_info->tx_types) {
  ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TX_TYPES,
      &ts_info->tx_types, NULL,
      __HWTSTAMP_TX_CNT,
      ts_tx_type_names, compact);
  if (ret < 0)
   return ret;
 }
 if (ts_info->rx_filters) {
  ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_RX_FILTERS,
      &ts_info->rx_filters, NULL,
      __HWTSTAMP_FILTER_CNT,
      ts_rx_filter_names, compact);
  if (ret < 0)
   return ret;
 }
 if (ts_info->phc_index >= 0) {
  struct nlattr *nest;

  ret = nla_put_u32(skb, ETHTOOL_A_TSINFO_PHC_INDEX,
      ts_info->phc_index);
  if (ret)
   return -EMSGSIZE;

  nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER);
  if (!nest)
   return -EMSGSIZE;

  if (nla_put_u32(skb, ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX,
    ts_info->phc_index) ||
      nla_put_u32(skb,
    ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER,
    ts_info->phc_qualifier)) {
   nla_nest_cancel(skb, nest);
   return -EMSGSIZE;
  }

  nla_nest_end(skb, nest);
 }
 if (ts_info->phc_source) {
  if (nla_put_u32(skb, ETHTOOL_A_TSINFO_HWTSTAMP_SOURCE,
    ts_info->phc_source))
   return -EMSGSIZE;

  if (ts_info->phc_phyindex &&
      nla_put_u32(skb, ETHTOOL_A_TSINFO_HWTSTAMP_PHYINDEX,
    ts_info->phc_phyindex))
   return -EMSGSIZE;
 }
 if (req_base->flags & ETHTOOL_FLAG_STATS &&
     tsinfo_put_stats(skb, &data->stats))
  return -EMSGSIZE;

 return 0;
}

struct ethnl_tsinfo_dump_ctx {
 struct tsinfo_req_info  *req_info;
 struct tsinfo_reply_data *reply_data;
 unsigned long   pos_ifindex;
 bool    netdev_dump_done;
 unsigned long   pos_phyindex;
 enum hwtstamp_provider_qualifier pos_phcqualifier;
};

static void *ethnl_tsinfo_prepare_dump(struct sk_buff *skb,
           struct net_device *dev,
           struct tsinfo_reply_data *reply_data,
           struct netlink_callback *cb)
{
 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
 void *ehdr = NULL;

 ehdr = ethnl_dump_put(skb, cb,
         ETHTOOL_MSG_TSINFO_GET_REPLY);
 if (!ehdr)
  return ERR_PTR(-EMSGSIZE);

 reply_data = ctx->reply_data;
 memset(reply_data, 0, sizeof(*reply_data));
 reply_data->base.dev = dev;
 reply_data->ts_info.cmd = ETHTOOL_GET_TS_INFO;
 reply_data->ts_info.phc_index = -1;

 return ehdr;
}

static int ethnl_tsinfo_end_dump(struct sk_buff *skb,
     struct net_device *dev,
     struct tsinfo_req_info *req_info,
     struct tsinfo_reply_data *reply_data,
     void *ehdr)
{
 int ret;

 reply_data->ts_info.so_timestamping |= SOF_TIMESTAMPING_RX_SOFTWARE |
            SOF_TIMESTAMPING_SOFTWARE;

 ret = ethnl_fill_reply_header(skb, dev, ETHTOOL_A_TSINFO_HEADER);
 if (ret < 0)
  return ret;

 ret = tsinfo_fill_reply(skb, &req_info->base, &reply_data->base);
 if (ret < 0)
  return ret;

 reply_data->base.dev = NULL;
 genlmsg_end(skb, ehdr);

 return ret;
}

static int ethnl_tsinfo_dump_one_phydev(struct sk_buff *skb,
     struct net_device *dev,
     struct phy_device *phydev,
     struct netlink_callback *cb)
{
 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
 struct tsinfo_reply_data *reply_data;
 struct tsinfo_req_info *req_info;
 void *ehdr = NULL;
 int ret = 0;

 if (!phy_has_tsinfo(phydev))
  return -EOPNOTSUPP;

 reply_data = ctx->reply_data;
 req_info = ctx->req_info;
 ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb);
 if (IS_ERR(ehdr))
  return PTR_ERR(ehdr);

 ret = phy_ts_info(phydev, &reply_data->ts_info);
 if (ret < 0)
  goto err;

 if (reply_data->ts_info.phc_index >= 0) {
  reply_data->ts_info.phc_source = HWTSTAMP_SOURCE_PHYLIB;
  reply_data->ts_info.phc_phyindex = phydev->phyindex;
 }

 ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data, ehdr);
 if (ret < 0)
  goto err;

 return ret;
err:
 genlmsg_cancel(skb, ehdr);
 return ret;
}

static int ethnl_tsinfo_dump_one_netdev(struct sk_buff *skb,
     struct net_device *dev,
     struct netlink_callback *cb)
{
 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
 const struct ethtool_ops *ops = dev->ethtool_ops;
 struct tsinfo_reply_data *reply_data;
 struct tsinfo_req_info *req_info;
 void *ehdr = NULL;
 int ret = 0;

 if (!ops->get_ts_info)
  return -EOPNOTSUPP;

 reply_data = ctx->reply_data;
 req_info = ctx->req_info;
 for (; ctx->pos_phcqualifier < HWTSTAMP_PROVIDER_QUALIFIER_CNT;
      ctx->pos_phcqualifier++) {
  if (!net_support_hwtstamp_qualifier(dev,
          ctx->pos_phcqualifier))
   continue;

  ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb);
  if (IS_ERR(ehdr)) {
   ret = PTR_ERR(ehdr);
   goto err;
  }

  reply_data->ts_info.phc_qualifier = ctx->pos_phcqualifier;
  ret = ops->get_ts_info(dev, &reply_data->ts_info);
  if (ret < 0)
   goto err;

  if (reply_data->ts_info.phc_index >= 0)
   reply_data->ts_info.phc_source = HWTSTAMP_SOURCE_NETDEV;
  ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data,
         ehdr);
  if (ret < 0)
   goto err;
 }

 return ret;

err:
 genlmsg_cancel(skb, ehdr);
 return ret;
}

static int ethnl_tsinfo_dump_one_net_topo(struct sk_buff *skb,
       struct net_device *dev,
       struct netlink_callback *cb)
{
 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
 struct phy_device_node *pdn;
 int ret = 0;

 if (!ctx->netdev_dump_done) {
  ret = ethnl_tsinfo_dump_one_netdev(skb, dev, cb);
  if (ret < 0 && ret != -EOPNOTSUPP)
   return ret;
  ctx->netdev_dump_done = true;
 }

 if (!dev->link_topo) {
  if (phy_has_tsinfo(dev->phydev)) {
   ret = ethnl_tsinfo_dump_one_phydev(skb, dev,
          dev->phydev, cb);
   if (ret < 0 && ret != -EOPNOTSUPP)
    return ret;
  }

  return 0;
 }

 xa_for_each_start(&dev->link_topo->phys, ctx->pos_phyindex, pdn,
     ctx->pos_phyindex) {
  if (phy_has_tsinfo(pdn->phy)) {
   ret = ethnl_tsinfo_dump_one_phydev(skb, dev,
          pdn->phy, cb);
   if (ret < 0 && ret != -EOPNOTSUPP)
    return ret;
  }
 }

 return ret;
}

int ethnl_tsinfo_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
{
 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
 struct net *net = sock_net(skb->sk);
 struct net_device *dev;
 int ret = 0;

 rtnl_lock();
 if (ctx->req_info->base.dev) {
  dev = ctx->req_info->base.dev;
  netdev_lock_ops(dev);
  ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb);
  netdev_unlock_ops(dev);
 } else {
  for_each_netdev_dump(net, dev, ctx->pos_ifindex) {
   netdev_lock_ops(dev);
   ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb);
   netdev_unlock_ops(dev);
   if (ret < 0 && ret != -EOPNOTSUPP)
    break;
   ctx->pos_phyindex = 0;
   ctx->netdev_dump_done = false;
   ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE;
  }
 }
 rtnl_unlock();

 return ret;
}

int ethnl_tsinfo_start(struct netlink_callback *cb)
{
 const struct genl_dumpit_info *info = genl_dumpit_info(cb);
 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
 struct nlattr **tb = info->info.attrs;
 struct tsinfo_reply_data *reply_data;
 struct tsinfo_req_info *req_info;
 int ret;

 BUILD_BUG_ON(sizeof(*ctx) > sizeof(cb->ctx));

 req_info = kzalloc(sizeof(*req_info), GFP_KERNEL);
 if (!req_info)
  return -ENOMEM;
 reply_data = kzalloc(sizeof(*reply_data), GFP_KERNEL);
 if (!reply_data) {
  ret = -ENOMEM;
  goto free_req_info;
 }

 ret = ethnl_parse_header_dev_get(&req_info->base,
      tb[ETHTOOL_A_TSINFO_HEADER],
      sock_net(cb->skb->sk), cb->extack,
      false);
 if (ret < 0)
  goto free_reply_data;

 ctx->req_info = req_info;
 ctx->reply_data = reply_data;
 ctx->pos_ifindex = 0;
 ctx->pos_phyindex = 0;
 ctx->netdev_dump_done = false;
 ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE;

 return 0;

free_reply_data:
 kfree(reply_data);
free_req_info:
 kfree(req_info);

 return ret;
}

int ethnl_tsinfo_done(struct netlink_callback *cb)
{
 struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
 struct tsinfo_req_info *req_info = ctx->req_info;

 ethnl_parse_header_dev_put(&req_info->base);
 kfree(ctx->reply_data);
 kfree(ctx->req_info);

 return 0;
}

const struct ethnl_request_ops ethnl_tsinfo_request_ops = {
 .request_cmd  = ETHTOOL_MSG_TSINFO_GET,
 .reply_cmd  = ETHTOOL_MSG_TSINFO_GET_REPLY,
 .hdr_attr  = ETHTOOL_A_TSINFO_HEADER,
 .req_info_size  = sizeof(struct tsinfo_req_info),
 .reply_data_size = sizeof(struct tsinfo_reply_data),

 .parse_request  = tsinfo_parse_request,
 .prepare_data  = tsinfo_prepare_data,
 .reply_size  = tsinfo_reply_size,
 .fill_reply  = tsinfo_fill_reply,
};