1// SPDX-License-Identifier: GPL-2.0-only
  2
  3#include "netlink.h"
  4#include "common.h"
  5#include <linux/phy.h>
  6#include <linux/phylib_stubs.h>
  7
  8struct linkstate_req_info {
  9	struct ethnl_req_info		base;
 10};
 11
 12struct linkstate_reply_data {
 13	struct ethnl_reply_data			base;
 14	int					link;
 15	int					sqi;
 16	int					sqi_max;
 17	struct ethtool_link_ext_stats		link_stats;
 18	bool					link_ext_state_provided;
 19	struct ethtool_link_ext_state_info	ethtool_link_ext_state_info;
 20};
 21
 22#define LINKSTATE_REPDATA(__reply_base) \
 23	container_of(__reply_base, struct linkstate_reply_data, base)
 24
 25const struct nla_policy ethnl_linkstate_get_policy[] = {
 26	[ETHTOOL_A_LINKSTATE_HEADER]		=
 27		NLA_POLICY_NESTED(ethnl_header_policy_stats),
 28};
 29
 30static int linkstate_get_sqi(struct phy_device *phydev)
 31{
 32	int ret;
 33
 34	if (!phydev)
 35		return -EOPNOTSUPP;
 36
 37	mutex_lock(&phydev->lock);
 38	if (!phydev->drv || !phydev->drv->get_sqi)
 39		ret = -EOPNOTSUPP;
 40	else if (!phydev->link)
 41		ret = -ENETDOWN;
 42	else
 43		ret = phydev->drv->get_sqi(phydev);
 44	mutex_unlock(&phydev->lock);
 45
 46	return ret;
 47}
 48
 49static int linkstate_get_sqi_max(struct phy_device *phydev)
 50{
 51	int ret;
 52
 53	if (!phydev)
 54		return -EOPNOTSUPP;
 55
 56	mutex_lock(&phydev->lock);
 57	if (!phydev->drv || !phydev->drv->get_sqi_max)
 58		ret = -EOPNOTSUPP;
 59	else if (!phydev->link)
 60		ret = -ENETDOWN;
 61	else
 62		ret = phydev->drv->get_sqi_max(phydev);
 63	mutex_unlock(&phydev->lock);
 64
 65	return ret;
 66};
 67
 68static bool linkstate_sqi_critical_error(int sqi)
 69{
 70	return sqi < 0 && sqi != -EOPNOTSUPP && sqi != -ENETDOWN;
 71}
 72
 73static bool linkstate_sqi_valid(struct linkstate_reply_data *data)
 74{
 75	return data->sqi >= 0 && data->sqi_max >= 0 &&
 76	       data->sqi <= data->sqi_max;
 77}
 78
 79static int linkstate_get_link_ext_state(struct net_device *dev,
 80					struct linkstate_reply_data *data)
 81{
 82	int err;
 83
 84	if (!dev->ethtool_ops->get_link_ext_state)
 85		return -EOPNOTSUPP;
 86
 87	err = dev->ethtool_ops->get_link_ext_state(dev, &data->ethtool_link_ext_state_info);
 88	if (err)
 89		return err;
 90
 91	data->link_ext_state_provided = true;
 92
 93	return 0;
 94}
 95
 96static int linkstate_prepare_data(const struct ethnl_req_info *req_base,
 97				  struct ethnl_reply_data *reply_base,
 98				  const struct genl_info *info)
 99{
100	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
101	struct net_device *dev = reply_base->dev;
102	struct nlattr **tb = info->attrs;
103	struct phy_device *phydev;
104	int ret;
105
106	phydev = ethnl_req_get_phydev(req_base, tb, ETHTOOL_A_LINKSTATE_HEADER,
107				      info->extack);
108	if (IS_ERR(phydev)) {
109		ret = PTR_ERR(phydev);
110		goto out;
111	}
112
113	ret = ethnl_ops_begin(dev);
114	if (ret < 0)
115		return ret;
116	data->link = __ethtool_get_link(dev);
117
118	ret = linkstate_get_sqi(phydev);
119	if (linkstate_sqi_critical_error(ret))
120		goto out;
121	data->sqi = ret;
122
123	ret = linkstate_get_sqi_max(phydev);
124	if (linkstate_sqi_critical_error(ret))
125		goto out;
126	data->sqi_max = ret;
127
128	if (dev->flags & IFF_UP) {
129		ret = linkstate_get_link_ext_state(dev, data);
130		if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA)
131			goto out;
132	}
133
134	ethtool_stats_init((u64 *)&data->link_stats,
135			   sizeof(data->link_stats) / 8);
136
137	if (req_base->flags & ETHTOOL_FLAG_STATS) {
138		if (phydev)
139			phy_ethtool_get_link_ext_stats(phydev,
140						       &data->link_stats);
141
142		if (dev->ethtool_ops->get_link_ext_stats)
143			dev->ethtool_ops->get_link_ext_stats(dev,
144							     &data->link_stats);
145	}
146
147	ret = 0;
148out:
149	ethnl_ops_complete(dev);
150	return ret;
151}
152
153static int linkstate_reply_size(const struct ethnl_req_info *req_base,
154				const struct ethnl_reply_data *reply_base)
155{
156	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
157	int len;
158
159	len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */
160		+ 0;
161
162	if (linkstate_sqi_valid(data)) {
163		len += nla_total_size(sizeof(u32)); /* LINKSTATE_SQI */
164		len += nla_total_size(sizeof(u32)); /* LINKSTATE_SQI_MAX */
165	}
166
167	if (data->link_ext_state_provided)
168		len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */
169
170	if (data->ethtool_link_ext_state_info.__link_ext_substate)
171		len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */
172
173	if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET)
174		len += nla_total_size(sizeof(u32));
175
176	return len;
177}
178
179static int linkstate_fill_reply(struct sk_buff *skb,
180				const struct ethnl_req_info *req_base,
181				const struct ethnl_reply_data *reply_base)
182{
183	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
184
185	if (data->link >= 0 &&
186	    nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link))
187		return -EMSGSIZE;
188
189	if (linkstate_sqi_valid(data)) {
190		if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi))
191			return -EMSGSIZE;
192
193		if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX,
194				data->sqi_max))
195			return -EMSGSIZE;
196	}
197
198	if (data->link_ext_state_provided) {
199		if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE,
200			       data->ethtool_link_ext_state_info.link_ext_state))
201			return -EMSGSIZE;
202
203		if (data->ethtool_link_ext_state_info.__link_ext_substate &&
204		    nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE,
205			       data->ethtool_link_ext_state_info.__link_ext_substate))
206			return -EMSGSIZE;
207	}
208
209	if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET)
210		if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_EXT_DOWN_CNT,
211				data->link_stats.link_down_events))
212			return -EMSGSIZE;
213
214	return 0;
215}
216
217const struct ethnl_request_ops ethnl_linkstate_request_ops = {
218	.request_cmd		= ETHTOOL_MSG_LINKSTATE_GET,
219	.reply_cmd		= ETHTOOL_MSG_LINKSTATE_GET_REPLY,
220	.hdr_attr		= ETHTOOL_A_LINKSTATE_HEADER,
221	.req_info_size		= sizeof(struct linkstate_req_info),
222	.reply_data_size	= sizeof(struct linkstate_reply_data),
223
224	.prepare_data		= linkstate_prepare_data,
225	.reply_size		= linkstate_reply_size,
226	.fill_reply		= linkstate_fill_reply,
227};