projects/linux·blob master · refs

226 lines · 5.5 KiB

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