[mirror_ubuntu-hirsute-kernel.git] / net / ethtool / linkstate.c

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

#include "netlink.h"
#include "common.h"
#include <linux/phy.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;
	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)

static const struct nla_policy
linkstate_get_policy[ETHTOOL_A_LINKSTATE_MAX + 1] = {
	[ETHTOOL_A_LINKSTATE_UNSPEC]		= { .type = NLA_REJECT },
	[ETHTOOL_A_LINKSTATE_HEADER]		= { .type = NLA_NESTED },
	[ETHTOOL_A_LINKSTATE_LINK]		= { .type = NLA_REJECT },
	[ETHTOOL_A_LINKSTATE_SQI]		= { .type = NLA_REJECT },
	[ETHTOOL_A_LINKSTATE_SQI_MAX]		= { .type = NLA_REJECT },
	[ETHTOOL_A_LINKSTATE_EXT_STATE]		= { .type = NLA_REJECT },
	[ETHTOOL_A_LINKSTATE_EXT_SUBSTATE]	= { .type = NLA_REJECT },
};

static int linkstate_get_sqi(struct net_device *dev)
{
	struct phy_device *phydev = dev->phydev;
	int ret;

	if (!phydev)
		return -EOPNOTSUPP;

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

	return ret;
}

static int linkstate_get_sqi_max(struct net_device *dev)
{
	struct phy_device *phydev = dev->phydev;
	int ret;

	if (!phydev)
		return -EOPNOTSUPP;

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

	return ret;
};

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,
				  struct genl_info *info)
{
	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
	struct net_device *dev = reply_base->dev;
	int ret;

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

	ret = linkstate_get_sqi(dev);
	if (ret < 0 && ret != -EOPNOTSUPP)
		goto out;
	data->sqi = ret;

	ret = linkstate_get_sqi_max(dev);
	if (ret < 0 && ret != -EOPNOTSUPP)
		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;
	}

	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 (data->sqi != -EOPNOTSUPP)
		len += nla_total_size(sizeof(u32));

	if (data->sqi_max != -EOPNOTSUPP)
		len += nla_total_size(sizeof(u32));

	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 */

	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 (data->sqi != -EOPNOTSUPP &&
	    nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi))
		return -EMSGSIZE;

	if (data->sqi_max != -EOPNOTSUPP &&
	    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;
	}

	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,
	.max_attr		= ETHTOOL_A_LINKSTATE_MAX,
	.req_info_size		= sizeof(struct linkstate_req_info),
	.reply_data_size	= sizeof(struct linkstate_reply_data),
	.request_policy		= linkstate_get_policy,

	.prepare_data		= linkstate_prepare_data,
	.reply_size		= linkstate_reply_size,
	.fill_reply		= linkstate_fill_reply,
};
Commit	Line	Data
3d2b847f MK	1	// SPDX-License-Identifier: GPL-2.0-only
	2
	3	#include "netlink.h"
	4	#include "common.h"
80660219	5	#include <linux/phy.h>
3d2b847f MK	6
	7	struct linkstate_req_info {
	8	struct ethnl_req_info base;
	9	};
	10
	11	struct linkstate_reply_data {
ecc31c60 AC	12	struct ethnl_reply_data base;
	13	int link;
	14	int sqi;
	15	int sqi_max;
	16	bool link_ext_state_provided;
	17	struct ethtool_link_ext_state_info ethtool_link_ext_state_info;
3d2b847f MK	18	};
	19
	20	#define LINKSTATE_REPDATA(__reply_base) \
	21	container_of(__reply_base, struct linkstate_reply_data, base)
	22
	23	static const struct nla_policy
	24	linkstate_get_policy[ETHTOOL_A_LINKSTATE_MAX + 1] = {
	25	[ETHTOOL_A_LINKSTATE_UNSPEC] = { .type = NLA_REJECT },
	26	[ETHTOOL_A_LINKSTATE_HEADER] = { .type = NLA_NESTED },
	27	[ETHTOOL_A_LINKSTATE_LINK] = { .type = NLA_REJECT },
80660219 OR	28	[ETHTOOL_A_LINKSTATE_SQI] = { .type = NLA_REJECT },
80660219 OR	29	[ETHTOOL_A_LINKSTATE_SQI_MAX] = { .type = NLA_REJECT },
ecc31c60 AC	30	[ETHTOOL_A_LINKSTATE_EXT_STATE] = { .type = NLA_REJECT },
ecc31c60 AC	31	[ETHTOOL_A_LINKSTATE_EXT_SUBSTATE] = { .type = NLA_REJECT },
3d2b847f MK	32	};
3d2b847f MK	33
80660219 OR	34	static int linkstate_get_sqi(struct net_device *dev)
	35	{
	36	struct phy_device *phydev = dev->phydev;
	37	int ret;
	38
	39	if (!phydev)
	40	return -EOPNOTSUPP;
	41
	42	mutex_lock(&phydev->lock);
	43	if (!phydev->drv \|\| !phydev->drv->get_sqi)
	44	ret = -EOPNOTSUPP;
	45	else
	46	ret = phydev->drv->get_sqi(phydev);
	47	mutex_unlock(&phydev->lock);
	48
	49	return ret;
	50	}
	51
	52	static int linkstate_get_sqi_max(struct net_device *dev)
	53	{
	54	struct phy_device *phydev = dev->phydev;
	55	int ret;
	56
	57	if (!phydev)
	58	return -EOPNOTSUPP;
	59
	60	mutex_lock(&phydev->lock);
	61	if (!phydev->drv \|\| !phydev->drv->get_sqi_max)
	62	ret = -EOPNOTSUPP;
	63	else
	64	ret = phydev->drv->get_sqi_max(phydev);
	65	mutex_unlock(&phydev->lock);
	66
	67	return ret;
ecc31c60 AC	68	};
	69
	70	static int linkstate_get_link_ext_state(struct net_device *dev,
	71	struct linkstate_reply_data *data)
	72	{
	73	int err;
	74
	75	if (!dev->ethtool_ops->get_link_ext_state)
	76	return -EOPNOTSUPP;
	77
	78	err = dev->ethtool_ops->get_link_ext_state(dev, &data->ethtool_link_ext_state_info);
	79	if (err)
	80	return err;
	81
	82	data->link_ext_state_provided = true;
	83
	84	return 0;
80660219 OR	85	}
80660219 OR	86
3d2b847f MK	87	static int linkstate_prepare_data(const struct ethnl_req_info *req_base,
	88	struct ethnl_reply_data *reply_base,
	89	struct genl_info *info)
	90	{
	91	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
	92	struct net_device *dev = reply_base->dev;
	93	int ret;
	94
	95	ret = ethnl_ops_begin(dev);
	96	if (ret < 0)
	97	return ret;
	98	data->link = __ethtool_get_link(dev);
80660219 OR	99
	100	ret = linkstate_get_sqi(dev);
	101	if (ret < 0 && ret != -EOPNOTSUPP)
1ae71d99	102	goto out;
80660219 OR	103	data->sqi = ret;
	104
	105	ret = linkstate_get_sqi_max(dev);
	106	if (ret < 0 && ret != -EOPNOTSUPP)
1ae71d99	107	goto out;
80660219 OR	108	data->sqi_max = ret;
80660219 OR	109
ecc31c60 AC	110	if (dev->flags & IFF_UP) {
	111	ret = linkstate_get_link_ext_state(dev, data);
	112	if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA)
	113	goto out;
	114	}
	115
1ae71d99 MK	116	ret = 0;
1ae71d99 MK	117	out:
3d2b847f	118	ethnl_ops_complete(dev);
1ae71d99	119	return ret;
3d2b847f MK	120	}
	121
	122	static int linkstate_reply_size(const struct ethnl_req_info *req_base,
	123	const struct ethnl_reply_data *reply_base)
	124	{
80660219 OR	125	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
	126	int len;
	127
	128	len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */
3d2b847f	129	+ 0;
80660219 OR	130
	131	if (data->sqi != -EOPNOTSUPP)
	132	len += nla_total_size(sizeof(u32));
	133
	134	if (data->sqi_max != -EOPNOTSUPP)
	135	len += nla_total_size(sizeof(u32));
	136
ecc31c60 AC	137	if (data->link_ext_state_provided)
	138	len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */
	139
	140	if (data->ethtool_link_ext_state_info.__link_ext_substate)
	141	len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */
	142
80660219	143	return len;
3d2b847f MK	144	}
	145
	146	static int linkstate_fill_reply(struct sk_buff *skb,
	147	const struct ethnl_req_info *req_base,
	148	const struct ethnl_reply_data *reply_base)
	149	{
	150	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
	151
	152	if (data->link >= 0 &&
	153	nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link))
	154	return -EMSGSIZE;
	155
80660219 OR	156	if (data->sqi != -EOPNOTSUPP &&
	157	nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi))
	158	return -EMSGSIZE;
	159
	160	if (data->sqi_max != -EOPNOTSUPP &&
	161	nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX, data->sqi_max))
	162	return -EMSGSIZE;
	163
ecc31c60 AC	164	if (data->link_ext_state_provided) {
	165	if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE,
	166	data->ethtool_link_ext_state_info.link_ext_state))
	167	return -EMSGSIZE;
	168
	169	if (data->ethtool_link_ext_state_info.__link_ext_substate &&
	170	nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE,
	171	data->ethtool_link_ext_state_info.__link_ext_substate))
	172	return -EMSGSIZE;
	173	}
	174
3d2b847f MK	175	return 0;
	176	}
	177
	178	const struct ethnl_request_ops ethnl_linkstate_request_ops = {
	179	.request_cmd = ETHTOOL_MSG_LINKSTATE_GET,
	180	.reply_cmd = ETHTOOL_MSG_LINKSTATE_GET_REPLY,
	181	.hdr_attr = ETHTOOL_A_LINKSTATE_HEADER,
	182	.max_attr = ETHTOOL_A_LINKSTATE_MAX,
	183	.req_info_size = sizeof(struct linkstate_req_info),
	184	.reply_data_size = sizeof(struct linkstate_reply_data),
	185	.request_policy = linkstate_get_policy,
	186
	187	.prepare_data = linkstate_prepare_data,
	188	.reply_size = linkstate_reply_size,
	189	.fill_reply = linkstate_fill_reply,
	190	};