464 lines · 11.7 KB · C
RawDownload
// SPDX-License-Identifier: GPL-2.0-only #include <linux/net_tstamp.h>#include <linux/ptp_clock_kernel.h>#include <net/netdev_lock.h> #include "bitset.h"#include "common.h"#include "netlink.h"#include "ts.h"#include "../core/dev.h" struct tsconfig_req_info {	struct ethnl_req_info base;}; struct tsconfig_reply_data {	struct ethnl_reply_data		base;	struct hwtstamp_provider_desc	hwprov_desc;	struct {		u32 tx_type;		u32 rx_filter;		u32 flags;	} hwtst_config;}; #define TSCONFIG_REPDATA(__reply_base) \	container_of(__reply_base, struct tsconfig_reply_data, base) const struct nla_policy ethnl_tsconfig_get_policy[ETHTOOL_A_TSCONFIG_HEADER + 1] = {	[ETHTOOL_A_TSCONFIG_HEADER]		=		NLA_POLICY_NESTED(ethnl_header_policy),}; static int tsconfig_prepare_data(const struct ethnl_req_info *req_base,				 struct ethnl_reply_data *reply_base,				 const struct genl_info *info){	struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base);	struct hwtstamp_provider *hwprov = NULL;	struct net_device *dev = reply_base->dev;	struct kernel_hwtstamp_config cfg = {};	int ret; 	if (!dev->netdev_ops->ndo_hwtstamp_get)		return -EOPNOTSUPP; 	ret = ethnl_ops_begin(dev);	if (ret < 0)		return ret; 	ret = dev_get_hwtstamp_phylib(dev, &cfg);	if (ret)		goto out; 	data->hwtst_config.tx_type = BIT(cfg.tx_type);	data->hwtst_config.rx_filter = BIT(cfg.rx_filter);	data->hwtst_config.flags = cfg.flags; 	data->hwprov_desc.index = -1;	hwprov = netdev_ops_lock_dereference(dev->hwprov, dev);	if (hwprov) {		data->hwprov_desc.index = hwprov->desc.index;		data->hwprov_desc.qualifier = hwprov->desc.qualifier;	} else {		struct kernel_ethtool_ts_info ts_info = {}; 		ts_info.phc_index = -1;		ret = __ethtool_get_ts_info(dev, &ts_info);		if (ret)			goto out; 		if (ts_info.phc_index == -1) {			ret = -ENODEV;			goto out;		} 		data->hwprov_desc.index = ts_info.phc_index;		data->hwprov_desc.qualifier = ts_info.phc_qualifier;	} out:	ethnl_ops_complete(dev);	return ret;} static int tsconfig_reply_size(const struct ethnl_req_info *req_base,			       const struct ethnl_reply_data *reply_base){	const struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base);	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;	int len = 0;	int ret; 	BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32);	BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32);	BUILD_BUG_ON(__HWTSTAMP_FLAG_CNT > 32); 	if (data->hwtst_config.flags) {		ret = ethnl_bitset32_size(&data->hwtst_config.flags,					  NULL, __HWTSTAMP_FLAG_CNT,					  ts_flags_names, compact);		if (ret < 0)			return ret;		len += ret;	/* _TSCONFIG_HWTSTAMP_FLAGS */	} 	if (data->hwtst_config.tx_type) {		ret = ethnl_bitset32_size(&data->hwtst_config.tx_type,					  NULL, __HWTSTAMP_TX_CNT,					  ts_tx_type_names, compact);		if (ret < 0)			return ret;		len += ret;	/* _TSCONFIG_TX_TYPES */	}	if (data->hwtst_config.rx_filter) {		ret = ethnl_bitset32_size(&data->hwtst_config.rx_filter,					  NULL, __HWTSTAMP_FILTER_CNT,					  ts_rx_filter_names, compact);		if (ret < 0)			return ret;		len += ret;	/* _TSCONFIG_RX_FILTERS */	} 	if (data->hwprov_desc.index >= 0)		/* _TSCONFIG_HWTSTAMP_PROVIDER */		len += nla_total_size(0) +		       2 * nla_total_size(sizeof(u32)); 	return len;} static int tsconfig_fill_reply(struct sk_buff *skb,			       const struct ethnl_req_info *req_base,			       const struct ethnl_reply_data *reply_base){	const struct tsconfig_reply_data *data = TSCONFIG_REPDATA(reply_base);	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;	int ret; 	if (data->hwtst_config.flags) {		ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS,					 &data->hwtst_config.flags, NULL,					 __HWTSTAMP_FLAG_CNT,					 ts_flags_names, compact);		if (ret < 0)			return ret;	} 	if (data->hwtst_config.tx_type) {		ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_TX_TYPES,					 &data->hwtst_config.tx_type, NULL,					 __HWTSTAMP_TX_CNT,					 ts_tx_type_names, compact);		if (ret < 0)			return ret;	} 	if (data->hwtst_config.rx_filter) {		ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSCONFIG_RX_FILTERS,					 &data->hwtst_config.rx_filter,					 NULL, __HWTSTAMP_FILTER_CNT,					 ts_rx_filter_names, compact);		if (ret < 0)			return ret;	} 	if (data->hwprov_desc.index >= 0) {		struct nlattr *nest; 		nest = nla_nest_start(skb, ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER);		if (!nest)			return -EMSGSIZE; 		if (nla_put_u32(skb, ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX,				data->hwprov_desc.index) ||		    nla_put_u32(skb,				ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER,				data->hwprov_desc.qualifier)) {			nla_nest_cancel(skb, nest);			return -EMSGSIZE;		} 		nla_nest_end(skb, nest);	}	return 0;} /* TSCONFIG_SET */const struct nla_policy ethnl_tsconfig_set_policy[ETHTOOL_A_TSCONFIG_MAX + 1] = {	[ETHTOOL_A_TSCONFIG_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),	[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER] =		NLA_POLICY_NESTED(ethnl_ts_hwtst_prov_policy),	[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS] = { .type = NLA_NESTED },	[ETHTOOL_A_TSCONFIG_RX_FILTERS] = { .type = NLA_NESTED },	[ETHTOOL_A_TSCONFIG_TX_TYPES] = { .type = NLA_NESTED },}; static int tsconfig_send_reply(struct net_device *dev, struct genl_info *info){	struct tsconfig_reply_data *reply_data;	struct tsconfig_req_info *req_info;	struct sk_buff *rskb;	void *reply_payload;	int reply_len = 0;	int ret; 	req_info = kzalloc_obj(*req_info);	if (!req_info)		return -ENOMEM;	reply_data = kmalloc_obj(*reply_data);	if (!reply_data) {		kfree(req_info);		return -ENOMEM;	} 	netdev_assert_locked_ops_compat(dev);	reply_data->base.dev = dev;	ret = tsconfig_prepare_data(&req_info->base, &reply_data->base, info);	if (ret < 0)		goto err_cleanup; 	ret = tsconfig_reply_size(&req_info->base, &reply_data->base);	if (ret < 0)		goto err_cleanup; 	reply_len = ret + ethnl_reply_header_size();	rskb = ethnl_reply_init(reply_len, dev, ETHTOOL_MSG_TSCONFIG_SET_REPLY,				ETHTOOL_A_TSCONFIG_HEADER, info, &reply_payload);	if (!rskb) {		ret = -ENOMEM;		goto err_cleanup;	} 	ret = tsconfig_fill_reply(rskb, &req_info->base, &reply_data->base);	if (ret < 0)		goto err_free_msg; 	genlmsg_end(rskb, reply_payload);	ret = genlmsg_reply(rskb, info);	rskb = NULL; err_free_msg:	nlmsg_free(rskb);err_cleanup:	kfree(reply_data);	kfree(req_info);	return ret;} static int ethnl_set_tsconfig_validate(struct ethnl_req_info *req_base,				       struct genl_info *info){	const struct net_device_ops *ops = req_base->dev->netdev_ops; 	if (!ops->ndo_hwtstamp_set || !ops->ndo_hwtstamp_get)		return -EOPNOTSUPP; 	return 1;} static struct hwtstamp_provider *tsconfig_set_hwprov_from_desc(struct net_device *dev,			      struct genl_info *info,			      struct hwtstamp_provider_desc *hwprov_desc){	struct kernel_ethtool_ts_info ts_info;	struct hwtstamp_provider *hwprov;	struct nlattr **tb = info->attrs;	struct phy_device *phy = NULL;	enum hwtstamp_source source;	int ret; 	ret = ethtool_net_get_ts_info_by_phc(dev, &ts_info, hwprov_desc);	if (!ret) {		/* Found */		source = HWTSTAMP_SOURCE_NETDEV;	} else {		phy = ethtool_phy_get_ts_info_by_phc(dev, &ts_info, hwprov_desc);		if (IS_ERR(phy)) {			if (PTR_ERR(phy) == -ENODEV)				NL_SET_ERR_MSG_ATTR(info->extack,						    tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER],						    "phc not in this net device topology");			return ERR_CAST(phy);		} 		source = HWTSTAMP_SOURCE_PHYLIB;	} 	hwprov = kzalloc_obj(*hwprov);	if (!hwprov)		return ERR_PTR(-ENOMEM); 	hwprov->desc.index = hwprov_desc->index;	hwprov->desc.qualifier = hwprov_desc->qualifier;	hwprov->source = source;	hwprov->phydev = phy; 	return hwprov;} static int ethnl_set_tsconfig(struct ethnl_req_info *req_base,			      struct genl_info *info){	struct kernel_hwtstamp_config hwtst_config = {0};	bool hwprov_mod = false, config_mod = false;	struct hwtstamp_provider *hwprov = NULL;	struct net_device *dev = req_base->dev;	struct nlattr **tb = info->attrs;	int ret; 	if (!netif_device_present(dev))		return -ENODEV; 	if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER]) {		struct hwtstamp_provider_desc __hwprov_desc = {.index = -1};		struct hwtstamp_provider *__hwprov; 		__hwprov = netdev_ops_lock_dereference(dev->hwprov, dev);		if (__hwprov) {			__hwprov_desc.index = __hwprov->desc.index;			__hwprov_desc.qualifier = __hwprov->desc.qualifier;		} 		ret = ts_parse_hwtst_provider(tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_PROVIDER],					      &__hwprov_desc, info->extack,					      &hwprov_mod);		if (ret < 0)			return ret; 		if (hwprov_mod) {			hwprov = tsconfig_set_hwprov_from_desc(dev, info,							       &__hwprov_desc);			if (IS_ERR(hwprov))				return PTR_ERR(hwprov);		}	} 	/* Get current hwtstamp config if we are not changing the	 * hwtstamp source. It will be zeroed in the other case.	 */	if (!hwprov_mod) {		ret = dev_get_hwtstamp_phylib(dev, &hwtst_config);		if (ret < 0 && ret != -EOPNOTSUPP)			goto err_free_hwprov;	} 	/* Get the hwtstamp config from netlink */	if (tb[ETHTOOL_A_TSCONFIG_TX_TYPES]) {		u32 req_tx_type; 		req_tx_type = BIT(hwtst_config.tx_type);		ret = ethnl_update_bitset32(&req_tx_type,					    __HWTSTAMP_TX_CNT,					    tb[ETHTOOL_A_TSCONFIG_TX_TYPES],					    ts_tx_type_names, info->extack,					    &config_mod);		if (ret < 0)			goto err_free_hwprov; 		/* Select exactly one tx type at a time */		if (hweight32(req_tx_type) != 1) {			NL_SET_BAD_ATTR(info->extack,					tb[ETHTOOL_A_TSCONFIG_TX_TYPES]);			ret = -EINVAL;			goto err_free_hwprov;		} 		hwtst_config.tx_type = ffs(req_tx_type) - 1;	} 	if (tb[ETHTOOL_A_TSCONFIG_RX_FILTERS]) {		u32 req_rx_filter; 		req_rx_filter = BIT(hwtst_config.rx_filter);		ret = ethnl_update_bitset32(&req_rx_filter,					    __HWTSTAMP_FILTER_CNT,					    tb[ETHTOOL_A_TSCONFIG_RX_FILTERS],					    ts_rx_filter_names, info->extack,					    &config_mod);		if (ret < 0)			goto err_free_hwprov; 		/* Select exactly one rx filter at a time */		if (hweight32(req_rx_filter) != 1) {			NL_SET_BAD_ATTR(info->extack,					tb[ETHTOOL_A_TSCONFIG_RX_FILTERS]);			ret = -EINVAL;			goto err_free_hwprov;		} 		hwtst_config.rx_filter = ffs(req_rx_filter) - 1;	} 	if (tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS]) {		ret = ethnl_update_bitset32(&hwtst_config.flags,					    __HWTSTAMP_FLAG_CNT,					    tb[ETHTOOL_A_TSCONFIG_HWTSTAMP_FLAGS],					    ts_flags_names, info->extack,					    &config_mod);		if (ret < 0)			goto err_free_hwprov;	} 	ret = net_hwtstamp_validate(&hwtst_config);	if (ret)		goto err_free_hwprov; 	if (hwprov_mod) {		struct kernel_hwtstamp_config zero_config = {0};		struct hwtstamp_provider *__hwprov; 		/* Disable current time stamping if we try to enable		 * another one		 */		ret = dev_set_hwtstamp_phylib(dev, &zero_config, info->extack);		if (ret < 0)			goto err_free_hwprov; 		/* Change the selected hwtstamp source */		__hwprov = rcu_replace_pointer(dev->hwprov, hwprov,					       netdev_is_locked_ops_compat(dev));		if (__hwprov)			kfree_rcu(__hwprov, rcu_head);	} 	if (config_mod) {		ret = dev_set_hwtstamp_phylib(dev, &hwtst_config,					      info->extack);		if (ret < 0)			return ret;	} 	ret = tsconfig_send_reply(dev, info);	if (ret && ret != -EOPNOTSUPP) {		NL_SET_ERR_MSG(info->extack,			       "error while reading the new configuration set");		return ret;	} 	/* tsconfig has no notification */	return 0; err_free_hwprov:	kfree(hwprov); 	return ret;} const struct ethnl_request_ops ethnl_tsconfig_request_ops = {	.request_cmd		= ETHTOOL_MSG_TSCONFIG_GET,	.reply_cmd		= ETHTOOL_MSG_TSCONFIG_GET_REPLY,	.hdr_attr		= ETHTOOL_A_TSCONFIG_HEADER,	.req_info_size		= sizeof(struct tsconfig_req_info),	.reply_data_size	= sizeof(struct tsconfig_reply_data), 	.prepare_data		= tsconfig_prepare_data,	.reply_size		= tsconfig_reply_size,	.fill_reply		= tsconfig_fill_reply, 	.set_validate		= ethnl_set_tsconfig_validate,	.set			= ethnl_set_tsconfig,};

Keyboard shortcuts

General

Show keyboard shortcuts
?
Focus the current search
/

Issues and pull requests

Open filters
f

Issues, pull requests and Inbox

Select next item
j↓
Select previous item
k↑
Open selected item
Enter
Dismiss filters or clear selection
Esc

Repository code

Go to file
t