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
 23const struct nla_policy ethnl_linkstate_get_policy[] = {
 24	[ETHTOOL_A_LINKSTATE_HEADER]		=
 25		NLA_POLICY_NESTED(ethnl_header_policy),
 26};
 27
 28static int linkstate_get_sqi(struct net_device *dev)
 29{
 30	struct phy_device *phydev = dev->phydev;
 31	int ret;
 32
 33	if (!phydev)
 34		return -EOPNOTSUPP;
 35
 36	mutex_lock(&phydev->lock);
 37	if (!phydev->drv || !phydev->drv->get_sqi)
 38		ret = -EOPNOTSUPP;
 39	else
 40		ret = phydev->drv->get_sqi(phydev);
 41	mutex_unlock(&phydev->lock);
 42
 43	return ret;
 44}
 45
 46static int linkstate_get_sqi_max(struct net_device *dev)
 47{
 48	struct phy_device *phydev = dev->phydev;
 49	int ret;
 50
 51	if (!phydev)
 52		return -EOPNOTSUPP;
 53
 54	mutex_lock(&phydev->lock);
 55	if (!phydev->drv || !phydev->drv->get_sqi_max)
 56		ret = -EOPNOTSUPP;
 57	else
 58		ret = phydev->drv->get_sqi_max(phydev);
 59	mutex_unlock(&phydev->lock);
 60
 61	return ret;
 62};
 63
 64static int linkstate_get_link_ext_state(struct net_device *dev,
 65					struct linkstate_reply_data *data)
 66{
 67	int err;
 68
 69	if (!dev->ethtool_ops->get_link_ext_state)
 70		return -EOPNOTSUPP;
 71
 72	err = dev->ethtool_ops->get_link_ext_state(dev, &data->ethtool_link_ext_state_info);
 73	if (err)
 74		return err;
 75
 76	data->link_ext_state_provided = true;
 77
 78	return 0;
 79}
 80
 81static int linkstate_prepare_data(const struct ethnl_req_info *req_base,
 82				  struct ethnl_reply_data *reply_base,
 83				  struct genl_info *info)
 84{
 85	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
 86	struct net_device *dev = reply_base->dev;
 87	int ret;
 88
 89	ret = ethnl_ops_begin(dev);
 90	if (ret < 0)
 91		return ret;
 92	data->link = __ethtool_get_link(dev);
 93
 94	ret = linkstate_get_sqi(dev);
 95	if (ret < 0 && ret != -EOPNOTSUPP)
 96		goto out;
 97	data->sqi = ret;
 98
 99	ret = linkstate_get_sqi_max(dev);
100	if (ret < 0 && ret != -EOPNOTSUPP)
101		goto out;
102	data->sqi_max = ret;
103
104	if (dev->flags & IFF_UP) {
105		ret = linkstate_get_link_ext_state(dev, data);
106		if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA)
107			goto out;
108	}
109
110	ret = 0;
111out:
112	ethnl_ops_complete(dev);
113	return ret;
114}
115
116static int linkstate_reply_size(const struct ethnl_req_info *req_base,
117				const struct ethnl_reply_data *reply_base)
118{
119	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
120	int len;
121
122	len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */
123		+ 0;
124
125	if (data->sqi != -EOPNOTSUPP)
126		len += nla_total_size(sizeof(u32));
127
128	if (data->sqi_max != -EOPNOTSUPP)
129		len += nla_total_size(sizeof(u32));
130
131	if (data->link_ext_state_provided)
132		len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */
133
134	if (data->ethtool_link_ext_state_info.__link_ext_substate)
135		len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */
136
137	return len;
138}
139
140static int linkstate_fill_reply(struct sk_buff *skb,
141				const struct ethnl_req_info *req_base,
142				const struct ethnl_reply_data *reply_base)
143{
144	struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
145
146	if (data->link >= 0 &&
147	    nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link))
148		return -EMSGSIZE;
149
150	if (data->sqi != -EOPNOTSUPP &&
151	    nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi))
152		return -EMSGSIZE;
153
154	if (data->sqi_max != -EOPNOTSUPP &&
155	    nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX, data->sqi_max))
156		return -EMSGSIZE;
157
158	if (data->link_ext_state_provided) {
159		if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE,
160			       data->ethtool_link_ext_state_info.link_ext_state))
161			return -EMSGSIZE;
162
163		if (data->ethtool_link_ext_state_info.__link_ext_substate &&
164		    nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE,
165			       data->ethtool_link_ext_state_info.__link_ext_substate))
166			return -EMSGSIZE;
167	}
168
169	return 0;
170}
171
172const struct ethnl_request_ops ethnl_linkstate_request_ops = {
173	.request_cmd		= ETHTOOL_MSG_LINKSTATE_GET,
174	.reply_cmd		= ETHTOOL_MSG_LINKSTATE_GET_REPLY,
175	.hdr_attr		= ETHTOOL_A_LINKSTATE_HEADER,
176	.req_info_size		= sizeof(struct linkstate_req_info),
177	.reply_data_size	= sizeof(struct linkstate_reply_data),
178
179	.prepare_data		= linkstate_prepare_data,
180	.reply_size		= linkstate_reply_size,
181	.fill_reply		= linkstate_fill_reply,
182};