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 | ||
c8907043 DR |
7 | /* LINKMODES_GET */ |
8 | ||
f625aa9b MK |
9 | struct linkmodes_req_info { |
10 | struct ethnl_req_info base; | |
11 | }; | |
12 | ||
13 | struct linkmodes_reply_data { | |
14 | struct ethnl_reply_data base; | |
15 | struct ethtool_link_ksettings ksettings; | |
16 | struct ethtool_link_settings *lsettings; | |
17 | bool peer_empty; | |
18 | }; | |
19 | ||
20 | #define LINKMODES_REPDATA(__reply_base) \ | |
21 | container_of(__reply_base, struct linkmodes_reply_data, base) | |
22 | ||
ff419afa | 23 | const struct nla_policy ethnl_linkmodes_get_policy[] = { |
329d9c33 JK |
24 | [ETHTOOL_A_LINKMODES_HEADER] = |
25 | NLA_POLICY_NESTED(ethnl_header_policy), | |
f625aa9b MK |
26 | }; |
27 | ||
28 | static int linkmodes_prepare_data(const struct ethnl_req_info *req_base, | |
29 | struct ethnl_reply_data *reply_base, | |
30 | struct genl_info *info) | |
31 | { | |
32 | struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base); | |
33 | struct net_device *dev = reply_base->dev; | |
34 | int ret; | |
35 | ||
36 | data->lsettings = &data->ksettings.base; | |
37 | ||
38 | ret = ethnl_ops_begin(dev); | |
39 | if (ret < 0) | |
40 | return ret; | |
41 | ||
42 | ret = __ethtool_get_link_ksettings(dev, &data->ksettings); | |
43 | if (ret < 0 && info) { | |
44 | GENL_SET_ERR_MSG(info, "failed to retrieve link settings"); | |
45 | goto out; | |
46 | } | |
47 | ||
7dc33f09 DR |
48 | if (!dev->ethtool_ops->cap_link_lanes_supported) |
49 | data->ksettings.lanes = 0; | |
50 | ||
f625aa9b MK |
51 | data->peer_empty = |
52 | bitmap_empty(data->ksettings.link_modes.lp_advertising, | |
53 | __ETHTOOL_LINK_MODE_MASK_NBITS); | |
54 | ||
55 | out: | |
56 | ethnl_ops_complete(dev); | |
57 | return ret; | |
58 | } | |
59 | ||
60 | static int linkmodes_reply_size(const struct ethnl_req_info *req_base, | |
61 | const struct ethnl_reply_data *reply_base) | |
62 | { | |
63 | const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base); | |
64 | const struct ethtool_link_ksettings *ksettings = &data->ksettings; | |
bdbdac76 | 65 | const struct ethtool_link_settings *lsettings = &ksettings->base; |
f625aa9b MK |
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 */ | |
7dc33f09 | 71 | + nla_total_size(sizeof(u32)) /* LINKMODES_LANES */ |
f625aa9b | 72 | + nla_total_size(sizeof(u8)) /* LINKMODES_DUPLEX */ |
0c3e10cb | 73 | + nla_total_size(sizeof(u8)) /* LINKMODES_RATE_MATCHING */ |
f625aa9b MK |
74 | + 0; |
75 | ret = ethnl_bitset_size(ksettings->link_modes.advertising, | |
76 | ksettings->link_modes.supported, | |
77 | __ETHTOOL_LINK_MODE_MASK_NBITS, | |
78 | link_mode_names, compact); | |
79 | if (ret < 0) | |
80 | return ret; | |
81 | len += ret; | |
82 | if (!data->peer_empty) { | |
83 | ret = ethnl_bitset_size(ksettings->link_modes.lp_advertising, | |
84 | NULL, __ETHTOOL_LINK_MODE_MASK_NBITS, | |
85 | link_mode_names, compact); | |
86 | if (ret < 0) | |
87 | return ret; | |
88 | len += ret; | |
89 | } | |
90 | ||
bdbdac76 OR |
91 | if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED) |
92 | len += nla_total_size(sizeof(u8)); | |
93 | ||
94 | if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED) | |
95 | len += nla_total_size(sizeof(u8)); | |
96 | ||
f625aa9b MK |
97 | return len; |
98 | } | |
99 | ||
100 | static int linkmodes_fill_reply(struct sk_buff *skb, | |
101 | const struct ethnl_req_info *req_base, | |
102 | const struct ethnl_reply_data *reply_base) | |
103 | { | |
104 | const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base); | |
105 | const struct ethtool_link_ksettings *ksettings = &data->ksettings; | |
106 | const struct ethtool_link_settings *lsettings = &ksettings->base; | |
107 | bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; | |
108 | int ret; | |
109 | ||
110 | if (nla_put_u8(skb, ETHTOOL_A_LINKMODES_AUTONEG, lsettings->autoneg)) | |
111 | return -EMSGSIZE; | |
112 | ||
113 | ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_OURS, | |
114 | ksettings->link_modes.advertising, | |
115 | ksettings->link_modes.supported, | |
116 | __ETHTOOL_LINK_MODE_MASK_NBITS, link_mode_names, | |
117 | compact); | |
118 | if (ret < 0) | |
119 | return -EMSGSIZE; | |
120 | if (!data->peer_empty) { | |
121 | ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_PEER, | |
122 | ksettings->link_modes.lp_advertising, | |
123 | NULL, __ETHTOOL_LINK_MODE_MASK_NBITS, | |
124 | link_mode_names, compact); | |
125 | if (ret < 0) | |
126 | return -EMSGSIZE; | |
127 | } | |
128 | ||
129 | if (nla_put_u32(skb, ETHTOOL_A_LINKMODES_SPEED, lsettings->speed) || | |
130 | nla_put_u8(skb, ETHTOOL_A_LINKMODES_DUPLEX, lsettings->duplex)) | |
131 | return -EMSGSIZE; | |
132 | ||
7dc33f09 DR |
133 | if (ksettings->lanes && |
134 | nla_put_u32(skb, ETHTOOL_A_LINKMODES_LANES, ksettings->lanes)) | |
135 | return -EMSGSIZE; | |
136 | ||
bdbdac76 OR |
137 | if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED && |
138 | nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG, | |
139 | lsettings->master_slave_cfg)) | |
140 | return -EMSGSIZE; | |
141 | ||
142 | if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED && | |
143 | nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE, | |
144 | lsettings->master_slave_state)) | |
145 | return -EMSGSIZE; | |
146 | ||
0c3e10cb SA |
147 | if (nla_put_u8(skb, ETHTOOL_A_LINKMODES_RATE_MATCHING, |
148 | lsettings->rate_matching)) | |
149 | return -EMSGSIZE; | |
150 | ||
f625aa9b MK |
151 | return 0; |
152 | } | |
153 | ||
154 | const struct ethnl_request_ops ethnl_linkmodes_request_ops = { | |
155 | .request_cmd = ETHTOOL_MSG_LINKMODES_GET, | |
156 | .reply_cmd = ETHTOOL_MSG_LINKMODES_GET_REPLY, | |
157 | .hdr_attr = ETHTOOL_A_LINKMODES_HEADER, | |
f625aa9b MK |
158 | .req_info_size = sizeof(struct linkmodes_req_info), |
159 | .reply_data_size = sizeof(struct linkmodes_reply_data), | |
f625aa9b MK |
160 | |
161 | .prepare_data = linkmodes_prepare_data, | |
162 | .reply_size = linkmodes_reply_size, | |
163 | .fill_reply = linkmodes_fill_reply, | |
164 | }; | |
bfbcfe20 MK |
165 | |
166 | /* LINKMODES_SET */ | |
167 | ||
ff419afa | 168 | const struct nla_policy ethnl_linkmodes_set_policy[] = { |
329d9c33 JK |
169 | [ETHTOOL_A_LINKMODES_HEADER] = |
170 | NLA_POLICY_NESTED(ethnl_header_policy), | |
bfbcfe20 MK |
171 | [ETHTOOL_A_LINKMODES_AUTONEG] = { .type = NLA_U8 }, |
172 | [ETHTOOL_A_LINKMODES_OURS] = { .type = NLA_NESTED }, | |
bfbcfe20 MK |
173 | [ETHTOOL_A_LINKMODES_SPEED] = { .type = NLA_U32 }, |
174 | [ETHTOOL_A_LINKMODES_DUPLEX] = { .type = NLA_U8 }, | |
bdbdac76 | 175 | [ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG] = { .type = NLA_U8 }, |
012ce4dd | 176 | [ETHTOOL_A_LINKMODES_LANES] = NLA_POLICY_RANGE(NLA_U32, 1, 8), |
bfbcfe20 MK |
177 | }; |
178 | ||
012ce4dd DR |
179 | /* Set advertised link modes to all supported modes matching requested speed, |
180 | * lanes and duplex values. Called when autonegotiation is on, speed, lanes or | |
181 | * duplex is requested but no link mode change. This is done in userspace with | |
182 | * ioctl() interface, move it into kernel for netlink. | |
bfbcfe20 MK |
183 | * Returns true if advertised modes bitmap was modified. |
184 | */ | |
185 | static bool ethnl_auto_linkmodes(struct ethtool_link_ksettings *ksettings, | |
012ce4dd | 186 | bool req_speed, bool req_lanes, bool req_duplex) |
bfbcfe20 MK |
187 | { |
188 | unsigned long *advertising = ksettings->link_modes.advertising; | |
189 | unsigned long *supported = ksettings->link_modes.supported; | |
190 | DECLARE_BITMAP(old_adv, __ETHTOOL_LINK_MODE_MASK_NBITS); | |
191 | unsigned int i; | |
192 | ||
bfbcfe20 MK |
193 | bitmap_copy(old_adv, advertising, __ETHTOOL_LINK_MODE_MASK_NBITS); |
194 | ||
195 | for (i = 0; i < __ETHTOOL_LINK_MODE_MASK_NBITS; i++) { | |
196 | const struct link_mode_info *info = &link_mode_params[i]; | |
197 | ||
198 | if (info->speed == SPEED_UNKNOWN) | |
199 | continue; | |
200 | if (test_bit(i, supported) && | |
201 | (!req_speed || info->speed == ksettings->base.speed) && | |
012ce4dd | 202 | (!req_lanes || info->lanes == ksettings->lanes) && |
bfbcfe20 MK |
203 | (!req_duplex || info->duplex == ksettings->base.duplex)) |
204 | set_bit(i, advertising); | |
205 | else | |
206 | clear_bit(i, advertising); | |
207 | } | |
208 | ||
209 | return !bitmap_equal(old_adv, advertising, | |
210 | __ETHTOOL_LINK_MODE_MASK_NBITS); | |
211 | } | |
212 | ||
bdbdac76 OR |
213 | static bool ethnl_validate_master_slave_cfg(u8 cfg) |
214 | { | |
215 | switch (cfg) { | |
216 | case MASTER_SLAVE_CFG_MASTER_PREFERRED: | |
217 | case MASTER_SLAVE_CFG_SLAVE_PREFERRED: | |
218 | case MASTER_SLAVE_CFG_MASTER_FORCE: | |
219 | case MASTER_SLAVE_CFG_SLAVE_FORCE: | |
220 | return true; | |
221 | } | |
222 | ||
223 | return false; | |
224 | } | |
225 | ||
189e7a8d DR |
226 | static int ethnl_check_linkmodes(struct genl_info *info, struct nlattr **tb) |
227 | { | |
012ce4dd | 228 | const struct nlattr *master_slave_cfg, *lanes_cfg; |
189e7a8d DR |
229 | |
230 | master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG]; | |
231 | if (master_slave_cfg && | |
232 | !ethnl_validate_master_slave_cfg(nla_get_u8(master_slave_cfg))) { | |
233 | NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg, | |
234 | "master/slave value is invalid"); | |
235 | return -EOPNOTSUPP; | |
236 | } | |
237 | ||
012ce4dd DR |
238 | lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES]; |
239 | if (lanes_cfg && !is_power_of_2(nla_get_u32(lanes_cfg))) { | |
240 | NL_SET_ERR_MSG_ATTR(info->extack, lanes_cfg, | |
241 | "lanes value is invalid"); | |
242 | return -EINVAL; | |
243 | } | |
244 | ||
189e7a8d DR |
245 | return 0; |
246 | } | |
247 | ||
bfbcfe20 MK |
248 | static int ethnl_update_linkmodes(struct genl_info *info, struct nlattr **tb, |
249 | struct ethtool_link_ksettings *ksettings, | |
012ce4dd | 250 | bool *mod, const struct net_device *dev) |
bfbcfe20 MK |
251 | { |
252 | struct ethtool_link_settings *lsettings = &ksettings->base; | |
012ce4dd DR |
253 | bool req_speed, req_lanes, req_duplex; |
254 | const struct nlattr *master_slave_cfg, *lanes_cfg; | |
bfbcfe20 MK |
255 | int ret; |
256 | ||
bdbdac76 OR |
257 | master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG]; |
258 | if (master_slave_cfg) { | |
bdbdac76 OR |
259 | if (lsettings->master_slave_cfg == MASTER_SLAVE_CFG_UNSUPPORTED) { |
260 | NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg, | |
261 | "master/slave configuration not supported by device"); | |
262 | return -EOPNOTSUPP; | |
263 | } | |
bdbdac76 OR |
264 | } |
265 | ||
bfbcfe20 MK |
266 | *mod = false; |
267 | req_speed = tb[ETHTOOL_A_LINKMODES_SPEED]; | |
012ce4dd | 268 | req_lanes = tb[ETHTOOL_A_LINKMODES_LANES]; |
bfbcfe20 MK |
269 | req_duplex = tb[ETHTOOL_A_LINKMODES_DUPLEX]; |
270 | ||
271 | ethnl_update_u8(&lsettings->autoneg, tb[ETHTOOL_A_LINKMODES_AUTONEG], | |
272 | mod); | |
012ce4dd DR |
273 | |
274 | lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES]; | |
275 | if (lanes_cfg) { | |
276 | /* If autoneg is off and lanes parameter is not supported by the | |
277 | * driver, return an error. | |
278 | */ | |
279 | if (!lsettings->autoneg && | |
280 | !dev->ethtool_ops->cap_link_lanes_supported) { | |
281 | NL_SET_ERR_MSG_ATTR(info->extack, lanes_cfg, | |
282 | "lanes configuration not supported by device"); | |
283 | return -EOPNOTSUPP; | |
284 | } | |
285 | } else if (!lsettings->autoneg) { | |
286 | /* If autoneg is off and lanes parameter is not passed from user, | |
287 | * set the lanes parameter to 0. | |
288 | */ | |
289 | ksettings->lanes = 0; | |
290 | } | |
291 | ||
bfbcfe20 MK |
292 | ret = ethnl_update_bitset(ksettings->link_modes.advertising, |
293 | __ETHTOOL_LINK_MODE_MASK_NBITS, | |
294 | tb[ETHTOOL_A_LINKMODES_OURS], link_mode_names, | |
295 | info->extack, mod); | |
296 | if (ret < 0) | |
297 | return ret; | |
298 | ethnl_update_u32(&lsettings->speed, tb[ETHTOOL_A_LINKMODES_SPEED], | |
299 | mod); | |
012ce4dd | 300 | ethnl_update_u32(&ksettings->lanes, lanes_cfg, mod); |
bfbcfe20 MK |
301 | ethnl_update_u8(&lsettings->duplex, tb[ETHTOOL_A_LINKMODES_DUPLEX], |
302 | mod); | |
bdbdac76 | 303 | ethnl_update_u8(&lsettings->master_slave_cfg, master_slave_cfg, mod); |
bfbcfe20 MK |
304 | |
305 | if (!tb[ETHTOOL_A_LINKMODES_OURS] && lsettings->autoneg && | |
012ce4dd DR |
306 | (req_speed || req_lanes || req_duplex) && |
307 | ethnl_auto_linkmodes(ksettings, req_speed, req_lanes, req_duplex)) | |
bfbcfe20 MK |
308 | *mod = true; |
309 | ||
310 | return 0; | |
311 | } | |
312 | ||
313 | int ethnl_set_linkmodes(struct sk_buff *skb, struct genl_info *info) | |
314 | { | |
bfbcfe20 | 315 | struct ethtool_link_ksettings ksettings = {}; |
bfbcfe20 | 316 | struct ethnl_req_info req_info = {}; |
5028588b | 317 | struct nlattr **tb = info->attrs; |
bfbcfe20 MK |
318 | struct net_device *dev; |
319 | bool mod = false; | |
320 | int ret; | |
321 | ||
189e7a8d DR |
322 | ret = ethnl_check_linkmodes(info, tb); |
323 | if (ret < 0) | |
324 | return ret; | |
325 | ||
98130546 MK |
326 | ret = ethnl_parse_header_dev_get(&req_info, |
327 | tb[ETHTOOL_A_LINKMODES_HEADER], | |
328 | genl_info_net(info), info->extack, | |
329 | true); | |
bfbcfe20 MK |
330 | if (ret < 0) |
331 | return ret; | |
332 | dev = req_info.dev; | |
2f599ec4 | 333 | ret = -EOPNOTSUPP; |
bfbcfe20 MK |
334 | if (!dev->ethtool_ops->get_link_ksettings || |
335 | !dev->ethtool_ops->set_link_ksettings) | |
2f599ec4 | 336 | goto out_dev; |
bfbcfe20 MK |
337 | |
338 | rtnl_lock(); | |
339 | ret = ethnl_ops_begin(dev); | |
340 | if (ret < 0) | |
341 | goto out_rtnl; | |
342 | ||
343 | ret = __ethtool_get_link_ksettings(dev, &ksettings); | |
344 | if (ret < 0) { | |
71fed0bc | 345 | GENL_SET_ERR_MSG(info, "failed to retrieve link settings"); |
bfbcfe20 MK |
346 | goto out_ops; |
347 | } | |
bfbcfe20 | 348 | |
012ce4dd | 349 | ret = ethnl_update_linkmodes(info, tb, &ksettings, &mod, dev); |
bfbcfe20 MK |
350 | if (ret < 0) |
351 | goto out_ops; | |
352 | ||
353 | if (mod) { | |
354 | ret = dev->ethtool_ops->set_link_ksettings(dev, &ksettings); | |
355 | if (ret < 0) | |
356 | GENL_SET_ERR_MSG(info, "link settings update failed"); | |
1b1b1847 MK |
357 | else |
358 | ethtool_notify(dev, ETHTOOL_MSG_LINKMODES_NTF, NULL); | |
bfbcfe20 MK |
359 | } |
360 | ||
361 | out_ops: | |
362 | ethnl_ops_complete(dev); | |
363 | out_rtnl: | |
364 | rtnl_unlock(); | |
2f599ec4 | 365 | out_dev: |
34ac17ec | 366 | ethnl_parse_header_dev_put(&req_info); |
bfbcfe20 MK |
367 | return ret; |
368 | } |