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
linkstate_get_sqi(struct net_device * dev)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 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
linkstate_get_sqi_max(struct net_device * dev)49 static int linkstate_get_sqi_max(struct net_device *dev)
50 {
51 struct phy_device *phydev = dev->phydev;
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
linkstate_sqi_critical_error(int sqi)69 static bool linkstate_sqi_critical_error(int sqi)
70 {
71 return sqi < 0 && sqi != -EOPNOTSUPP && sqi != -ENETDOWN;
72 }
73
linkstate_sqi_valid(struct linkstate_reply_data * data)74 static 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
linkstate_get_link_ext_state(struct net_device * dev,struct linkstate_reply_data * data)80 static 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
linkstate_prepare_data(const struct ethnl_req_info * req_base,struct ethnl_reply_data * reply_base,const struct genl_info * info)97 static 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 int ret;
104
105 ret = ethnl_ops_begin(dev);
106 if (ret < 0)
107 return ret;
108 data->link = __ethtool_get_link(dev);
109
110 ret = linkstate_get_sqi(dev);
111 if (linkstate_sqi_critical_error(ret))
112 goto out;
113 data->sqi = ret;
114
115 ret = linkstate_get_sqi_max(dev);
116 if (linkstate_sqi_critical_error(ret))
117 goto out;
118 data->sqi_max = ret;
119
120 if (dev->flags & IFF_UP) {
121 ret = linkstate_get_link_ext_state(dev, data);
122 if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA)
123 goto out;
124 }
125
126 ethtool_stats_init((u64 *)&data->link_stats,
127 sizeof(data->link_stats) / 8);
128
129 if (req_base->flags & ETHTOOL_FLAG_STATS) {
130 if (dev->phydev)
131 data->link_stats.link_down_events =
132 READ_ONCE(dev->phydev->link_down_events);
133
134 if (dev->ethtool_ops->get_link_ext_stats)
135 dev->ethtool_ops->get_link_ext_stats(dev,
136 &data->link_stats);
137 }
138
139 ret = 0;
140 out:
141 ethnl_ops_complete(dev);
142 return ret;
143 }
144
linkstate_reply_size(const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)145 static int linkstate_reply_size(const struct ethnl_req_info *req_base,
146 const struct ethnl_reply_data *reply_base)
147 {
148 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
149 int len;
150
151 len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */
152 + 0;
153
154 if (linkstate_sqi_valid(data)) {
155 len += nla_total_size(sizeof(u32)); /* LINKSTATE_SQI */
156 len += nla_total_size(sizeof(u32)); /* LINKSTATE_SQI_MAX */
157 }
158
159 if (data->link_ext_state_provided)
160 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */
161
162 if (data->ethtool_link_ext_state_info.__link_ext_substate)
163 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */
164
165 if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET)
166 len += nla_total_size(sizeof(u32));
167
168 return len;
169 }
170
linkstate_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)171 static int linkstate_fill_reply(struct sk_buff *skb,
172 const struct ethnl_req_info *req_base,
173 const struct ethnl_reply_data *reply_base)
174 {
175 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
176
177 if (data->link >= 0 &&
178 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link))
179 return -EMSGSIZE;
180
181 if (linkstate_sqi_valid(data)) {
182 if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi))
183 return -EMSGSIZE;
184
185 if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX,
186 data->sqi_max))
187 return -EMSGSIZE;
188 }
189
190 if (data->link_ext_state_provided) {
191 if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE,
192 data->ethtool_link_ext_state_info.link_ext_state))
193 return -EMSGSIZE;
194
195 if (data->ethtool_link_ext_state_info.__link_ext_substate &&
196 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE,
197 data->ethtool_link_ext_state_info.__link_ext_substate))
198 return -EMSGSIZE;
199 }
200
201 if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET)
202 if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_EXT_DOWN_CNT,
203 data->link_stats.link_down_events))
204 return -EMSGSIZE;
205
206 return 0;
207 }
208
209 const struct ethnl_request_ops ethnl_linkstate_request_ops = {
210 .request_cmd = ETHTOOL_MSG_LINKSTATE_GET,
211 .reply_cmd = ETHTOOL_MSG_LINKSTATE_GET_REPLY,
212 .hdr_attr = ETHTOOL_A_LINKSTATE_HEADER,
213 .req_info_size = sizeof(struct linkstate_req_info),
214 .reply_data_size = sizeof(struct linkstate_reply_data),
215
216 .prepare_data = linkstate_prepare_data,
217 .reply_size = linkstate_reply_size,
218 .fill_reply = linkstate_fill_reply,
219 };
220