// 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,};
// 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,
};