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 }; 187