tsconfig.c

  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);
 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
 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(&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
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, 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 */
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(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
237err_cleanup:
238	kfree(reply_data);
239	kfree(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, &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
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(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	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(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};