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