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(lock: &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(lock: &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(stats: (u64 *)&data->link_stats, |
112 | n: 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(payload: sizeof(u8)) /* LINKSTATE_LINK */ |
137 | + 0; |
138 | |
139 | if (data->sqi != -EOPNOTSUPP) |
140 | len += nla_total_size(payload: sizeof(u32)); |
141 | |
142 | if (data->sqi_max != -EOPNOTSUPP) |
143 | len += nla_total_size(payload: sizeof(u32)); |
144 | |
145 | if (data->link_ext_state_provided) |
146 | len += nla_total_size(payload: sizeof(u8)); /* LINKSTATE_EXT_STATE */ |
147 | |
148 | if (data->ethtool_link_ext_state_info.__link_ext_substate) |
149 | len += nla_total_size(payload: sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */ |
150 | |
151 | if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET) |
152 | len += nla_total_size(payload: 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, attrtype: ETHTOOL_A_LINKSTATE_LINK, value: !!data->link)) |
165 | return -EMSGSIZE; |
166 | |
167 | if (data->sqi != -EOPNOTSUPP && |
168 | nla_put_u32(skb, attrtype: ETHTOOL_A_LINKSTATE_SQI, value: data->sqi)) |
169 | return -EMSGSIZE; |
170 | |
171 | if (data->sqi_max != -EOPNOTSUPP && |
172 | nla_put_u32(skb, attrtype: ETHTOOL_A_LINKSTATE_SQI_MAX, value: data->sqi_max)) |
173 | return -EMSGSIZE; |
174 | |
175 | if (data->link_ext_state_provided) { |
176 | if (nla_put_u8(skb, attrtype: ETHTOOL_A_LINKSTATE_EXT_STATE, |
177 | value: 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, attrtype: ETHTOOL_A_LINKSTATE_EXT_SUBSTATE, |
182 | value: 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, attrtype: ETHTOOL_A_LINKSTATE_EXT_DOWN_CNT, |
188 | value: 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 | |