1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | |
3 | #include "netlink.h" |
4 | #include "common.h" |
5 | #include "bitset.h" |
6 | |
7 | /* LINKMODES_GET */ |
8 | |
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 | |
23 | const struct nla_policy ethnl_linkmodes_get_policy[] = { |
24 | [ETHTOOL_A_LINKMODES_HEADER] = |
25 | NLA_POLICY_NESTED(ethnl_header_policy), |
26 | }; |
27 | |
28 | static int linkmodes_prepare_data(const struct ethnl_req_info *req_base, |
29 | struct ethnl_reply_data *reply_base, |
30 | const 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, link_ksettings: &data->ksettings); |
43 | if (ret < 0 && info) { |
44 | GENL_SET_ERR_MSG(info, "failed to retrieve link settings" ); |
45 | goto out; |
46 | } |
47 | |
48 | if (!dev->ethtool_ops->cap_link_lanes_supported) |
49 | data->ksettings.lanes = 0; |
50 | |
51 | data->peer_empty = |
52 | bitmap_empty(src: data->ksettings.link_modes.lp_advertising, |
53 | nbits: __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; |
65 | const struct ethtool_link_settings *lsettings = &ksettings->base; |
66 | bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS; |
67 | int len, ret; |
68 | |
69 | len = nla_total_size(payload: sizeof(u8)) /* LINKMODES_AUTONEG */ |
70 | + nla_total_size(payload: sizeof(u32)) /* LINKMODES_SPEED */ |
71 | + nla_total_size(payload: sizeof(u32)) /* LINKMODES_LANES */ |
72 | + nla_total_size(payload: sizeof(u8)) /* LINKMODES_DUPLEX */ |
73 | + nla_total_size(payload: sizeof(u8)) /* LINKMODES_RATE_MATCHING */ |
74 | + 0; |
75 | ret = ethnl_bitset_size(val: ksettings->link_modes.advertising, |
76 | mask: ksettings->link_modes.supported, |
77 | nbits: __ETHTOOL_LINK_MODE_MASK_NBITS, |
78 | names: link_mode_names, compact); |
79 | if (ret < 0) |
80 | return ret; |
81 | len += ret; |
82 | if (!data->peer_empty) { |
83 | ret = ethnl_bitset_size(val: ksettings->link_modes.lp_advertising, |
84 | NULL, nbits: __ETHTOOL_LINK_MODE_MASK_NBITS, |
85 | names: link_mode_names, compact); |
86 | if (ret < 0) |
87 | return ret; |
88 | len += ret; |
89 | } |
90 | |
91 | if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED) |
92 | len += nla_total_size(payload: sizeof(u8)); |
93 | |
94 | if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED) |
95 | len += nla_total_size(payload: sizeof(u8)); |
96 | |
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, attrtype: ETHTOOL_A_LINKMODES_AUTONEG, value: lsettings->autoneg)) |
111 | return -EMSGSIZE; |
112 | |
113 | ret = ethnl_put_bitset(skb, attrtype: ETHTOOL_A_LINKMODES_OURS, |
114 | val: ksettings->link_modes.advertising, |
115 | mask: ksettings->link_modes.supported, |
116 | nbits: __ETHTOOL_LINK_MODE_MASK_NBITS, names: link_mode_names, |
117 | compact); |
118 | if (ret < 0) |
119 | return -EMSGSIZE; |
120 | if (!data->peer_empty) { |
121 | ret = ethnl_put_bitset(skb, attrtype: ETHTOOL_A_LINKMODES_PEER, |
122 | val: ksettings->link_modes.lp_advertising, |
123 | NULL, nbits: __ETHTOOL_LINK_MODE_MASK_NBITS, |
124 | names: link_mode_names, compact); |
125 | if (ret < 0) |
126 | return -EMSGSIZE; |
127 | } |
128 | |
129 | if (nla_put_u32(skb, attrtype: ETHTOOL_A_LINKMODES_SPEED, value: lsettings->speed) || |
130 | nla_put_u8(skb, attrtype: ETHTOOL_A_LINKMODES_DUPLEX, value: lsettings->duplex)) |
131 | return -EMSGSIZE; |
132 | |
133 | if (ksettings->lanes && |
134 | nla_put_u32(skb, attrtype: ETHTOOL_A_LINKMODES_LANES, value: ksettings->lanes)) |
135 | return -EMSGSIZE; |
136 | |
137 | if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED && |
138 | nla_put_u8(skb, attrtype: ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG, |
139 | value: lsettings->master_slave_cfg)) |
140 | return -EMSGSIZE; |
141 | |
142 | if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED && |
143 | nla_put_u8(skb, attrtype: ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE, |
144 | value: lsettings->master_slave_state)) |
145 | return -EMSGSIZE; |
146 | |
147 | if (nla_put_u8(skb, attrtype: ETHTOOL_A_LINKMODES_RATE_MATCHING, |
148 | value: lsettings->rate_matching)) |
149 | return -EMSGSIZE; |
150 | |
151 | return 0; |
152 | } |
153 | |
154 | /* LINKMODES_SET */ |
155 | |
156 | const struct nla_policy ethnl_linkmodes_set_policy[] = { |
157 | [ETHTOOL_A_LINKMODES_HEADER] = |
158 | NLA_POLICY_NESTED(ethnl_header_policy), |
159 | [ETHTOOL_A_LINKMODES_AUTONEG] = { .type = NLA_U8 }, |
160 | [ETHTOOL_A_LINKMODES_OURS] = { .type = NLA_NESTED }, |
161 | [ETHTOOL_A_LINKMODES_SPEED] = { .type = NLA_U32 }, |
162 | [ETHTOOL_A_LINKMODES_DUPLEX] = { .type = NLA_U8 }, |
163 | [ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG] = { .type = NLA_U8 }, |
164 | [ETHTOOL_A_LINKMODES_LANES] = NLA_POLICY_RANGE(NLA_U32, 1, 8), |
165 | }; |
166 | |
167 | /* Set advertised link modes to all supported modes matching requested speed, |
168 | * lanes and duplex values. Called when autonegotiation is on, speed, lanes or |
169 | * duplex is requested but no link mode change. This is done in userspace with |
170 | * ioctl() interface, move it into kernel for netlink. |
171 | * Returns true if advertised modes bitmap was modified. |
172 | */ |
173 | static bool ethnl_auto_linkmodes(struct ethtool_link_ksettings *ksettings, |
174 | bool req_speed, bool req_lanes, bool req_duplex) |
175 | { |
176 | unsigned long *advertising = ksettings->link_modes.advertising; |
177 | unsigned long *supported = ksettings->link_modes.supported; |
178 | DECLARE_BITMAP(old_adv, __ETHTOOL_LINK_MODE_MASK_NBITS); |
179 | unsigned int i; |
180 | |
181 | bitmap_copy(dst: old_adv, src: advertising, nbits: __ETHTOOL_LINK_MODE_MASK_NBITS); |
182 | |
183 | for (i = 0; i < __ETHTOOL_LINK_MODE_MASK_NBITS; i++) { |
184 | const struct link_mode_info *info = &link_mode_params[i]; |
185 | |
186 | if (info->speed == SPEED_UNKNOWN) |
187 | continue; |
188 | if (test_bit(i, supported) && |
189 | (!req_speed || info->speed == ksettings->base.speed) && |
190 | (!req_lanes || info->lanes == ksettings->lanes) && |
191 | (!req_duplex || info->duplex == ksettings->base.duplex)) |
192 | set_bit(nr: i, addr: advertising); |
193 | else |
194 | clear_bit(nr: i, addr: advertising); |
195 | } |
196 | |
197 | return !bitmap_equal(src1: old_adv, src2: advertising, |
198 | nbits: __ETHTOOL_LINK_MODE_MASK_NBITS); |
199 | } |
200 | |
201 | static bool ethnl_validate_master_slave_cfg(u8 cfg) |
202 | { |
203 | switch (cfg) { |
204 | case MASTER_SLAVE_CFG_MASTER_PREFERRED: |
205 | case MASTER_SLAVE_CFG_SLAVE_PREFERRED: |
206 | case MASTER_SLAVE_CFG_MASTER_FORCE: |
207 | case MASTER_SLAVE_CFG_SLAVE_FORCE: |
208 | return true; |
209 | } |
210 | |
211 | return false; |
212 | } |
213 | |
214 | static int ethnl_check_linkmodes(struct genl_info *info, struct nlattr **tb) |
215 | { |
216 | const struct nlattr *master_slave_cfg, *lanes_cfg; |
217 | |
218 | master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG]; |
219 | if (master_slave_cfg && |
220 | !ethnl_validate_master_slave_cfg(cfg: nla_get_u8(nla: master_slave_cfg))) { |
221 | NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg, |
222 | "master/slave value is invalid" ); |
223 | return -EOPNOTSUPP; |
224 | } |
225 | |
226 | lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES]; |
227 | if (lanes_cfg && !is_power_of_2(n: nla_get_u32(nla: lanes_cfg))) { |
228 | NL_SET_ERR_MSG_ATTR(info->extack, lanes_cfg, |
229 | "lanes value is invalid" ); |
230 | return -EINVAL; |
231 | } |
232 | |
233 | return 0; |
234 | } |
235 | |
236 | static int ethnl_update_linkmodes(struct genl_info *info, struct nlattr **tb, |
237 | struct ethtool_link_ksettings *ksettings, |
238 | bool *mod, const struct net_device *dev) |
239 | { |
240 | struct ethtool_link_settings *lsettings = &ksettings->base; |
241 | bool req_speed, req_lanes, req_duplex; |
242 | const struct nlattr *master_slave_cfg, *lanes_cfg; |
243 | int ret; |
244 | |
245 | master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG]; |
246 | if (master_slave_cfg) { |
247 | if (lsettings->master_slave_cfg == MASTER_SLAVE_CFG_UNSUPPORTED) { |
248 | NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg, |
249 | "master/slave configuration not supported by device" ); |
250 | return -EOPNOTSUPP; |
251 | } |
252 | } |
253 | |
254 | *mod = false; |
255 | req_speed = tb[ETHTOOL_A_LINKMODES_SPEED]; |
256 | req_lanes = tb[ETHTOOL_A_LINKMODES_LANES]; |
257 | req_duplex = tb[ETHTOOL_A_LINKMODES_DUPLEX]; |
258 | |
259 | ethnl_update_u8(dst: &lsettings->autoneg, attr: tb[ETHTOOL_A_LINKMODES_AUTONEG], |
260 | mod); |
261 | |
262 | lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES]; |
263 | if (lanes_cfg) { |
264 | /* If autoneg is off and lanes parameter is not supported by the |
265 | * driver, return an error. |
266 | */ |
267 | if (!lsettings->autoneg && |
268 | !dev->ethtool_ops->cap_link_lanes_supported) { |
269 | NL_SET_ERR_MSG_ATTR(info->extack, lanes_cfg, |
270 | "lanes configuration not supported by device" ); |
271 | return -EOPNOTSUPP; |
272 | } |
273 | } else if (!lsettings->autoneg && ksettings->lanes) { |
274 | /* If autoneg is off and lanes parameter is not passed from user but |
275 | * it was defined previously then set the lanes parameter to 0. |
276 | */ |
277 | ksettings->lanes = 0; |
278 | *mod = true; |
279 | } |
280 | |
281 | ret = ethnl_update_bitset(bitmap: ksettings->link_modes.advertising, |
282 | nbits: __ETHTOOL_LINK_MODE_MASK_NBITS, |
283 | attr: tb[ETHTOOL_A_LINKMODES_OURS], names: link_mode_names, |
284 | extack: info->extack, mod); |
285 | if (ret < 0) |
286 | return ret; |
287 | ethnl_update_u32(dst: &lsettings->speed, attr: tb[ETHTOOL_A_LINKMODES_SPEED], |
288 | mod); |
289 | ethnl_update_u32(dst: &ksettings->lanes, attr: lanes_cfg, mod); |
290 | ethnl_update_u8(dst: &lsettings->duplex, attr: tb[ETHTOOL_A_LINKMODES_DUPLEX], |
291 | mod); |
292 | ethnl_update_u8(dst: &lsettings->master_slave_cfg, attr: master_slave_cfg, mod); |
293 | |
294 | if (!tb[ETHTOOL_A_LINKMODES_OURS] && lsettings->autoneg && |
295 | (req_speed || req_lanes || req_duplex) && |
296 | ethnl_auto_linkmodes(ksettings, req_speed, req_lanes, req_duplex)) |
297 | *mod = true; |
298 | |
299 | return 0; |
300 | } |
301 | |
302 | static int |
303 | ethnl_set_linkmodes_validate(struct ethnl_req_info *req_info, |
304 | struct genl_info *info) |
305 | { |
306 | const struct ethtool_ops *ops = req_info->dev->ethtool_ops; |
307 | int ret; |
308 | |
309 | ret = ethnl_check_linkmodes(info, tb: info->attrs); |
310 | if (ret < 0) |
311 | return ret; |
312 | |
313 | if (!ops->get_link_ksettings || !ops->set_link_ksettings) |
314 | return -EOPNOTSUPP; |
315 | return 1; |
316 | } |
317 | |
318 | static int |
319 | ethnl_set_linkmodes(struct ethnl_req_info *req_info, struct genl_info *info) |
320 | { |
321 | struct ethtool_link_ksettings ksettings = {}; |
322 | struct net_device *dev = req_info->dev; |
323 | struct nlattr **tb = info->attrs; |
324 | bool mod = false; |
325 | int ret; |
326 | |
327 | ret = __ethtool_get_link_ksettings(dev, link_ksettings: &ksettings); |
328 | if (ret < 0) { |
329 | GENL_SET_ERR_MSG(info, "failed to retrieve link settings" ); |
330 | return ret; |
331 | } |
332 | |
333 | ret = ethnl_update_linkmodes(info, tb, ksettings: &ksettings, mod: &mod, dev); |
334 | if (ret < 0) |
335 | return ret; |
336 | if (!mod) |
337 | return 0; |
338 | |
339 | ret = dev->ethtool_ops->set_link_ksettings(dev, &ksettings); |
340 | if (ret < 0) { |
341 | GENL_SET_ERR_MSG(info, "link settings update failed" ); |
342 | return ret; |
343 | } |
344 | |
345 | return 1; |
346 | } |
347 | |
348 | const struct ethnl_request_ops ethnl_linkmodes_request_ops = { |
349 | .request_cmd = ETHTOOL_MSG_LINKMODES_GET, |
350 | .reply_cmd = ETHTOOL_MSG_LINKMODES_GET_REPLY, |
351 | .hdr_attr = ETHTOOL_A_LINKMODES_HEADER, |
352 | .req_info_size = sizeof(struct linkmodes_req_info), |
353 | .reply_data_size = sizeof(struct linkmodes_reply_data), |
354 | |
355 | .prepare_data = linkmodes_prepare_data, |
356 | .reply_size = linkmodes_reply_size, |
357 | .fill_reply = linkmodes_fill_reply, |
358 | |
359 | .set_validate = ethnl_set_linkmodes_validate, |
360 | .set = ethnl_set_linkmodes, |
361 | .set_ntf_cmd = ETHTOOL_MSG_LINKMODES_NTF, |
362 | }; |
363 | |