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 ethtool_ts_info ts_info;
16 };
17
18 #define TSINFO_REPDATA(__reply_base) \
19 container_of(__reply_base, struct tsinfo_reply_data, base)
20
21 const struct nla_policy ethnl_tsinfo_get_policy[] = {
22 [ETHTOOL_A_TSINFO_HEADER] =
23 NLA_POLICY_NESTED(ethnl_header_policy),
24 };
25
tsinfo_prepare_data(const struct ethnl_req_info * req_base,struct ethnl_reply_data * reply_base,const struct genl_info * info)26 static int tsinfo_prepare_data(const struct ethnl_req_info *req_base,
27 struct ethnl_reply_data *reply_base,
28 const struct genl_info *info)
29 {
30 struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
31 struct net_device *dev = reply_base->dev;
32 int ret;
33
34 ret = ethnl_ops_begin(dev);
35 if (ret < 0)
36 return ret;
37 ret = __ethtool_get_ts_info(dev, &data->ts_info);
38 ethnl_ops_complete(dev);
39
40 return ret;
41 }
42
tsinfo_reply_size(const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)43 static int tsinfo_reply_size(const struct ethnl_req_info *req_base,
44 const struct ethnl_reply_data *reply_base)
45 {
46 const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
47 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
48 const struct ethtool_ts_info *ts_info = &data->ts_info;
49 int len = 0;
50 int ret;
51
52 BUILD_BUG_ON(__SOF_TIMESTAMPING_CNT > 32);
53 BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32);
54 BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32);
55
56 if (ts_info->so_timestamping) {
57 ret = ethnl_bitset32_size(&ts_info->so_timestamping, NULL,
58 __SOF_TIMESTAMPING_CNT,
59 sof_timestamping_names, compact);
60 if (ret < 0)
61 return ret;
62 len += ret; /* _TSINFO_TIMESTAMPING */
63 }
64 if (ts_info->tx_types) {
65 ret = ethnl_bitset32_size(&ts_info->tx_types, NULL,
66 __HWTSTAMP_TX_CNT,
67 ts_tx_type_names, compact);
68 if (ret < 0)
69 return ret;
70 len += ret; /* _TSINFO_TX_TYPES */
71 }
72 if (ts_info->rx_filters) {
73 ret = ethnl_bitset32_size(&ts_info->rx_filters, NULL,
74 __HWTSTAMP_FILTER_CNT,
75 ts_rx_filter_names, compact);
76 if (ret < 0)
77 return ret;
78 len += ret; /* _TSINFO_RX_FILTERS */
79 }
80 if (ts_info->phc_index >= 0)
81 len += nla_total_size(sizeof(u32)); /* _TSINFO_PHC_INDEX */
82
83 return len;
84 }
85
tsinfo_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)86 static int tsinfo_fill_reply(struct sk_buff *skb,
87 const struct ethnl_req_info *req_base,
88 const struct ethnl_reply_data *reply_base)
89 {
90 const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
91 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
92 const struct ethtool_ts_info *ts_info = &data->ts_info;
93 int ret;
94
95 if (ts_info->so_timestamping) {
96 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TIMESTAMPING,
97 &ts_info->so_timestamping, NULL,
98 __SOF_TIMESTAMPING_CNT,
99 sof_timestamping_names, compact);
100 if (ret < 0)
101 return ret;
102 }
103 if (ts_info->tx_types) {
104 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TX_TYPES,
105 &ts_info->tx_types, NULL,
106 __HWTSTAMP_TX_CNT,
107 ts_tx_type_names, compact);
108 if (ret < 0)
109 return ret;
110 }
111 if (ts_info->rx_filters) {
112 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_RX_FILTERS,
113 &ts_info->rx_filters, NULL,
114 __HWTSTAMP_FILTER_CNT,
115 ts_rx_filter_names, compact);
116 if (ret < 0)
117 return ret;
118 }
119 if (ts_info->phc_index >= 0 &&
120 nla_put_u32(skb, ETHTOOL_A_TSINFO_PHC_INDEX, ts_info->phc_index))
121 return -EMSGSIZE;
122
123 return 0;
124 }
125
126 const struct ethnl_request_ops ethnl_tsinfo_request_ops = {
127 .request_cmd = ETHTOOL_MSG_TSINFO_GET,
128 .reply_cmd = ETHTOOL_MSG_TSINFO_GET_REPLY,
129 .hdr_attr = ETHTOOL_A_TSINFO_HEADER,
130 .req_info_size = sizeof(struct tsinfo_req_info),
131 .reply_data_size = sizeof(struct tsinfo_reply_data),
132
133 .prepare_data = tsinfo_prepare_data,
134 .reply_size = tsinfo_reply_size,
135 .fill_reply = tsinfo_fill_reply,
136 };
137