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