net/ethtool/tsinfo.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2
   3 #include <linux/net_tstamp.h>
   4
   5 #include "netlink.h"
   6 #include "common.h"
   7 #include "bitset.h"
   8
   9 struct tsinfo_req_info {
  10         struct ethnl_req_info           base;
  11 };
  12
  13 struct tsinfo_reply_data {
  14         struct ethnl_reply_data         base;
  15         struct kernel_ethtool_ts_info   ts_info;
  16         struct ethtool_ts_stats         stats;
  17 };
  18
  19 #define TSINFO_REPDATA(__reply_base) \
  20         container_of(__reply_base, struct tsinfo_reply_data, base)
  21
  22 #define ETHTOOL_TS_STAT_CNT \
  23         (__ETHTOOL_A_TS_STAT_CNT - (ETHTOOL_A_TS_STAT_UNSPEC + 1))
  24
  25 const struct nla_policy ethnl_tsinfo_get_policy[] = {
  26         [ETHTOOL_A_TSINFO_HEADER]               =
  27                 NLA_POLICY_NESTED(ethnl_header_policy_stats),
  28 };
  29
  30 static int tsinfo_prepare_data(const struct ethnl_req_info *req_base,
  31                                struct ethnl_reply_data *reply_base,
  32                                const struct genl_info *info)
  33 {
  34         struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
  35         struct net_device *dev = reply_base->dev;
  36         int ret;
  37
  38         ret = ethnl_ops_begin(dev);
  39         if (ret < 0)
  40                 return ret;
  41         if (req_base->flags & ETHTOOL_FLAG_STATS) {
  42                 ethtool_stats_init((u64 *)&data->stats,
  43                                    sizeof(data->stats) / sizeof(u64));
  44                 if (dev->ethtool_ops->get_ts_stats)
  45                         dev->ethtool_ops->get_ts_stats(dev, &data->stats);
  46         }
  47         ret = __ethtool_get_ts_info(dev, &data->ts_info);
  48         ethnl_ops_complete(dev);
  49
  50         return ret;
  51 }
  52
  53 static int tsinfo_reply_size(const struct ethnl_req_info *req_base,
  54                              const struct ethnl_reply_data *reply_base)
  55 {
  56         const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
  57         bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
  58         const struct kernel_ethtool_ts_info *ts_info = &data->ts_info;
  59         int len = 0;
  60         int ret;
  61
  62         BUILD_BUG_ON(__SOF_TIMESTAMPING_CNT > 32);
  63         BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32);
  64         BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32);
  65
  66         if (ts_info->so_timestamping) {
  67                 ret = ethnl_bitset32_size(&ts_info->so_timestamping, NULL,
  68                                           __SOF_TIMESTAMPING_CNT,
  69                                           sof_timestamping_names, compact);
  70                 if (ret < 0)
  71                         return ret;
  72                 len += ret;     /* _TSINFO_TIMESTAMPING */
  73         }
  74         if (ts_info->tx_types) {
  75                 ret = ethnl_bitset32_size(&ts_info->tx_types, NULL,
  76                                           __HWTSTAMP_TX_CNT,
  77                                           ts_tx_type_names, compact);
  78                 if (ret < 0)
  79                         return ret;
  80                 len += ret;     /* _TSINFO_TX_TYPES */
  81         }
  82         if (ts_info->rx_filters) {
  83                 ret = ethnl_bitset32_size(&ts_info->rx_filters, NULL,
  84                                           __HWTSTAMP_FILTER_CNT,
  85                                           ts_rx_filter_names, compact);
  86                 if (ret < 0)
  87                         return ret;
  88                 len += ret;     /* _TSINFO_RX_FILTERS */
  89         }
  90         if (ts_info->phc_index >= 0)
  91                 len += nla_total_size(sizeof(u32));     /* _TSINFO_PHC_INDEX */
  92         if (req_base->flags & ETHTOOL_FLAG_STATS)
  93                 len += nla_total_size(0) + /* _TSINFO_STATS */
  94                        nla_total_size_64bit(sizeof(u64)) * ETHTOOL_TS_STAT_CNT;
  95
  96         return len;
  97 }
  98
  99 static int tsinfo_put_stat(struct sk_buff *skb, u64 val, u16 attrtype)
 100 {
 101         if (val == ETHTOOL_STAT_NOT_SET)
 102                 return 0;
 103         if (nla_put_uint(skb, attrtype, val))
 104                 return -EMSGSIZE;
 105         return 0;
 106 }
 107
 108 static int tsinfo_put_stats(struct sk_buff *skb,
 109                             const struct ethtool_ts_stats *stats)
 110 {
 111         struct nlattr *nest;
 112
 113         nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_STATS);
 114         if (!nest)
 115                 return -EMSGSIZE;
 116
 117         if (tsinfo_put_stat(skb, stats->tx_stats.pkts,
 118                             ETHTOOL_A_TS_STAT_TX_PKTS) ||
 119             tsinfo_put_stat(skb, stats->tx_stats.lost,
 120                             ETHTOOL_A_TS_STAT_TX_LOST) ||
 121             tsinfo_put_stat(skb, stats->tx_stats.err,
 122                             ETHTOOL_A_TS_STAT_TX_ERR))
 123                 goto err_cancel;
 124
 125         nla_nest_end(skb, nest);
 126         return 0;
 127
 128 err_cancel:
 129         nla_nest_cancel(skb, nest);
 130         return -EMSGSIZE;
 131 }
 132
 133 static int tsinfo_fill_reply(struct sk_buff *skb,
 134                              const struct ethnl_req_info *req_base,
 135                              const struct ethnl_reply_data *reply_base)
 136 {
 137         const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
 138         bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
 139         const struct kernel_ethtool_ts_info *ts_info = &data->ts_info;
 140         int ret;
 141
 142         if (ts_info->so_timestamping) {
 143                 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TIMESTAMPING,
 144                                          &ts_info->so_timestamping, NULL,
 145                                          __SOF_TIMESTAMPING_CNT,
 146                                          sof_timestamping_names, compact);
 147                 if (ret < 0)
 148                         return ret;
 149         }
 150         if (ts_info->tx_types) {
 151                 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TX_TYPES,
 152                                          &ts_info->tx_types, NULL,
 153                                          __HWTSTAMP_TX_CNT,
 154                                          ts_tx_type_names, compact);
 155                 if (ret < 0)
 156                         return ret;
 157         }
 158         if (ts_info->rx_filters) {
 159                 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_RX_FILTERS,
 160                                          &ts_info->rx_filters, NULL,
 161                                          __HWTSTAMP_FILTER_CNT,
 162                                          ts_rx_filter_names, compact);
 163                 if (ret < 0)
 164                         return ret;
 165         }
 166         if (ts_info->phc_index >= 0 &&
 167             nla_put_u32(skb, ETHTOOL_A_TSINFO_PHC_INDEX, ts_info->phc_index))
 168                 return -EMSGSIZE;
 169         if (req_base->flags & ETHTOOL_FLAG_STATS &&
 170             tsinfo_put_stats(skb, &data->stats))
 171                 return -EMSGSIZE;
 172
 173         return 0;
 174 }
 175
 176 const struct ethnl_request_ops ethnl_tsinfo_request_ops = {
 177         .request_cmd            = ETHTOOL_MSG_TSINFO_GET,
 178         .reply_cmd              = ETHTOOL_MSG_TSINFO_GET_REPLY,
 179         .hdr_attr               = ETHTOOL_A_TSINFO_HEADER,
 180         .req_info_size          = sizeof(struct tsinfo_req_info),
 181         .reply_data_size        = sizeof(struct tsinfo_reply_data),
 182
 183         .prepare_data           = tsinfo_prepare_data,
 184         .reply_size             = tsinfo_reply_size,
 185         .fill_reply             = tsinfo_fill_reply,
 186 };