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