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