#include <linux/net_tstamp.h>
#include <linux/phy.h>
#include <linux/phy_link_topology.h>
#include <linux/ptp_clock_kernel.h>
#include <net/netdev_lock.h>
#include "netlink.h"
#include "common.h"
#include "bitset.h"
#include "ts.h"
struct tsinfo_req_info {
struct ethnl_req_info base;
struct hwtstamp_provider_desc hwprov_desc;
};
struct tsinfo_reply_data {
struct ethnl_reply_data base;
struct kernel_ethtool_ts_info ts_info;
struct ethtool_ts_stats stats;
};
#define TSINFO_REQINFO(__req_base) \
container_of(__req_base, struct tsinfo_req_info, base)
#define TSINFO_REPDATA(__reply_base) \
container_of(__reply_base, struct tsinfo_reply_data, base)
#define ETHTOOL_TS_STAT_CNT \
(__ETHTOOL_A_TS_STAT_CNT - (ETHTOOL_A_TS_STAT_UNSPEC + 1))
const struct nla_policy ethnl_tsinfo_get_policy[ETHTOOL_A_TSINFO_MAX + 1] = {
[ETHTOOL_A_TSINFO_HEADER] =
NLA_POLICY_NESTED(ethnl_header_policy_stats),
[ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER] =
NLA_POLICY_NESTED(ethnl_ts_hwtst_prov_policy),
};
int ts_parse_hwtst_provider(const struct nlattr *nest,
struct hwtstamp_provider_desc *hwprov_desc,
struct netlink_ext_ack *extack,
bool *mod)
{
struct nlattr *tb[ARRAY_SIZE(ethnl_ts_hwtst_prov_policy)];
int ret;
ret = nla_parse_nested(tb,
ARRAY_SIZE(ethnl_ts_hwtst_prov_policy) - 1,
nest,
ethnl_ts_hwtst_prov_policy, extack);
if (ret < 0)
return ret;
if (NL_REQ_ATTR_CHECK(extack, nest, tb,
ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX) ||
NL_REQ_ATTR_CHECK(extack, nest, tb,
ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER))
return -EINVAL;
ethnl_update_u32(&hwprov_desc->index,
tb[ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX],
mod);
ethnl_update_u32(&hwprov_desc->qualifier,
tb[ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER],
mod);
return 0;
}
static int
tsinfo_parse_request(struct ethnl_req_info *req_base, struct nlattr **tb,
struct netlink_ext_ack *extack)
{
struct tsinfo_req_info *req = TSINFO_REQINFO(req_base);
bool mod = false;
req->hwprov_desc.index = -1;
if (!tb[ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER])
return 0;
return ts_parse_hwtst_provider(tb[ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER],
&req->hwprov_desc, extack, &mod);
}
static int tsinfo_prepare_data(const struct ethnl_req_info *req_base,
struct ethnl_reply_data *reply_base,
const struct genl_info *info)
{
struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
struct tsinfo_req_info *req = TSINFO_REQINFO(req_base);
struct net_device *dev = reply_base->dev;
int ret;
ret = ethnl_ops_begin(dev);
if (ret < 0)
return ret;
if (req->hwprov_desc.index != -1) {
ret = ethtool_get_ts_info_by_phc(dev, &data->ts_info,
&req->hwprov_desc);
ethnl_ops_complete(dev);
return ret;
}
if (req_base->flags & ETHTOOL_FLAG_STATS) {
ethtool_stats_init((u64 *)&data->stats,
sizeof(data->stats) / sizeof(u64));
if (dev->ethtool_ops->get_ts_stats)
dev->ethtool_ops->get_ts_stats(dev, &data->stats);
}
ret = __ethtool_get_ts_info(dev, &data->ts_info);
ethnl_ops_complete(dev);
return ret;
}
static int tsinfo_reply_size(const struct ethnl_req_info *req_base,
const struct ethnl_reply_data *reply_base)
{
const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
const struct kernel_ethtool_ts_info *ts_info = &data->ts_info;
int len = 0;
int ret;
BUILD_BUG_ON(__SOF_TIMESTAMPING_CNT > 32);
BUILD_BUG_ON(__HWTSTAMP_TX_CNT > 32);
BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT > 32);
if (ts_info->so_timestamping) {
ret = ethnl_bitset32_size(&ts_info->so_timestamping, NULL,
__SOF_TIMESTAMPING_CNT,
sof_timestamping_names, compact);
if (ret < 0)
return ret;
len += ret;
}
if (ts_info->tx_types) {
ret = ethnl_bitset32_size(&ts_info->tx_types, NULL,
__HWTSTAMP_TX_CNT,
ts_tx_type_names, compact);
if (ret < 0)
return ret;
len += ret;
}
if (ts_info->rx_filters) {
ret = ethnl_bitset32_size(&ts_info->rx_filters, NULL,
__HWTSTAMP_FILTER_CNT,
ts_rx_filter_names, compact);
if (ret < 0)
return ret;
len += ret;
}
if (ts_info->phc_index >= 0) {
len += nla_total_size(sizeof(u32));
len += nla_total_size(0) + 2 * nla_total_size(sizeof(u32));
}
if (ts_info->phc_source) {
len += nla_total_size(sizeof(u32));
if (ts_info->phc_phyindex)
len += nla_total_size(sizeof(u32));
}
if (req_base->flags & ETHTOOL_FLAG_STATS)
len += nla_total_size(0) +
nla_total_size_64bit(sizeof(u64)) * ETHTOOL_TS_STAT_CNT;
return len;
}
static int tsinfo_put_stat(struct sk_buff *skb, u64 val, u16 attrtype)
{
if (val == ETHTOOL_STAT_NOT_SET)
return 0;
if (nla_put_uint(skb, attrtype, val))
return -EMSGSIZE;
return 0;
}
static int tsinfo_put_stats(struct sk_buff *skb,
const struct ethtool_ts_stats *stats)
{
struct nlattr *nest;
nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_STATS);
if (!nest)
return -EMSGSIZE;
if (tsinfo_put_stat(skb, stats->tx_stats.pkts,
ETHTOOL_A_TS_STAT_TX_PKTS) ||
tsinfo_put_stat(skb, stats->tx_stats.onestep_pkts_unconfirmed,
ETHTOOL_A_TS_STAT_TX_ONESTEP_PKTS_UNCONFIRMED) ||
tsinfo_put_stat(skb, stats->tx_stats.lost,
ETHTOOL_A_TS_STAT_TX_LOST) ||
tsinfo_put_stat(skb, stats->tx_stats.err,
ETHTOOL_A_TS_STAT_TX_ERR))
goto err_cancel;
nla_nest_end(skb, nest);
return 0;
err_cancel:
nla_nest_cancel(skb, nest);
return -EMSGSIZE;
}
static int tsinfo_fill_reply(struct sk_buff *skb,
const struct ethnl_req_info *req_base,
const struct ethnl_reply_data *reply_base)
{
const struct tsinfo_reply_data *data = TSINFO_REPDATA(reply_base);
bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
const struct kernel_ethtool_ts_info *ts_info = &data->ts_info;
int ret;
if (ts_info->so_timestamping) {
ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TIMESTAMPING,
&ts_info->so_timestamping, NULL,
__SOF_TIMESTAMPING_CNT,
sof_timestamping_names, compact);
if (ret < 0)
return ret;
}
if (ts_info->tx_types) {
ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_TX_TYPES,
&ts_info->tx_types, NULL,
__HWTSTAMP_TX_CNT,
ts_tx_type_names, compact);
if (ret < 0)
return ret;
}
if (ts_info->rx_filters) {
ret = ethnl_put_bitset32(skb, ETHTOOL_A_TSINFO_RX_FILTERS,
&ts_info->rx_filters, NULL,
__HWTSTAMP_FILTER_CNT,
ts_rx_filter_names, compact);
if (ret < 0)
return ret;
}
if (ts_info->phc_index >= 0) {
struct nlattr *nest;
ret = nla_put_u32(skb, ETHTOOL_A_TSINFO_PHC_INDEX,
ts_info->phc_index);
if (ret)
return -EMSGSIZE;
nest = nla_nest_start(skb, ETHTOOL_A_TSINFO_HWTSTAMP_PROVIDER);
if (!nest)
return -EMSGSIZE;
if (nla_put_u32(skb, ETHTOOL_A_TS_HWTSTAMP_PROVIDER_INDEX,
ts_info->phc_index) ||
nla_put_u32(skb,
ETHTOOL_A_TS_HWTSTAMP_PROVIDER_QUALIFIER,
ts_info->phc_qualifier)) {
nla_nest_cancel(skb, nest);
return -EMSGSIZE;
}
nla_nest_end(skb, nest);
}
if (ts_info->phc_source) {
if (nla_put_u32(skb, ETHTOOL_A_TSINFO_HWTSTAMP_SOURCE,
ts_info->phc_source))
return -EMSGSIZE;
if (ts_info->phc_phyindex &&
nla_put_u32(skb, ETHTOOL_A_TSINFO_HWTSTAMP_PHYINDEX,
ts_info->phc_phyindex))
return -EMSGSIZE;
}
if (req_base->flags & ETHTOOL_FLAG_STATS &&
tsinfo_put_stats(skb, &data->stats))
return -EMSGSIZE;
return 0;
}
struct ethnl_tsinfo_dump_ctx {
struct tsinfo_req_info *req_info;
struct tsinfo_reply_data *reply_data;
unsigned long pos_ifindex;
bool netdev_dump_done;
unsigned long pos_phyindex;
enum hwtstamp_provider_qualifier pos_phcqualifier;
};
static void *ethnl_tsinfo_prepare_dump(struct sk_buff *skb,
struct net_device *dev,
struct tsinfo_reply_data *reply_data,
struct netlink_callback *cb)
{
struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
void *ehdr = NULL;
ehdr = ethnl_dump_put(skb, cb,
ETHTOOL_MSG_TSINFO_GET_REPLY);
if (!ehdr)
return ERR_PTR(-EMSGSIZE);
reply_data = ctx->reply_data;
memset(reply_data, 0, sizeof(*reply_data));
reply_data->base.dev = dev;
reply_data->ts_info.cmd = ETHTOOL_GET_TS_INFO;
reply_data->ts_info.phc_index = -1;
return ehdr;
}
static int ethnl_tsinfo_end_dump(struct sk_buff *skb,
struct net_device *dev,
struct tsinfo_req_info *req_info,
struct tsinfo_reply_data *reply_data,
void *ehdr)
{
int ret;
reply_data->ts_info.so_timestamping |= SOF_TIMESTAMPING_RX_SOFTWARE |
SOF_TIMESTAMPING_SOFTWARE;
ret = ethnl_fill_reply_header(skb, dev, ETHTOOL_A_TSINFO_HEADER);
if (ret < 0)
return ret;
ret = tsinfo_fill_reply(skb, &req_info->base, &reply_data->base);
if (ret < 0)
return ret;
reply_data->base.dev = NULL;
genlmsg_end(skb, ehdr);
return ret;
}
static int ethnl_tsinfo_dump_one_phydev(struct sk_buff *skb,
struct net_device *dev,
struct phy_device *phydev,
struct netlink_callback *cb)
{
struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
struct tsinfo_reply_data *reply_data;
struct tsinfo_req_info *req_info;
void *ehdr = NULL;
int ret = 0;
if (!phy_has_tsinfo(phydev))
return -EOPNOTSUPP;
reply_data = ctx->reply_data;
req_info = ctx->req_info;
ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb);
if (IS_ERR(ehdr))
return PTR_ERR(ehdr);
ret = phy_ts_info(phydev, &reply_data->ts_info);
if (ret < 0)
goto err;
if (reply_data->ts_info.phc_index >= 0) {
reply_data->ts_info.phc_source = HWTSTAMP_SOURCE_PHYLIB;
reply_data->ts_info.phc_phyindex = phydev->phyindex;
}
ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data, ehdr);
if (ret < 0)
goto err;
return ret;
err:
genlmsg_cancel(skb, ehdr);
return ret;
}
static int ethnl_tsinfo_dump_one_netdev(struct sk_buff *skb,
struct net_device *dev,
struct netlink_callback *cb)
{
struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
const struct ethtool_ops *ops = dev->ethtool_ops;
struct tsinfo_reply_data *reply_data;
struct tsinfo_req_info *req_info;
void *ehdr = NULL;
int ret = 0;
if (!ops->get_ts_info)
return -EOPNOTSUPP;
reply_data = ctx->reply_data;
req_info = ctx->req_info;
for (; ctx->pos_phcqualifier < HWTSTAMP_PROVIDER_QUALIFIER_CNT;
ctx->pos_phcqualifier++) {
if (!net_support_hwtstamp_qualifier(dev,
ctx->pos_phcqualifier))
continue;
ehdr = ethnl_tsinfo_prepare_dump(skb, dev, reply_data, cb);
if (IS_ERR(ehdr)) {
ret = PTR_ERR(ehdr);
goto err;
}
reply_data->ts_info.phc_qualifier = ctx->pos_phcqualifier;
ret = ops->get_ts_info(dev, &reply_data->ts_info);
if (ret < 0)
goto err;
if (reply_data->ts_info.phc_index >= 0)
reply_data->ts_info.phc_source = HWTSTAMP_SOURCE_NETDEV;
ret = ethnl_tsinfo_end_dump(skb, dev, req_info, reply_data,
ehdr);
if (ret < 0)
goto err;
}
return ret;
err:
genlmsg_cancel(skb, ehdr);
return ret;
}
static int ethnl_tsinfo_dump_one_net_topo(struct sk_buff *skb,
struct net_device *dev,
struct netlink_callback *cb)
{
struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
struct phy_device_node *pdn;
int ret = 0;
if (!ctx->netdev_dump_done) {
ret = ethnl_tsinfo_dump_one_netdev(skb, dev, cb);
if (ret < 0 && ret != -EOPNOTSUPP)
return ret;
ctx->netdev_dump_done = true;
}
if (!dev->link_topo) {
if (phy_has_tsinfo(dev->phydev)) {
ret = ethnl_tsinfo_dump_one_phydev(skb, dev,
dev->phydev, cb);
if (ret < 0 && ret != -EOPNOTSUPP)
return ret;
}
return 0;
}
xa_for_each_start(&dev->link_topo->phys, ctx->pos_phyindex, pdn,
ctx->pos_phyindex) {
if (phy_has_tsinfo(pdn->phy)) {
ret = ethnl_tsinfo_dump_one_phydev(skb, dev,
pdn->phy, cb);
if (ret < 0 && ret != -EOPNOTSUPP)
return ret;
}
}
return ret;
}
int ethnl_tsinfo_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
{
struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
struct net *net = sock_net(skb->sk);
struct net_device *dev;
int ret = 0;
rtnl_lock();
if (ctx->req_info->base.dev) {
dev = ctx->req_info->base.dev;
netdev_lock_ops(dev);
ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb);
netdev_unlock_ops(dev);
} else {
for_each_netdev_dump(net, dev, ctx->pos_ifindex) {
netdev_lock_ops(dev);
ret = ethnl_tsinfo_dump_one_net_topo(skb, dev, cb);
netdev_unlock_ops(dev);
if (ret < 0 && ret != -EOPNOTSUPP)
break;
ctx->pos_phyindex = 0;
ctx->netdev_dump_done = false;
ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE;
}
}
rtnl_unlock();
return ret;
}
int ethnl_tsinfo_start(struct netlink_callback *cb)
{
const struct genl_dumpit_info *info = genl_dumpit_info(cb);
struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
struct nlattr **tb = info->info.attrs;
struct tsinfo_reply_data *reply_data;
struct tsinfo_req_info *req_info;
int ret;
BUILD_BUG_ON(sizeof(*ctx) > sizeof(cb->ctx));
req_info = kzalloc_obj(*req_info);
if (!req_info)
return -ENOMEM;
reply_data = kzalloc_obj(*reply_data);
if (!reply_data) {
ret = -ENOMEM;
goto free_req_info;
}
ret = ethnl_parse_header_dev_get(&req_info->base,
tb[ETHTOOL_A_TSINFO_HEADER],
sock_net(cb->skb->sk), cb->extack,
false);
if (ret < 0)
goto free_reply_data;
ctx->req_info = req_info;
ctx->reply_data = reply_data;
ctx->pos_ifindex = 0;
ctx->pos_phyindex = 0;
ctx->netdev_dump_done = false;
ctx->pos_phcqualifier = HWTSTAMP_PROVIDER_QUALIFIER_PRECISE;
return 0;
free_reply_data:
kfree(reply_data);
free_req_info:
kfree(req_info);
return ret;
}
int ethnl_tsinfo_done(struct netlink_callback *cb)
{
struct ethnl_tsinfo_dump_ctx *ctx = (void *)cb->ctx;
struct tsinfo_req_info *req_info = ctx->req_info;
ethnl_parse_header_dev_put(&req_info->base);
kfree(ctx->reply_data);
kfree(ctx->req_info);
return 0;
}
const struct ethnl_request_ops ethnl_tsinfo_request_ops = {
.request_cmd = ETHTOOL_MSG_TSINFO_GET,
.reply_cmd = ETHTOOL_MSG_TSINFO_GET_REPLY,
.hdr_attr = ETHTOOL_A_TSINFO_HEADER,
.req_info_size = sizeof(struct tsinfo_req_info),
.reply_data_size = sizeof(struct tsinfo_reply_data),
.parse_request = tsinfo_parse_request,
.prepare_data = tsinfo_prepare_data,
.reply_size = tsinfo_reply_size,
.fill_reply = tsinfo_fill_reply,
};
