1 // SPDX-License-Identifier: GPL-2.0-only
9 struct linkmodes_req_info
{
10 struct ethnl_req_info base
;
13 struct linkmodes_reply_data
{
14 struct ethnl_reply_data base
;
15 struct ethtool_link_ksettings ksettings
;
16 struct ethtool_link_settings
*lsettings
;
20 #define LINKMODES_REPDATA(__reply_base) \
21 container_of(__reply_base, struct linkmodes_reply_data, base)
23 const struct nla_policy ethnl_linkmodes_get_policy
[] = {
24 [ETHTOOL_A_LINKMODES_HEADER
] =
25 NLA_POLICY_NESTED(ethnl_header_policy
),
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
)
32 struct linkmodes_reply_data
*data
= LINKMODES_REPDATA(reply_base
);
33 struct net_device
*dev
= reply_base
->dev
;
36 data
->lsettings
= &data
->ksettings
.base
;
38 ret
= ethnl_ops_begin(dev
);
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");
48 if (!dev
->ethtool_ops
->cap_link_lanes_supported
)
49 data
->ksettings
.lanes
= 0;
52 bitmap_empty(data
->ksettings
.link_modes
.lp_advertising
,
53 __ETHTOOL_LINK_MODE_MASK_NBITS
);
56 ethnl_ops_complete(dev
);
60 static int linkmodes_reply_size(const struct ethnl_req_info
*req_base
,
61 const struct ethnl_reply_data
*reply_base
)
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
;
69 len
= nla_total_size(sizeof(u8
)) /* LINKMODES_AUTONEG */
70 + nla_total_size(sizeof(u32
)) /* LINKMODES_SPEED */
71 + nla_total_size(sizeof(u32
)) /* LINKMODES_LANES */
72 + nla_total_size(sizeof(u8
)) /* LINKMODES_DUPLEX */
74 ret
= ethnl_bitset_size(ksettings
->link_modes
.advertising
,
75 ksettings
->link_modes
.supported
,
76 __ETHTOOL_LINK_MODE_MASK_NBITS
,
77 link_mode_names
, compact
);
81 if (!data
->peer_empty
) {
82 ret
= ethnl_bitset_size(ksettings
->link_modes
.lp_advertising
,
83 NULL
, __ETHTOOL_LINK_MODE_MASK_NBITS
,
84 link_mode_names
, compact
);
90 if (lsettings
->master_slave_cfg
!= MASTER_SLAVE_CFG_UNSUPPORTED
)
91 len
+= nla_total_size(sizeof(u8
));
93 if (lsettings
->master_slave_state
!= MASTER_SLAVE_STATE_UNSUPPORTED
)
94 len
+= nla_total_size(sizeof(u8
));
99 static int linkmodes_fill_reply(struct sk_buff
*skb
,
100 const struct ethnl_req_info
*req_base
,
101 const struct ethnl_reply_data
*reply_base
)
103 const struct linkmodes_reply_data
*data
= LINKMODES_REPDATA(reply_base
);
104 const struct ethtool_link_ksettings
*ksettings
= &data
->ksettings
;
105 const struct ethtool_link_settings
*lsettings
= &ksettings
->base
;
106 bool compact
= req_base
->flags
& ETHTOOL_FLAG_COMPACT_BITSETS
;
109 if (nla_put_u8(skb
, ETHTOOL_A_LINKMODES_AUTONEG
, lsettings
->autoneg
))
112 ret
= ethnl_put_bitset(skb
, ETHTOOL_A_LINKMODES_OURS
,
113 ksettings
->link_modes
.advertising
,
114 ksettings
->link_modes
.supported
,
115 __ETHTOOL_LINK_MODE_MASK_NBITS
, link_mode_names
,
119 if (!data
->peer_empty
) {
120 ret
= ethnl_put_bitset(skb
, ETHTOOL_A_LINKMODES_PEER
,
121 ksettings
->link_modes
.lp_advertising
,
122 NULL
, __ETHTOOL_LINK_MODE_MASK_NBITS
,
123 link_mode_names
, compact
);
128 if (nla_put_u32(skb
, ETHTOOL_A_LINKMODES_SPEED
, lsettings
->speed
) ||
129 nla_put_u8(skb
, ETHTOOL_A_LINKMODES_DUPLEX
, lsettings
->duplex
))
132 if (ksettings
->lanes
&&
133 nla_put_u32(skb
, ETHTOOL_A_LINKMODES_LANES
, ksettings
->lanes
))
136 if (lsettings
->master_slave_cfg
!= MASTER_SLAVE_CFG_UNSUPPORTED
&&
137 nla_put_u8(skb
, ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG
,
138 lsettings
->master_slave_cfg
))
141 if (lsettings
->master_slave_state
!= MASTER_SLAVE_STATE_UNSUPPORTED
&&
142 nla_put_u8(skb
, ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE
,
143 lsettings
->master_slave_state
))
149 const struct ethnl_request_ops ethnl_linkmodes_request_ops
= {
150 .request_cmd
= ETHTOOL_MSG_LINKMODES_GET
,
151 .reply_cmd
= ETHTOOL_MSG_LINKMODES_GET_REPLY
,
152 .hdr_attr
= ETHTOOL_A_LINKMODES_HEADER
,
153 .req_info_size
= sizeof(struct linkmodes_req_info
),
154 .reply_data_size
= sizeof(struct linkmodes_reply_data
),
156 .prepare_data
= linkmodes_prepare_data
,
157 .reply_size
= linkmodes_reply_size
,
158 .fill_reply
= linkmodes_fill_reply
,
163 const struct nla_policy ethnl_linkmodes_set_policy
[] = {
164 [ETHTOOL_A_LINKMODES_HEADER
] =
165 NLA_POLICY_NESTED(ethnl_header_policy
),
166 [ETHTOOL_A_LINKMODES_AUTONEG
] = { .type
= NLA_U8
},
167 [ETHTOOL_A_LINKMODES_OURS
] = { .type
= NLA_NESTED
},
168 [ETHTOOL_A_LINKMODES_SPEED
] = { .type
= NLA_U32
},
169 [ETHTOOL_A_LINKMODES_DUPLEX
] = { .type
= NLA_U8
},
170 [ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG
] = { .type
= NLA_U8
},
171 [ETHTOOL_A_LINKMODES_LANES
] = NLA_POLICY_RANGE(NLA_U32
, 1, 8),
174 /* Set advertised link modes to all supported modes matching requested speed,
175 * lanes and duplex values. Called when autonegotiation is on, speed, lanes or
176 * duplex is requested but no link mode change. This is done in userspace with
177 * ioctl() interface, move it into kernel for netlink.
178 * Returns true if advertised modes bitmap was modified.
180 static bool ethnl_auto_linkmodes(struct ethtool_link_ksettings
*ksettings
,
181 bool req_speed
, bool req_lanes
, bool req_duplex
)
183 unsigned long *advertising
= ksettings
->link_modes
.advertising
;
184 unsigned long *supported
= ksettings
->link_modes
.supported
;
185 DECLARE_BITMAP(old_adv
, __ETHTOOL_LINK_MODE_MASK_NBITS
);
188 bitmap_copy(old_adv
, advertising
, __ETHTOOL_LINK_MODE_MASK_NBITS
);
190 for (i
= 0; i
< __ETHTOOL_LINK_MODE_MASK_NBITS
; i
++) {
191 const struct link_mode_info
*info
= &link_mode_params
[i
];
193 if (info
->speed
== SPEED_UNKNOWN
)
195 if (test_bit(i
, supported
) &&
196 (!req_speed
|| info
->speed
== ksettings
->base
.speed
) &&
197 (!req_lanes
|| info
->lanes
== ksettings
->lanes
) &&
198 (!req_duplex
|| info
->duplex
== ksettings
->base
.duplex
))
199 set_bit(i
, advertising
);
201 clear_bit(i
, advertising
);
204 return !bitmap_equal(old_adv
, advertising
,
205 __ETHTOOL_LINK_MODE_MASK_NBITS
);
208 static bool ethnl_validate_master_slave_cfg(u8 cfg
)
211 case MASTER_SLAVE_CFG_MASTER_PREFERRED
:
212 case MASTER_SLAVE_CFG_SLAVE_PREFERRED
:
213 case MASTER_SLAVE_CFG_MASTER_FORCE
:
214 case MASTER_SLAVE_CFG_SLAVE_FORCE
:
221 static int ethnl_check_linkmodes(struct genl_info
*info
, struct nlattr
**tb
)
223 const struct nlattr
*master_slave_cfg
, *lanes_cfg
;
225 master_slave_cfg
= tb
[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG
];
226 if (master_slave_cfg
&&
227 !ethnl_validate_master_slave_cfg(nla_get_u8(master_slave_cfg
))) {
228 NL_SET_ERR_MSG_ATTR(info
->extack
, master_slave_cfg
,
229 "master/slave value is invalid");
233 lanes_cfg
= tb
[ETHTOOL_A_LINKMODES_LANES
];
234 if (lanes_cfg
&& !is_power_of_2(nla_get_u32(lanes_cfg
))) {
235 NL_SET_ERR_MSG_ATTR(info
->extack
, lanes_cfg
,
236 "lanes value is invalid");
243 static int ethnl_update_linkmodes(struct genl_info
*info
, struct nlattr
**tb
,
244 struct ethtool_link_ksettings
*ksettings
,
245 bool *mod
, const struct net_device
*dev
)
247 struct ethtool_link_settings
*lsettings
= &ksettings
->base
;
248 bool req_speed
, req_lanes
, req_duplex
;
249 const struct nlattr
*master_slave_cfg
, *lanes_cfg
;
252 master_slave_cfg
= tb
[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG
];
253 if (master_slave_cfg
) {
254 if (lsettings
->master_slave_cfg
== MASTER_SLAVE_CFG_UNSUPPORTED
) {
255 NL_SET_ERR_MSG_ATTR(info
->extack
, master_slave_cfg
,
256 "master/slave configuration not supported by device");
262 req_speed
= tb
[ETHTOOL_A_LINKMODES_SPEED
];
263 req_lanes
= tb
[ETHTOOL_A_LINKMODES_LANES
];
264 req_duplex
= tb
[ETHTOOL_A_LINKMODES_DUPLEX
];
266 ethnl_update_u8(&lsettings
->autoneg
, tb
[ETHTOOL_A_LINKMODES_AUTONEG
],
269 lanes_cfg
= tb
[ETHTOOL_A_LINKMODES_LANES
];
271 /* If autoneg is off and lanes parameter is not supported by the
272 * driver, return an error.
274 if (!lsettings
->autoneg
&&
275 !dev
->ethtool_ops
->cap_link_lanes_supported
) {
276 NL_SET_ERR_MSG_ATTR(info
->extack
, lanes_cfg
,
277 "lanes configuration not supported by device");
280 } else if (!lsettings
->autoneg
) {
281 /* If autoneg is off and lanes parameter is not passed from user,
282 * set the lanes parameter to 0.
284 ksettings
->lanes
= 0;
287 ret
= ethnl_update_bitset(ksettings
->link_modes
.advertising
,
288 __ETHTOOL_LINK_MODE_MASK_NBITS
,
289 tb
[ETHTOOL_A_LINKMODES_OURS
], link_mode_names
,
293 ethnl_update_u32(&lsettings
->speed
, tb
[ETHTOOL_A_LINKMODES_SPEED
],
295 ethnl_update_u32(&ksettings
->lanes
, lanes_cfg
, mod
);
296 ethnl_update_u8(&lsettings
->duplex
, tb
[ETHTOOL_A_LINKMODES_DUPLEX
],
298 ethnl_update_u8(&lsettings
->master_slave_cfg
, master_slave_cfg
, mod
);
300 if (!tb
[ETHTOOL_A_LINKMODES_OURS
] && lsettings
->autoneg
&&
301 (req_speed
|| req_lanes
|| req_duplex
) &&
302 ethnl_auto_linkmodes(ksettings
, req_speed
, req_lanes
, req_duplex
))
308 int ethnl_set_linkmodes(struct sk_buff
*skb
, struct genl_info
*info
)
310 struct ethtool_link_ksettings ksettings
= {};
311 struct ethnl_req_info req_info
= {};
312 struct nlattr
**tb
= info
->attrs
;
313 struct net_device
*dev
;
317 ret
= ethnl_check_linkmodes(info
, tb
);
321 ret
= ethnl_parse_header_dev_get(&req_info
,
322 tb
[ETHTOOL_A_LINKMODES_HEADER
],
323 genl_info_net(info
), info
->extack
,
329 if (!dev
->ethtool_ops
->get_link_ksettings
||
330 !dev
->ethtool_ops
->set_link_ksettings
)
334 ret
= ethnl_ops_begin(dev
);
338 ret
= __ethtool_get_link_ksettings(dev
, &ksettings
);
340 GENL_SET_ERR_MSG(info
, "failed to retrieve link settings");
344 ret
= ethnl_update_linkmodes(info
, tb
, &ksettings
, &mod
, dev
);
349 ret
= dev
->ethtool_ops
->set_link_ksettings(dev
, &ksettings
);
351 GENL_SET_ERR_MSG(info
, "link settings update failed");
353 ethtool_notify(dev
, ETHTOOL_MSG_LINKMODES_NTF
, NULL
);
357 ethnl_ops_complete(dev
);