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
9struct tsinfo_req_info {
10	struct ethnl_req_info		base;
11};
12
13struct tsinfo_reply_data {
14	struct ethnl_reply_data		base;
15	struct 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
25const struct nla_policy ethnl_tsinfo_get_policy[] = {
26	[ETHTOOL_A_TSINFO_HEADER]		=
27		NLA_POLICY_NESTED(ethnl_header_policy_stats),
28};
29
30static 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
53static 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 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
99static 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
108static 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
128err_cancel:
129	nla_nest_cancel(skb, nest);
130	return -EMSGSIZE;
131}
132
133static 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 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
176const 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