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
12struct tsconfig_req_info {
13 struct ethnl_req_info base;
14};
15
16struct 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
29const 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
34static 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: &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, info: &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
79out:
80 ethnl_ops_complete(dev);
81 return ret;
82}
83
84static 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(val: &data->hwtst_config.flags,
98 NULL, __HWTSTAMP_FLAG_CNT,
99 names: 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(val: &data->hwtst_config.tx_type,
107 NULL, nbits: __HWTSTAMP_TX_CNT,
108 names: 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(val: &data->hwtst_config.rx_filter,
115 NULL, nbits: __HWTSTAMP_FILTER_CNT,
116 names: 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(payload: 0) +
125 2 * nla_total_size(payload: sizeof(u32));
126
127 return len;
128}
129
130static 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, attrtype: ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS,
140 val: &data->hwtst_config.flags, NULL,
141 __HWTSTAMP_FLAG_CNT,
142 names: 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, attrtype: ETHTOOL_A_TSCONFIG_TX_TYPES,
149 val: &data->hwtst_config.tx_type, NULL,
150 nbits: __HWTSTAMP_TX_CNT,
151 names: 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, attrtype: ETHTOOL_A_TSCONFIG_RX_FILTERS,
158 val: &data->hwtst_config.rx_filter,
159 NULL, nbits: __HWTSTAMP_FILTER_CNT,
160 names: 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, attrtype: ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER);
169 if (!nest)
170 return -EMSGSIZE;
171
172 if (nla_put_u32(skb, attrtype: ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX,
173 value: data->hwprov_desc.index) ||
174 nla_put_u32(skb,
175 attrtype: ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER,
176 value: data->hwprov_desc.qualifier)) {
177 nla_nest_cancel(skb, start: nest);
178 return -EMSGSIZE;
179 }
180
181 nla_nest_end(skb, start: nest);
182 }
183 return 0;
184}
185
186/* TSCONFIG_SET */
187const 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
196static 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(objp: req_info);
211 return -ENOMEM;
212 }
213
214 ASSERT_RTNL();
215 reply_data->base.dev = dev;
216 ret = tsconfig_prepare_data(req_base: &req_info->base, reply_base: &reply_data->base, info);
217 if (ret < 0)
218 goto err_cleanup;
219
220 ret = tsconfig_reply_size(req_base: &req_info->base, reply_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(payload: reply_len, dev, cmd: ETHTOOL_MSG_TSCONFIG_SET_REPLY,
226 hdr_attrtype: ETHTOOL_A_TSCONFIG_HEADER, info, ehdrp: &reply_payload);
227 if (!rskb)
228 goto err_cleanup;
229
230 ret = tsconfig_fill_reply(skb: rskb, req_base: &req_info->base, reply_base: &reply_data->base);
231 if (ret < 0)
232 goto err_cleanup;
233
234 genlmsg_end(skb: rskb, hdr: reply_payload);
235 ret = genlmsg_reply(skb: rskb, info);
236
237err_cleanup:
238 kfree(objp: reply_data);
239 kfree(objp: req_info);
240 return ret;
241}
242
243static 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
254static struct hwtstamp_provider *
255tsconfig_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, info: &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, info: &ts_info, hwprov_desc);
272 if (IS_ERR(ptr: phy)) {
273 if (PTR_ERR(ptr: 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(ptr: phy);
278 }
279
280 source = HWTSTAMP_SOURCE_PHYLIB;
281 }
282
283 hwprov = kzalloc(sizeof(*hwprov), GFP_KERNEL);
284 if (!hwprov)
285 return ERR_PTR(error: -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
295static 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(nest: tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER],
323 hwprov_desc: &__hwprov_desc, extack: info->extack,
324 mod: &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: &__hwprov_desc);
331 if (IS_ERR(ptr: hwprov))
332 return PTR_ERR(ptr: 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, cfg: &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(bitmap: &req_tx_type,
351 nbits: __HWTSTAMP_TX_CNT,
352 attr: tb[ETHTOOL_A_TSCONFIG_TX_TYPES],
353 names: ts_tx_type_names, extack: info->extack,
354 mod: &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(x: 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(bitmap: &req_rx_filter,
372 nbits: __HWTSTAMP_FILTER_CNT,
373 attr: tb[ETHTOOL_A_TSCONFIG_RX_FILTERS],
374 names: ts_rx_filter_names, extack: info->extack,
375 mod: &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(x: 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(bitmap: &hwtst_config.flags,
390 __HWTSTAMP_FLAG_CNT,
391 attr: tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS],
392 names: ts_flags_names, extack: info->extack,
393 mod: &config_mod);
394 if (ret < 0)
395 goto err_free_hwprov;
396 }
397
398 ret = net_hwtstamp_validate(cfg: &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, cfg: &zero_config, extack: 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, cfg: &hwtst_config,
421 extack: info->extack);
422 if (ret < 0)
423 return ret;
424 }
425
426 ret = tsconfig_send_reply(dev, info);
427 if (ret && ret != -EOPNOTSUPP) {
428 NL_SET_ERR_MSG(info->extack,
429 "error while reading the new configuration set");
430 return ret;
431 }
432
433 /* tsconfig has no notification */
434 return 0;
435
436err_free_hwprov:
437 kfree(objp: hwprov);
438
439 return ret;
440}
441
442const struct ethnl_request_ops ethnl_tsconfig_request_ops = {
443 .request_cmd = ETHTOOL_MSG_TSCONFIG_GET,
444 .reply_cmd = ETHTOOL_MSG_TSCONFIG_GET_REPLY,
445 .hdr_attr = ETHTOOL_A_TSCONFIG_HEADER,
446 .req_info_size = sizeof(struct tsconfig_req_info),
447 .reply_data_size = sizeof(struct tsconfig_reply_data),
448
449 .prepare_data = tsconfig_prepare_data,
450 .reply_size = tsconfig_reply_size,
451 .fill_reply = tsconfig_fill_reply,
452
453 .set_validate = ethnl_set_tsconfig_validate,
454 .set = ethnl_set_tsconfig,
455};
456