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
41 ret = phydev->drv->get_sqi(phydev);
42 mutex_unlock(&phydev->lock);
43
44 return ret;
45 }
46
linkstate_get_sqi_max(struct net_device * dev)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
linkstate_get_link_ext_state(struct net_device * dev,struct linkstate_reply_data * data)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
linkstate_prepare_data(const struct ethnl_req_info * req_base,struct ethnl_reply_data * reply_base,const struct genl_info * info)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
linkstate_reply_size(const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)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
linkstate_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)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