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