1// SPDX-License-Identifier: GPL-2.0-only
  2
  3#include "netlink.h"
  4#include "common.h"
  5#include <linux/phy.h>
  6
  7struct linkstate_req_info {
  8	struct ethnl_req_info		base;
  9};
 10
 11struct linkstate_reply_data {
 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;
 18};
 19
 20#define LINKSTATE_REPDATA(__reply_base) \
 21	container_of(__reply_base, struct linkstate_reply_data, base)
 22
 23static const struct nla_policy
 24linkstate_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 },
 28	[ETHTOOL_A_LINKSTATE_SQI]		= { .type = NLA_REJECT },
 29	[ETHTOOL_A_LINKSTATE_SQI_MAX]		= { .type = NLA_REJECT },
 30	[ETHTOOL_A_LINKSTATE_EXT_STATE]		= { .type = NLA_REJECT },
 31	[ETHTOOL_A_LINKSTATE_EXT_SUBSTATE]	= { .type = NLA_REJECT },
 32};
 33
 34static 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
 52static 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;
 68};
 69
 70static 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;
 85}
 86
 87static 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);
 99
100	ret = linkstate_get_sqi(dev);
101	if (ret < 0 && ret != -EOPNOTSUPP)
102		goto out;
103	data->sqi = ret;
104
105	ret = linkstate_get_sqi_max(dev);
106	if (ret < 0 && ret != -EOPNOTSUPP)
107		goto out;
108	data->sqi_max = ret;
109
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
116	ret = 0;
117out:
118	ethnl_ops_complete(dev);
119	return ret;
120}
121
122static int linkstate_reply_size(const struct ethnl_req_info *req_base,
123				const struct ethnl_reply_data *reply_base)
124{
125	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
126	int len;
127
128	len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */
129		+ 0;
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
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
143	return len;
144}
145
146static 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
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
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
175	return 0;
176}
177
178const 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};