// SPDX-License-Identifier: GPL-2.0-only

#include <linux/net_tstamp.h>
#include <linux/ptp_clock_kernel.h>

#include "netlink.h"
#include "common.h"
#include "bitset.h"
#include "../core/dev.h"
#include "ts.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 = rtnl_dereference(dev->hwprov);
        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)
                        return -ENODEV;

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

        ASSERT_RTNL();
        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)
                goto err_cleanup;

        ret = tsconfig_fill_reply(rskb, &req_info->base, &reply_data->base);
        if (ret < 0)
                goto err_cleanup;

        genlmsg_end(rskb, reply_payload);
        ret = genlmsg_reply(rskb, info);

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;

        BUILD_BUG_ON(__HWTSTAMP_TX_CNT >= 32);
        BUILD_BUG_ON(__HWTSTAMP_FILTER_CNT >= 32);
        BUILD_BUG_ON(__HWTSTAMP_FLAG_CNT > 32);

        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 = rtnl_dereference(dev->hwprov);
                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 only one tx type at a time */
                if (ffs(req_tx_type) != fls(req_tx_type)) {
                        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 only one rx filter at a time */
                if (ffs(req_rx_filter) != fls(req_rx_filter)) {
                        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_rtnl(dev->hwprov, hwprov);
                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,
};