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

#include <linux/phy.h>
#include <linux/phylib_stubs.h>

#include "common.h"
#include "netlink.h"

struct linkstate_req_info {
        struct ethnl_req_info           base;
};

struct linkstate_reply_data {
        struct ethnl_reply_data                 base;
        int                                     link;
        int                                     sqi;
        int                                     sqi_max;
        struct ethtool_link_ext_stats           link_stats;
        bool                                    link_ext_state_provided;
        struct ethtool_link_ext_state_info      ethtool_link_ext_state_info;
};

#define LINKSTATE_REPDATA(__reply_base) \
        container_of(__reply_base, struct linkstate_reply_data, base)

const struct nla_policy ethnl_linkstate_get_policy[] = {
        [ETHTOOL_A_LINKSTATE_HEADER]            =
                NLA_POLICY_NESTED(ethnl_header_policy_stats),
};

static int linkstate_get_sqi(struct phy_device *phydev)
{
        int ret;

        if (!phydev)
                return -EOPNOTSUPP;

        mutex_lock(&phydev->lock);
        if (!phydev->drv || !phydev->drv->get_sqi)
                ret = -EOPNOTSUPP;
        else if (!phydev->link)
                ret = -ENETDOWN;
        else
                ret = phydev->drv->get_sqi(phydev);
        mutex_unlock(&phydev->lock);

        return ret;
}

static int linkstate_get_sqi_max(struct phy_device *phydev)
{
        int ret;

        if (!phydev)
                return -EOPNOTSUPP;

        mutex_lock(&phydev->lock);
        if (!phydev->drv || !phydev->drv->get_sqi_max)
                ret = -EOPNOTSUPP;
        else if (!phydev->link)
                ret = -ENETDOWN;
        else
                ret = phydev->drv->get_sqi_max(phydev);
        mutex_unlock(&phydev->lock);

        return ret;
};

static bool linkstate_sqi_critical_error(int sqi)
{
        return sqi < 0 && sqi != -EOPNOTSUPP && sqi != -ENETDOWN;
}

static bool linkstate_sqi_valid(struct linkstate_reply_data *data)
{
        return data->sqi >= 0 && data->sqi_max >= 0 &&
               data->sqi <= data->sqi_max;
}

static int linkstate_get_link_ext_state(struct net_device *dev,
                                        struct linkstate_reply_data *data)
{
        int err;

        if (!dev->ethtool_ops->get_link_ext_state)
                return -EOPNOTSUPP;

        err = dev->ethtool_ops->get_link_ext_state(dev, &data->ethtool_link_ext_state_info);
        if (err)
                return err;

        data->link_ext_state_provided = true;

        return 0;
}

static int linkstate_prepare_data(const struct ethnl_req_info *req_base,
                                  struct ethnl_reply_data *reply_base,
                                  const struct genl_info *info)
{
        struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
        struct net_device *dev = reply_base->dev;
        struct nlattr **tb = info->attrs;
        struct phy_device *phydev;
        int ret;

        phydev = ethnl_req_get_phydev(req_base, tb, ETHTOOL_A_LINKSTATE_HEADER,
                                      info->extack);
        if (IS_ERR(phydev))
                return PTR_ERR(phydev);

        ret = ethnl_ops_begin(dev);
        if (ret < 0)
                return ret;
        data->link = __ethtool_get_link(dev);

        ret = linkstate_get_sqi(phydev);
        if (linkstate_sqi_critical_error(ret))
                goto out;
        data->sqi = ret;

        ret = linkstate_get_sqi_max(phydev);
        if (linkstate_sqi_critical_error(ret))
                goto out;
        data->sqi_max = ret;

        if (dev->flags & IFF_UP) {
                ret = linkstate_get_link_ext_state(dev, data);
                if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA)
                        goto out;
        }

        ethtool_stats_init((u64 *)&data->link_stats,
                           sizeof(data->link_stats) / 8);

        if (req_base->flags & ETHTOOL_FLAG_STATS) {
                if (phydev)
                        phy_ethtool_get_link_ext_stats(phydev,
                                                       &data->link_stats);

                if (dev->ethtool_ops->get_link_ext_stats)
                        dev->ethtool_ops->get_link_ext_stats(dev,
                                                             &data->link_stats);
        }

        ret = 0;
out:
        ethnl_ops_complete(dev);
        return ret;
}

static int linkstate_reply_size(const struct ethnl_req_info *req_base,
                                const struct ethnl_reply_data *reply_base)
{
        struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
        int len;

        len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */
                + 0;

        if (linkstate_sqi_valid(data)) {
                len += nla_total_size(sizeof(u32)); /* LINKSTATE_SQI */
                len += nla_total_size(sizeof(u32)); /* LINKSTATE_SQI_MAX */
        }

        if (data->link_ext_state_provided)
                len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */

        if (data->ethtool_link_ext_state_info.__link_ext_substate)
                len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */

        if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET)
                len += nla_total_size(sizeof(u32));

        return len;
}

static int linkstate_fill_reply(struct sk_buff *skb,
                                const struct ethnl_req_info *req_base,
                                const struct ethnl_reply_data *reply_base)
{
        struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);

        if (data->link >= 0 &&
            nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link))
                return -EMSGSIZE;

        if (linkstate_sqi_valid(data)) {
                if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi))
                        return -EMSGSIZE;

                if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX,
                                data->sqi_max))
                        return -EMSGSIZE;
        }

        if (data->link_ext_state_provided) {
                if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE,
                               data->ethtool_link_ext_state_info.link_ext_state))
                        return -EMSGSIZE;

                if (data->ethtool_link_ext_state_info.__link_ext_substate &&
                    nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE,
                               data->ethtool_link_ext_state_info.__link_ext_substate))
                        return -EMSGSIZE;
        }

        if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET)
                if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_EXT_DOWN_CNT,
                                data->link_stats.link_down_events))
                        return -EMSGSIZE;

        return 0;
}

const struct ethnl_request_ops ethnl_linkstate_request_ops = {
        .request_cmd            = ETHTOOL_MSG_LINKSTATE_GET,
        .reply_cmd              = ETHTOOL_MSG_LINKSTATE_GET_REPLY,
        .hdr_attr               = ETHTOOL_A_LINKSTATE_HEADER,
        .req_info_size          = sizeof(struct linkstate_req_info),
        .reply_data_size        = sizeof(struct linkstate_reply_data),

        .prepare_data           = linkstate_prepare_data,
        .reply_size             = linkstate_reply_size,
        .fill_reply             = linkstate_fill_reply,
};