]>
Commit | Line | Data |
---|---|---|
f625aa9b MK |
1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | ||
3 | #include "netlink.h" | |
4 | #include "common.h" | |
5 | #include "bitset.h" | |
6 | ||
7 | struct linkmodes_req_info { | |
8 | struct ethnl_req_info base; | |
9 | }; | |
10 | ||
11 | struct linkmodes_reply_data { | |
12 | struct ethnl_reply_data base; | |
13 | struct ethtool_link_ksettings ksettings; | |
14 | struct ethtool_link_settings *lsettings; | |
15 | bool peer_empty; | |
16 | }; | |
17 | ||
18 | #define LINKMODES_REPDATA(__reply_base) \ | |
19 | container_of(__reply_base, struct linkmodes_reply_data, base) | |
20 | ||
21 | static const struct nla_policy | |
22 | linkmodes_get_policy[ETHTOOL_A_LINKMODES_MAX + 1] = { | |
23 | [ETHTOOL_A_LINKMODES_UNSPEC] = { .type = NLA_REJECT }, | |
24 | [ETHTOOL_A_LINKMODES_HEADER] = { .type = NLA_NESTED }, | |
25 | [ETHTOOL_A_LINKMODES_AUTONEG] = { .type = NLA_REJECT }, | |
26 | [ETHTOOL_A_LINKMODES_OURS] = { .type = NLA_REJECT }, | |
27 | [ETHTOOL_A_LINKMODES_PEER] = { .type = NLA_REJECT }, | |
28 | [ETHTOOL_A_LINKMODES_SPEED] = { .type = NLA_REJECT }, | |
29 | [ETHTOOL_A_LINKMODES_DUPLEX] = { .type = NLA_REJECT }, | |
30 | }; | |
31 | ||
32 | static int linkmodes_prepare_data(const struct ethnl_req_info *req_base, | |
33 | struct ethnl_reply_data *reply_base, | |
34 | struct genl_info *info) | |
35 | { | |
36 | struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base); | |
37 | struct net_device *dev = reply_base->dev; | |
38 | int ret; | |
39 | ||
40 | data->lsettings = &data->ksettings.base; | |
41 | ||
42 | ret = ethnl_ops_begin(dev); | |
43 | if (ret < 0) | |
44 | return ret; | |
45 | ||
46 | ret = __ethtool_get_link_ksettings(dev, &data->ksettings); | |
47 | if (ret < 0 && info) { | |
48 | GENL_SET_ERR_MSG(info, "failed to retrieve link settings"); | |
49 | goto out; | |
50 | } | |
51 | ||
52 | data->peer_empty = | |
53 | bitmap_empty(data->ksettings.link_modes.lp_advertising, | |
54 | __ETHTOOL_LINK_MODE_MASK_NBITS); | |
55 | ||
56 | out: | |
57 | ethnl_ops_complete(dev); | |
58 | return ret; | |
59 | } | |
60 | ||
61 | static int linkmodes_reply_size(const struct ethnl_req_info *req_base, | |
62 | const struct ethnl_reply_data *reply_base) | |
63 | { | |
64 | const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base); | |
65 | const struct ethtool_link_ksettings *ksettings = &data->ksettings; | |
66 | bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; | |
67 | int len, ret; | |
68 | ||
69 | len = nla_total_size(sizeof(u8)) /* LINKMODES_AUTONEG */ | |
70 | + nla_total_size(sizeof(u32)) /* LINKMODES_SPEED */ | |
71 | + nla_total_size(sizeof(u8)) /* LINKMODES_DUPLEX */ | |
72 | + 0; | |
73 | ret = ethnl_bitset_size(ksettings->link_modes.advertising, | |
74 | ksettings->link_modes.supported, | |
75 | __ETHTOOL_LINK_MODE_MASK_NBITS, | |
76 | link_mode_names, compact); | |
77 | if (ret < 0) | |
78 | return ret; | |
79 | len += ret; | |
80 | if (!data->peer_empty) { | |
81 | ret = ethnl_bitset_size(ksettings->link_modes.lp_advertising, | |
82 | NULL, __ETHTOOL_LINK_MODE_MASK_NBITS, | |
83 | link_mode_names, compact); | |
84 | if (ret < 0) | |
85 | return ret; | |
86 | len += ret; | |
87 | } | |
88 | ||
89 | return len; | |
90 | } | |
91 | ||
92 | static int linkmodes_fill_reply(struct sk_buff *skb, | |
93 | const struct ethnl_req_info *req_base, | |
94 | const struct ethnl_reply_data *reply_base) | |
95 | { | |
96 | const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base); | |
97 | const struct ethtool_link_ksettings *ksettings = &data->ksettings; | |
98 | const struct ethtool_link_settings *lsettings = &ksettings->base; | |
99 | bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; | |
100 | int ret; | |
101 | ||
102 | if (nla_put_u8(skb, ETHTOOL_A_LINKMODES_AUTONEG, lsettings->autoneg)) | |
103 | return -EMSGSIZE; | |
104 | ||
105 | ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_OURS, | |
106 | ksettings->link_modes.advertising, | |
107 | ksettings->link_modes.supported, | |
108 | __ETHTOOL_LINK_MODE_MASK_NBITS, link_mode_names, | |
109 | compact); | |
110 | if (ret < 0) | |
111 | return -EMSGSIZE; | |
112 | if (!data->peer_empty) { | |
113 | ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_PEER, | |
114 | ksettings->link_modes.lp_advertising, | |
115 | NULL, __ETHTOOL_LINK_MODE_MASK_NBITS, | |
116 | link_mode_names, compact); | |
117 | if (ret < 0) | |
118 | return -EMSGSIZE; | |
119 | } | |
120 | ||
121 | if (nla_put_u32(skb, ETHTOOL_A_LINKMODES_SPEED, lsettings->speed) || | |
122 | nla_put_u8(skb, ETHTOOL_A_LINKMODES_DUPLEX, lsettings->duplex)) | |
123 | return -EMSGSIZE; | |
124 | ||
125 | return 0; | |
126 | } | |
127 | ||
128 | const struct ethnl_request_ops ethnl_linkmodes_request_ops = { | |
129 | .request_cmd = ETHTOOL_MSG_LINKMODES_GET, | |
130 | .reply_cmd = ETHTOOL_MSG_LINKMODES_GET_REPLY, | |
131 | .hdr_attr = ETHTOOL_A_LINKMODES_HEADER, | |
132 | .max_attr = ETHTOOL_A_LINKMODES_MAX, | |
133 | .req_info_size = sizeof(struct linkmodes_req_info), | |
134 | .reply_data_size = sizeof(struct linkmodes_reply_data), | |
135 | .request_policy = linkmodes_get_policy, | |
136 | ||
137 | .prepare_data = linkmodes_prepare_data, | |
138 | .reply_size = linkmodes_reply_size, | |
139 | .fill_reply = linkmodes_fill_reply, | |
140 | }; | |
bfbcfe20 MK |
141 | |
142 | /* LINKMODES_SET */ | |
143 | ||
144 | struct link_mode_info { | |
145 | int speed; | |
146 | u8 duplex; | |
147 | }; | |
148 | ||
149 | #define __DEFINE_LINK_MODE_PARAMS(_speed, _type, _duplex) \ | |
150 | [ETHTOOL_LINK_MODE(_speed, _type, _duplex)] = { \ | |
151 | .speed = SPEED_ ## _speed, \ | |
152 | .duplex = __DUPLEX_ ## _duplex \ | |
153 | } | |
154 | #define __DUPLEX_Half DUPLEX_HALF | |
155 | #define __DUPLEX_Full DUPLEX_FULL | |
156 | #define __DEFINE_SPECIAL_MODE_PARAMS(_mode) \ | |
157 | [ETHTOOL_LINK_MODE_ ## _mode ## _BIT] = { \ | |
158 | .speed = SPEED_UNKNOWN, \ | |
159 | .duplex = DUPLEX_UNKNOWN, \ | |
160 | } | |
161 | ||
162 | static const struct link_mode_info link_mode_params[] = { | |
163 | __DEFINE_LINK_MODE_PARAMS(10, T, Half), | |
164 | __DEFINE_LINK_MODE_PARAMS(10, T, Full), | |
165 | __DEFINE_LINK_MODE_PARAMS(100, T, Half), | |
166 | __DEFINE_LINK_MODE_PARAMS(100, T, Full), | |
167 | __DEFINE_LINK_MODE_PARAMS(1000, T, Half), | |
168 | __DEFINE_LINK_MODE_PARAMS(1000, T, Full), | |
169 | __DEFINE_SPECIAL_MODE_PARAMS(Autoneg), | |
170 | __DEFINE_SPECIAL_MODE_PARAMS(TP), | |
171 | __DEFINE_SPECIAL_MODE_PARAMS(AUI), | |
172 | __DEFINE_SPECIAL_MODE_PARAMS(MII), | |
173 | __DEFINE_SPECIAL_MODE_PARAMS(FIBRE), | |
174 | __DEFINE_SPECIAL_MODE_PARAMS(BNC), | |
175 | __DEFINE_LINK_MODE_PARAMS(10000, T, Full), | |
176 | __DEFINE_SPECIAL_MODE_PARAMS(Pause), | |
177 | __DEFINE_SPECIAL_MODE_PARAMS(Asym_Pause), | |
178 | __DEFINE_LINK_MODE_PARAMS(2500, X, Full), | |
179 | __DEFINE_SPECIAL_MODE_PARAMS(Backplane), | |
180 | __DEFINE_LINK_MODE_PARAMS(1000, KX, Full), | |
181 | __DEFINE_LINK_MODE_PARAMS(10000, KX4, Full), | |
182 | __DEFINE_LINK_MODE_PARAMS(10000, KR, Full), | |
183 | [ETHTOOL_LINK_MODE_10000baseR_FEC_BIT] = { | |
184 | .speed = SPEED_10000, | |
185 | .duplex = DUPLEX_FULL, | |
186 | }, | |
187 | __DEFINE_LINK_MODE_PARAMS(20000, MLD2, Full), | |
188 | __DEFINE_LINK_MODE_PARAMS(20000, KR2, Full), | |
189 | __DEFINE_LINK_MODE_PARAMS(40000, KR4, Full), | |
190 | __DEFINE_LINK_MODE_PARAMS(40000, CR4, Full), | |
191 | __DEFINE_LINK_MODE_PARAMS(40000, SR4, Full), | |
192 | __DEFINE_LINK_MODE_PARAMS(40000, LR4, Full), | |
193 | __DEFINE_LINK_MODE_PARAMS(56000, KR4, Full), | |
194 | __DEFINE_LINK_MODE_PARAMS(56000, CR4, Full), | |
195 | __DEFINE_LINK_MODE_PARAMS(56000, SR4, Full), | |
196 | __DEFINE_LINK_MODE_PARAMS(56000, LR4, Full), | |
197 | __DEFINE_LINK_MODE_PARAMS(25000, CR, Full), | |
198 | __DEFINE_LINK_MODE_PARAMS(25000, KR, Full), | |
199 | __DEFINE_LINK_MODE_PARAMS(25000, SR, Full), | |
200 | __DEFINE_LINK_MODE_PARAMS(50000, CR2, Full), | |
201 | __DEFINE_LINK_MODE_PARAMS(50000, KR2, Full), | |
202 | __DEFINE_LINK_MODE_PARAMS(100000, KR4, Full), | |
203 | __DEFINE_LINK_MODE_PARAMS(100000, SR4, Full), | |
204 | __DEFINE_LINK_MODE_PARAMS(100000, CR4, Full), | |
205 | __DEFINE_LINK_MODE_PARAMS(100000, LR4_ER4, Full), | |
206 | __DEFINE_LINK_MODE_PARAMS(50000, SR2, Full), | |
207 | __DEFINE_LINK_MODE_PARAMS(1000, X, Full), | |
208 | __DEFINE_LINK_MODE_PARAMS(10000, CR, Full), | |
209 | __DEFINE_LINK_MODE_PARAMS(10000, SR, Full), | |
210 | __DEFINE_LINK_MODE_PARAMS(10000, LR, Full), | |
211 | __DEFINE_LINK_MODE_PARAMS(10000, LRM, Full), | |
212 | __DEFINE_LINK_MODE_PARAMS(10000, ER, Full), | |
213 | __DEFINE_LINK_MODE_PARAMS(2500, T, Full), | |
214 | __DEFINE_LINK_MODE_PARAMS(5000, T, Full), | |
215 | __DEFINE_SPECIAL_MODE_PARAMS(FEC_NONE), | |
216 | __DEFINE_SPECIAL_MODE_PARAMS(FEC_RS), | |
217 | __DEFINE_SPECIAL_MODE_PARAMS(FEC_BASER), | |
218 | __DEFINE_LINK_MODE_PARAMS(50000, KR, Full), | |
219 | __DEFINE_LINK_MODE_PARAMS(50000, SR, Full), | |
220 | __DEFINE_LINK_MODE_PARAMS(50000, CR, Full), | |
221 | __DEFINE_LINK_MODE_PARAMS(50000, LR_ER_FR, Full), | |
222 | __DEFINE_LINK_MODE_PARAMS(50000, DR, Full), | |
223 | __DEFINE_LINK_MODE_PARAMS(100000, KR2, Full), | |
224 | __DEFINE_LINK_MODE_PARAMS(100000, SR2, Full), | |
225 | __DEFINE_LINK_MODE_PARAMS(100000, CR2, Full), | |
226 | __DEFINE_LINK_MODE_PARAMS(100000, LR2_ER2_FR2, Full), | |
227 | __DEFINE_LINK_MODE_PARAMS(100000, DR2, Full), | |
228 | __DEFINE_LINK_MODE_PARAMS(200000, KR4, Full), | |
229 | __DEFINE_LINK_MODE_PARAMS(200000, SR4, Full), | |
230 | __DEFINE_LINK_MODE_PARAMS(200000, LR4_ER4_FR4, Full), | |
231 | __DEFINE_LINK_MODE_PARAMS(200000, DR4, Full), | |
232 | __DEFINE_LINK_MODE_PARAMS(200000, CR4, Full), | |
233 | __DEFINE_LINK_MODE_PARAMS(100, T1, Full), | |
234 | __DEFINE_LINK_MODE_PARAMS(1000, T1, Full), | |
235 | __DEFINE_LINK_MODE_PARAMS(400000, KR8, Full), | |
236 | __DEFINE_LINK_MODE_PARAMS(400000, SR8, Full), | |
237 | __DEFINE_LINK_MODE_PARAMS(400000, LR8_ER8_FR8, Full), | |
238 | __DEFINE_LINK_MODE_PARAMS(400000, DR8, Full), | |
239 | __DEFINE_LINK_MODE_PARAMS(400000, CR8, Full), | |
f623e597 | 240 | __DEFINE_SPECIAL_MODE_PARAMS(FEC_LLRS), |
bfbcfe20 MK |
241 | }; |
242 | ||
243 | static const struct nla_policy | |
244 | linkmodes_set_policy[ETHTOOL_A_LINKMODES_MAX + 1] = { | |
245 | [ETHTOOL_A_LINKMODES_UNSPEC] = { .type = NLA_REJECT }, | |
246 | [ETHTOOL_A_LINKMODES_HEADER] = { .type = NLA_NESTED }, | |
247 | [ETHTOOL_A_LINKMODES_AUTONEG] = { .type = NLA_U8 }, | |
248 | [ETHTOOL_A_LINKMODES_OURS] = { .type = NLA_NESTED }, | |
249 | [ETHTOOL_A_LINKMODES_PEER] = { .type = NLA_REJECT }, | |
250 | [ETHTOOL_A_LINKMODES_SPEED] = { .type = NLA_U32 }, | |
251 | [ETHTOOL_A_LINKMODES_DUPLEX] = { .type = NLA_U8 }, | |
252 | }; | |
253 | ||
254 | /* Set advertised link modes to all supported modes matching requested speed | |
255 | * and duplex values. Called when autonegotiation is on, speed or duplex is | |
256 | * requested but no link mode change. This is done in userspace with ioctl() | |
257 | * interface, move it into kernel for netlink. | |
258 | * Returns true if advertised modes bitmap was modified. | |
259 | */ | |
260 | static bool ethnl_auto_linkmodes(struct ethtool_link_ksettings *ksettings, | |
261 | bool req_speed, bool req_duplex) | |
262 | { | |
263 | unsigned long *advertising = ksettings->link_modes.advertising; | |
264 | unsigned long *supported = ksettings->link_modes.supported; | |
265 | DECLARE_BITMAP(old_adv, __ETHTOOL_LINK_MODE_MASK_NBITS); | |
266 | unsigned int i; | |
267 | ||
268 | BUILD_BUG_ON(ARRAY_SIZE(link_mode_params) != | |
269 | __ETHTOOL_LINK_MODE_MASK_NBITS); | |
270 | ||
271 | bitmap_copy(old_adv, advertising, __ETHTOOL_LINK_MODE_MASK_NBITS); | |
272 | ||
273 | for (i = 0; i < __ETHTOOL_LINK_MODE_MASK_NBITS; i++) { | |
274 | const struct link_mode_info *info = &link_mode_params[i]; | |
275 | ||
276 | if (info->speed == SPEED_UNKNOWN) | |
277 | continue; | |
278 | if (test_bit(i, supported) && | |
279 | (!req_speed || info->speed == ksettings->base.speed) && | |
280 | (!req_duplex || info->duplex == ksettings->base.duplex)) | |
281 | set_bit(i, advertising); | |
282 | else | |
283 | clear_bit(i, advertising); | |
284 | } | |
285 | ||
286 | return !bitmap_equal(old_adv, advertising, | |
287 | __ETHTOOL_LINK_MODE_MASK_NBITS); | |
288 | } | |
289 | ||
290 | static int ethnl_update_linkmodes(struct genl_info *info, struct nlattr **tb, | |
291 | struct ethtool_link_ksettings *ksettings, | |
292 | bool *mod) | |
293 | { | |
294 | struct ethtool_link_settings *lsettings = &ksettings->base; | |
295 | bool req_speed, req_duplex; | |
296 | int ret; | |
297 | ||
298 | *mod = false; | |
299 | req_speed = tb[ETHTOOL_A_LINKMODES_SPEED]; | |
300 | req_duplex = tb[ETHTOOL_A_LINKMODES_DUPLEX]; | |
301 | ||
302 | ethnl_update_u8(&lsettings->autoneg, tb[ETHTOOL_A_LINKMODES_AUTONEG], | |
303 | mod); | |
304 | ret = ethnl_update_bitset(ksettings->link_modes.advertising, | |
305 | __ETHTOOL_LINK_MODE_MASK_NBITS, | |
306 | tb[ETHTOOL_A_LINKMODES_OURS], link_mode_names, | |
307 | info->extack, mod); | |
308 | if (ret < 0) | |
309 | return ret; | |
310 | ethnl_update_u32(&lsettings->speed, tb[ETHTOOL_A_LINKMODES_SPEED], | |
311 | mod); | |
312 | ethnl_update_u8(&lsettings->duplex, tb[ETHTOOL_A_LINKMODES_DUPLEX], | |
313 | mod); | |
314 | ||
315 | if (!tb[ETHTOOL_A_LINKMODES_OURS] && lsettings->autoneg && | |
316 | (req_speed || req_duplex) && | |
317 | ethnl_auto_linkmodes(ksettings, req_speed, req_duplex)) | |
318 | *mod = true; | |
319 | ||
320 | return 0; | |
321 | } | |
322 | ||
323 | int ethnl_set_linkmodes(struct sk_buff *skb, struct genl_info *info) | |
324 | { | |
325 | struct nlattr *tb[ETHTOOL_A_LINKMODES_MAX + 1]; | |
326 | struct ethtool_link_ksettings ksettings = {}; | |
bfbcfe20 MK |
327 | struct ethnl_req_info req_info = {}; |
328 | struct net_device *dev; | |
329 | bool mod = false; | |
330 | int ret; | |
331 | ||
332 | ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, | |
333 | ETHTOOL_A_LINKMODES_MAX, linkmodes_set_policy, | |
334 | info->extack); | |
335 | if (ret < 0) | |
336 | return ret; | |
98130546 MK |
337 | ret = ethnl_parse_header_dev_get(&req_info, |
338 | tb[ETHTOOL_A_LINKMODES_HEADER], | |
339 | genl_info_net(info), info->extack, | |
340 | true); | |
bfbcfe20 MK |
341 | if (ret < 0) |
342 | return ret; | |
343 | dev = req_info.dev; | |
2f599ec4 | 344 | ret = -EOPNOTSUPP; |
bfbcfe20 MK |
345 | if (!dev->ethtool_ops->get_link_ksettings || |
346 | !dev->ethtool_ops->set_link_ksettings) | |
2f599ec4 | 347 | goto out_dev; |
bfbcfe20 MK |
348 | |
349 | rtnl_lock(); | |
350 | ret = ethnl_ops_begin(dev); | |
351 | if (ret < 0) | |
352 | goto out_rtnl; | |
353 | ||
354 | ret = __ethtool_get_link_ksettings(dev, &ksettings); | |
355 | if (ret < 0) { | |
356 | if (info) | |
357 | GENL_SET_ERR_MSG(info, "failed to retrieve link settings"); | |
358 | goto out_ops; | |
359 | } | |
bfbcfe20 MK |
360 | |
361 | ret = ethnl_update_linkmodes(info, tb, &ksettings, &mod); | |
362 | if (ret < 0) | |
363 | goto out_ops; | |
364 | ||
365 | if (mod) { | |
366 | ret = dev->ethtool_ops->set_link_ksettings(dev, &ksettings); | |
367 | if (ret < 0) | |
368 | GENL_SET_ERR_MSG(info, "link settings update failed"); | |
1b1b1847 MK |
369 | else |
370 | ethtool_notify(dev, ETHTOOL_MSG_LINKMODES_NTF, NULL); | |
bfbcfe20 MK |
371 | } |
372 | ||
373 | out_ops: | |
374 | ethnl_ops_complete(dev); | |
375 | out_rtnl: | |
376 | rtnl_unlock(); | |
2f599ec4 | 377 | out_dev: |
bfbcfe20 MK |
378 | dev_put(dev); |
379 | return ret; | |
380 | } |