1 // SPDX-License-Identifier: GPL-2.0-only
2
3 #include <linux/net_tstamp.h>
4 #include <linux/ptp_clock_kernel.h>
5
6 #include "netlink.h"
7 #include "common.h"
8 #include "bitset.h"
9 #include "../core/dev.h"
10 #include "ts.h"
11
12 struct tsconfig_req_info {
13 struct ethnl_req_info base;
14 };
15
16 struct tsconfig_reply_data {
17 struct ethnl_reply_data base;
18 struct hwtstamp_provider_desc hwprov_desc;
19 struct {
20 u32 tx_type;
21 u32 rx_filter;
22 u32 flags;
23 } hwtst_config;
24 };
25
26 #define TSCONFIG_REPDATA(__reply_base) \
27 container_of(__reply_base, struct tsconfig_reply_data, base)
28
29 const struct nla_policy ethnl_tsconfig_get_policy[ETHTOOL_A_TSCONFIG_HEADER + 1] = {
30 [ETHTOOL_A_TSCONFIG_HEADER] =
31 NLA_POLICY_NESTED(ethnl_header_policy),
32 };
33
tsconfig_prepare_data(const struct ethnl_req_info * req_base,struct ethnl_reply_data * reply_base,const struct genl_info * info)34 static int tsconfig_prepare_data(const struct ethnl_req_info *req_base,
35 struct ethnl_reply_data *reply_base,
36 const struct genl_info *info)
37 {
38 struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base);
39 struct hwtstamp_provider *hwprov = NULL;
40 struct net_device *dev = reply_base->dev;
41 struct kernel_hwtstamp_config cfg = {};
42 int ret;
43
44 if (!dev->netdev_ops->ndo_hwtstamp_get)
45 return -EOPNOTSUPP;
46
47 ret = ethnl_ops_begin(dev);
48 if (ret < 0)
49 return ret;
50
51 ret = dev_get_hwtstamp_phylib(dev, &cfg);
52 if (ret)
53 goto out;
54
55 data->hwtst_config.tx_type = BIT(cfg.tx_type);
56 data->hwtst_config.rx_filter = BIT(cfg.rx_filter);
57 data->hwtst_config.flags = cfg.flags;
58
59 data->hwprov_desc.index = -1;
60 hwprov = rtnl_dereference(dev->hwprov);
61 if (hwprov) {
62 data->hwprov_desc.index = hwprov->desc.index;
63 data->hwprov_desc.qualifier = hwprov->desc.qualifier;
64 } else {
65 struct kernel_ethtool_ts_info ts_info = {};
66
67 ts_info.phc_index = -1;
68 ret = __ethtool_get_ts_info(dev, &ts_info);
69 if (ret)
70 goto out;
71
72 if (ts_info.phc_index == -1)
73 return -ENODEV;
74
75 data->hwprov_desc.index = ts_info.phc_index;
76 data->hwprov_desc.qualifier = ts_info.phc_qualifier;
77 }
78
79 out:
80 ethnl_ops_complete(dev);
81 return ret;
82 }
83
tsconfig_reply_size(const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)84 static int tsconfig_reply_size(const struct ethnl_req_info *req_base,
85 const struct ethnl_reply_data *reply_base)
86 {
87 const struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base);
88 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
89 int len = 0;
90 int ret;
91
92 BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32);
93 BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32);
94 BUILD_BUG_ON(__HWTSTAMP_FLAG_CNT > 32);
95
96 if (data->hwtst_config.flags) {
97 ret = ethnl_bitset32_size(&data->hwtst_config.flags,
98 NULL, __HWTSTAMP_FLAG_CNT,
99 ts_flags_names, compact);
100 if (ret < 0)
101 return ret;
102 len += ret; /* _TSCONFIG_HWTSTAMP_FLAGS */
103 }
104
105 if (data->hwtst_config.tx_type) {
106 ret = ethnl_bitset32_size(&data->hwtst_config.tx_type,
107 NULL, __HWTSTAMP_TX_CNT,
108 ts_tx_type_names, compact);
109 if (ret < 0)
110 return ret;
111 len += ret; /* _TSCONFIG_TX_TYPES */
112 }
113 if (data->hwtst_config.rx_filter) {
114 ret = ethnl_bitset32_size(&data->hwtst_config.rx_filter,
115 NULL, __HWTSTAMP_FILTER_CNT,
116 ts_rx_filter_names, compact);
117 if (ret < 0)
118 return ret;
119 len += ret; /* _TSCONFIG_RX_FILTERS */
120 }
121
122 if (data->hwprov_desc.index >= 0)
123 /* _TSCONFIG_HWTSTAMP_PROVIDER */
124 len += nla_total_size(0) +
125 2 * nla_total_size(sizeof(u32));
126
127 return len;
128 }
129
tsconfig_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)130 static int tsconfig_fill_reply(struct sk_buff *skb,
131 const struct ethnl_req_info *req_base,
132 const struct ethnl_reply_data *reply_base)
133 {
134 const struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base);
135 bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
136 int ret;
137
138 if (data->hwtst_config.flags) {
139 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS,
140 &data->hwtst_config.flags, NULL,
141 __HWTSTAMP_FLAG_CNT,
142 ts_flags_names, compact);
143 if (ret < 0)
144 return ret;
145 }
146
147 if (data->hwtst_config.tx_type) {
148 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_TX_TYPES,
149 &data->hwtst_config.tx_type, NULL,
150 __HWTSTAMP_TX_CNT,
151 ts_tx_type_names, compact);
152 if (ret < 0)
153 return ret;
154 }
155
156 if (data->hwtst_config.rx_filter) {
157 ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_RX_FILTERS,
158 &data->hwtst_config.rx_filter,
159 NULL, __HWTSTAMP_FILTER_CNT,
160 ts_rx_filter_names, compact);
161 if (ret < 0)
162 return ret;
163 }
164
165 if (data->hwprov_desc.index >= 0) {
166 struct nlattr *nest;
167
168 nest = nla_nest_start(skb, ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER);
169 if (!nest)
170 return -EMSGSIZE;
171
172 if (nla_put_u32(skb, ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX,
173 data->hwprov_desc.index) ||
174 nla_put_u32(skb,
175 ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER,
176 data->hwprov_desc.qualifier)) {
177 nla_nest_cancel(skb, nest);
178 return -EMSGSIZE;
179 }
180
181 nla_nest_end(skb, nest);
182 }
183 return 0;
184 }
185
186 /* TSCONFIG_SET */
187 const struct nla_policy ethnl_tsconfig_set_policy[ETHTOOL_A_TSCONFIG_MAX + 1] = {
188 [ETHTOOL_A_TSCONFIG_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
189 [ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER] =
190 NLA_POLICY_NESTED(ethnl_ts_hwtst_prov_policy),
191 [ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS] = { .type = NLA_NESTED },
192 [ETHTOOL_A_TSCONFIG_RX_FILTERS] = { .type = NLA_NESTED },
193 [ETHTOOL_A_TSCONFIG_TX_TYPES] = { .type = NLA_NESTED },
194 };
195
tsconfig_send_reply(struct net_device * dev,struct genl_info * info)196 static int tsconfig_send_reply(struct net_device *dev, struct genl_info *info)
197 {
198 struct tsconfig_reply_data *reply_data;
199 struct tsconfig_req_info *req_info;
200 struct sk_buff *rskb;
201 void *reply_payload;
202 int reply_len = 0;
203 int ret;
204
205 req_info = kzalloc(sizeof(*req_info), GFP_KERNEL);
206 if (!req_info)
207 return -ENOMEM;
208 reply_data = kmalloc(sizeof(*reply_data), GFP_KERNEL);
209 if (!reply_data) {
210 kfree(req_info);
211 return -ENOMEM;
212 }
213
214 ASSERT_RTNL();
215 reply_data->base.dev = dev;
216 ret = tsconfig_prepare_data(&req_info->base, &reply_data->base, info);
217 if (ret < 0)
218 goto err_cleanup;
219
220 ret = tsconfig_reply_size(&req_info->base, &reply_data->base);
221 if (ret < 0)
222 goto err_cleanup;
223
224 reply_len = ret + ethnl_reply_header_size();
225 rskb = ethnl_reply_init(reply_len, dev, ETHTOOL_MSG_TSCONFIG_SET_REPLY,
226 ETHTOOL_A_TSCONFIG_HEADER, info, &reply_payload);
227 if (!rskb)
228 goto err_cleanup;
229
230 ret = tsconfig_fill_reply(rskb, &req_info->base, &reply_data->base);
231 if (ret < 0)
232 goto err_cleanup;
233
234 genlmsg_end(rskb, reply_payload);
235 ret = genlmsg_reply(rskb, info);
236
237 err_cleanup:
238 kfree(reply_data);
239 kfree(req_info);
240 return ret;
241 }
242
ethnl_set_tsconfig_validate(struct ethnl_req_info * req_base,struct genl_info * info)243 static int ethnl_set_tsconfig_validate(struct ethnl_req_info *req_base,
244 struct genl_info *info)
245 {
246 const struct net_device_ops *ops = req_base->dev->netdev_ops;
247
248 if (!ops->ndo_hwtstamp_set || !ops->ndo_hwtstamp_get)
249 return -EOPNOTSUPP;
250
251 return 1;
252 }
253
254 static struct hwtstamp_provider *
tsconfig_set_hwprov_from_desc(struct net_device * dev,struct genl_info * info,struct hwtstamp_provider_desc * hwprov_desc)255 tsconfig_set_hwprov_from_desc(struct net_device *dev,
256 struct genl_info *info,
257 struct hwtstamp_provider_desc *hwprov_desc)
258 {
259 struct kernel_ethtool_ts_info ts_info;
260 struct hwtstamp_provider *hwprov;
261 struct nlattr **tb = info->attrs;
262 struct phy_device *phy = NULL;
263 enum hwtstamp_source source;
264 int ret;
265
266 ret = ethtool_net_get_ts_info_by_phc(dev, &ts_info, hwprov_desc);
267 if (!ret) {
268 /* Found */
269 source = HWTSTAMP_SOURCE_NETDEV;
270 } else {
271 phy = ethtool_phy_get_ts_info_by_phc(dev, &ts_info, hwprov_desc);
272 if (IS_ERR(phy)) {
273 if (PTR_ERR(phy) == -ENODEV)
274 NL_SET_ERR_MSG_ATTR(info->extack,
275 tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER],
276 "phc not in this net device topology");
277 return ERR_CAST(phy);
278 }
279
280 source = HWTSTAMP_SOURCE_PHYLIB;
281 }
282
283 hwprov = kzalloc(sizeof(*hwprov), GFP_KERNEL);
284 if (!hwprov)
285 return ERR_PTR(-ENOMEM);
286
287 hwprov->desc.index = hwprov_desc->index;
288 hwprov->desc.qualifier = hwprov_desc->qualifier;
289 hwprov->source = source;
290 hwprov->phydev = phy;
291
292 return hwprov;
293 }
294
ethnl_set_tsconfig(struct ethnl_req_info * req_base,struct genl_info * info)295 static int ethnl_set_tsconfig(struct ethnl_req_info *req_base,
296 struct genl_info *info)
297 {
298 struct kernel_hwtstamp_config hwtst_config = {0};
299 bool hwprov_mod = false, config_mod = false;
300 struct hwtstamp_provider *hwprov = NULL;
301 struct net_device *dev = req_base->dev;
302 struct nlattr **tb = info->attrs;
303 int ret;
304
305 BUILD_BUG_ON(__HWTSTAMP_TX_CNT >= 32);
306 BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT >= 32);
307 BUILD_BUG_ON(__HWTSTAMP_FLAG_CNT > 32);
308
309 if (!netif_device_present(dev))
310 return -ENODEV;
311
312 if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER]) {
313 struct hwtstamp_provider_desc __hwprov_desc = {.index = -1};
314 struct hwtstamp_provider *__hwprov;
315
316 __hwprov = rtnl_dereference(dev->hwprov);
317 if (__hwprov) {
318 __hwprov_desc.index = __hwprov->desc.index;
319 __hwprov_desc.qualifier = __hwprov->desc.qualifier;
320 }
321
322 ret = ts_parse_hwtst_provider(tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER],
323 &__hwprov_desc, info->extack,
324 &hwprov_mod);
325 if (ret < 0)
326 return ret;
327
328 if (hwprov_mod) {
329 hwprov = tsconfig_set_hwprov_from_desc(dev, info,
330 &__hwprov_desc);
331 if (IS_ERR(hwprov))
332 return PTR_ERR(hwprov);
333 }
334 }
335
336 /* Get current hwtstamp config if we are not changing the
337 * hwtstamp source. It will be zeroed in the other case.
338 */
339 if (!hwprov_mod) {
340 ret = dev_get_hwtstamp_phylib(dev, &hwtst_config);
341 if (ret < 0 && ret != -EOPNOTSUPP)
342 goto err_free_hwprov;
343 }
344
345 /* Get the hwtstamp config from netlink */
346 if (tb[ETHTOOL_A_TSCONFIG_TX_TYPES]) {
347 u32 req_tx_type;
348
349 req_tx_type = BIT(hwtst_config.tx_type);
350 ret = ethnl_update_bitset32(&req_tx_type,
351 __HWTSTAMP_TX_CNT,
352 tb[ETHTOOL_A_TSCONFIG_TX_TYPES],
353 ts_tx_type_names, info->extack,
354 &config_mod);
355 if (ret < 0)
356 goto err_free_hwprov;
357
358 /* Select only one tx type at a time */
359 if (ffs(req_tx_type) != fls(req_tx_type)) {
360 ret = -EINVAL;
361 goto err_free_hwprov;
362 }
363
364 hwtst_config.tx_type = ffs(req_tx_type) - 1;
365 }
366
367 if (tb[ETHTOOL_A_TSCONFIG_RX_FILTERS]) {
368 u32 req_rx_filter;
369
370 req_rx_filter = BIT(hwtst_config.rx_filter);
371 ret = ethnl_update_bitset32(&req_rx_filter,
372 __HWTSTAMP_FILTER_CNT,
373 tb[ETHTOOL_A_TSCONFIG_RX_FILTERS],
374 ts_rx_filter_names, info->extack,
375 &config_mod);
376 if (ret < 0)
377 goto err_free_hwprov;
378
379 /* Select only one rx filter at a time */
380 if (ffs(req_rx_filter) != fls(req_rx_filter)) {
381 ret = -EINVAL;
382 goto err_free_hwprov;
383 }
384
385 hwtst_config.rx_filter = ffs(req_rx_filter) - 1;
386 }
387
388 if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS]) {
389 ret = ethnl_update_bitset32(&hwtst_config.flags,
390 __HWTSTAMP_FLAG_CNT,
391 tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS],
392 ts_flags_names, info->extack,
393 &config_mod);
394 if (ret < 0)
395 goto err_free_hwprov;
396 }
397
398 ret = net_hwtstamp_validate(&hwtst_config);
399 if (ret)
400 goto err_free_hwprov;
401
402 if (hwprov_mod) {
403 struct kernel_hwtstamp_config zero_config = {0};
404 struct hwtstamp_provider *__hwprov;
405
406 /* Disable current time stamping if we try to enable
407 * another one
408 */
409 ret = dev_set_hwtstamp_phylib(dev, &zero_config, info->extack);
410 if (ret < 0)
411 goto err_free_hwprov;
412
413 /* Change the selected hwtstamp source */
414 __hwprov = rcu_replace_pointer_rtnl(dev->hwprov, hwprov);
415 if (__hwprov)
416 kfree_rcu(__hwprov, rcu_head);
417 }
418
419 if (config_mod) {
420 ret = dev_set_hwtstamp_phylib(dev, &hwtst_config,
421 info->extack);
422 if (ret < 0)
423 return ret;
424 }
425
426 if (hwprov_mod || config_mod) {
427 ret = tsconfig_send_reply(dev, info);
428 if (ret && ret != -EOPNOTSUPP) {
429 NL_SET_ERR_MSG(info->extack,
430 "error while reading the new configuration set");
431 return ret;
432 }
433 }
434
435 /* tsconfig has no notification */
436 return 0;
437
438 err_free_hwprov:
439 kfree(hwprov);
440
441 return ret;
442 }
443
444 const struct ethnl_request_ops ethnl_tsconfig_request_ops = {
445 .request_cmd = ETHTOOL_MSG_TSCONFIG_GET,
446 .reply_cmd = ETHTOOL_MSG_TSCONFIG_GET_REPLY,
447 .hdr_attr = ETHTOOL_A_TSCONFIG_HEADER,
448 .req_info_size = sizeof(struct tsconfig_req_info),
449 .reply_data_size = sizeof(struct tsconfig_reply_data),
450
451 .prepare_data = tsconfig_prepare_data,
452 .reply_size = tsconfig_reply_size,
453 .fill_reply = tsconfig_fill_reply,
454
455 .set_validate = ethnl_set_tsconfig_validate,
456 .set = ethnl_set_tsconfig,
457 };
458