1f625aa9bSMichal Kubecek// SPDX-License-Identifier: GPL-2.0-only
2f625aa9bSMichal Kubecek
3f625aa9bSMichal Kubecek#include "netlink.h"
4f625aa9bSMichal Kubecek#include "common.h"
5f625aa9bSMichal Kubecek#include "bitset.h"
6f625aa9bSMichal Kubecek
7c8907043SDanielle Ratson/* LINKMODES_GET */
8c8907043SDanielle Ratson
9f625aa9bSMichal Kubecekstruct linkmodes_req_info {
10f625aa9bSMichal Kubecek	struct ethnl_req_info		base;
11f625aa9bSMichal Kubecek};
12f625aa9bSMichal Kubecek
13f625aa9bSMichal Kubecekstruct linkmodes_reply_data {
14f625aa9bSMichal Kubecek	struct ethnl_reply_data		base;
15f625aa9bSMichal Kubecek	struct ethtool_link_ksettings	ksettings;
16f625aa9bSMichal Kubecek	struct ethtool_link_settings	*lsettings;
17f625aa9bSMichal Kubecek	bool				peer_empty;
18f625aa9bSMichal Kubecek};
19f625aa9bSMichal Kubecek
20f625aa9bSMichal Kubecek#define LINKMODES_REPDATA(__reply_base) \
21f625aa9bSMichal Kubecek	container_of(__reply_base, struct linkmodes_reply_data, base)
22f625aa9bSMichal Kubecek
23ff419afaSJakub Kicinskiconst struct nla_policy ethnl_linkmodes_get_policy[] = {
24329d9c33SJakub Kicinski	[ETHTOOL_A_LINKMODES_HEADER]		=
25329d9c33SJakub Kicinski		NLA_POLICY_NESTED(ethnl_header_policy),
26f625aa9bSMichal Kubecek};
27f625aa9bSMichal Kubecek
28f625aa9bSMichal Kubecekstatic int linkmodes_prepare_data(const struct ethnl_req_info *req_base,
29f625aa9bSMichal Kubecek				  struct ethnl_reply_data *reply_base,
30f625aa9bSMichal Kubecek				  struct genl_info *info)
31f625aa9bSMichal Kubecek{
32f625aa9bSMichal Kubecek	struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base);
33f625aa9bSMichal Kubecek	struct net_device *dev = reply_base->dev;
34f625aa9bSMichal Kubecek	int ret;
35f625aa9bSMichal Kubecek
36f625aa9bSMichal Kubecek	data->lsettings = &data->ksettings.base;
37f625aa9bSMichal Kubecek
38f625aa9bSMichal Kubecek	ret = ethnl_ops_begin(dev);
39f625aa9bSMichal Kubecek	if (ret < 0)
40f625aa9bSMichal Kubecek		return ret;
41f625aa9bSMichal Kubecek
42f625aa9bSMichal Kubecek	ret = __ethtool_get_link_ksettings(dev, &data->ksettings);
43f625aa9bSMichal Kubecek	if (ret < 0 && info) {
44f625aa9bSMichal Kubecek		GENL_SET_ERR_MSG(info, "failed to retrieve link settings");
45f625aa9bSMichal Kubecek		goto out;
46f625aa9bSMichal Kubecek	}
47f625aa9bSMichal Kubecek
487dc33f09SDanielle Ratson	if (!dev->ethtool_ops->cap_link_lanes_supported)
497dc33f09SDanielle Ratson		data->ksettings.lanes = 0;
507dc33f09SDanielle Ratson
51f625aa9bSMichal Kubecek	data->peer_empty =
52f625aa9bSMichal Kubecek		bitmap_empty(data->ksettings.link_modes.lp_advertising,
53f625aa9bSMichal Kubecek			     __ETHTOOL_LINK_MODE_MASK_NBITS);
54f625aa9bSMichal Kubecek
55f625aa9bSMichal Kubecekout:
56f625aa9bSMichal Kubecek	ethnl_ops_complete(dev);
57f625aa9bSMichal Kubecek	return ret;
58f625aa9bSMichal Kubecek}
59f625aa9bSMichal Kubecek
60f625aa9bSMichal Kubecekstatic int linkmodes_reply_size(const struct ethnl_req_info *req_base,
61f625aa9bSMichal Kubecek				const struct ethnl_reply_data *reply_base)
62f625aa9bSMichal Kubecek{
63f625aa9bSMichal Kubecek	const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base);
64f625aa9bSMichal Kubecek	const struct ethtool_link_ksettings *ksettings = &data->ksettings;
65bdbdac76SOleksij Rempel	const struct ethtool_link_settings *lsettings = &ksettings->base;
66f625aa9bSMichal Kubecek	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
67f625aa9bSMichal Kubecek	int len, ret;
68f625aa9bSMichal Kubecek
69f625aa9bSMichal Kubecek	len = nla_total_size(sizeof(u8)) /* LINKMODES_AUTONEG */
70f625aa9bSMichal Kubecek		+ nla_total_size(sizeof(u32)) /* LINKMODES_SPEED */
717dc33f09SDanielle Ratson		+ nla_total_size(sizeof(u32)) /* LINKMODES_LANES */
72f625aa9bSMichal Kubecek		+ nla_total_size(sizeof(u8)) /* LINKMODES_DUPLEX */
73f625aa9bSMichal Kubecek		+ 0;
74f625aa9bSMichal Kubecek	ret = ethnl_bitset_size(ksettings->link_modes.advertising,
75f625aa9bSMichal Kubecek				ksettings->link_modes.supported,
76f625aa9bSMichal Kubecek				__ETHTOOL_LINK_MODE_MASK_NBITS,
77f625aa9bSMichal Kubecek				link_mode_names, compact);
78f625aa9bSMichal Kubecek	if (ret < 0)
79f625aa9bSMichal Kubecek		return ret;
80f625aa9bSMichal Kubecek	len += ret;
81f625aa9bSMichal Kubecek	if (!data->peer_empty) {
82f625aa9bSMichal Kubecek		ret = ethnl_bitset_size(ksettings->link_modes.lp_advertising,
83f625aa9bSMichal Kubecek					NULL, __ETHTOOL_LINK_MODE_MASK_NBITS,
84f625aa9bSMichal Kubecek					link_mode_names, compact);
85f625aa9bSMichal Kubecek		if (ret < 0)
86f625aa9bSMichal Kubecek			return ret;
87f625aa9bSMichal Kubecek		len += ret;
88f625aa9bSMichal Kubecek	}
89f625aa9bSMichal Kubecek
90bdbdac76SOleksij Rempel	if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED)
91bdbdac76SOleksij Rempel		len += nla_total_size(sizeof(u8));
92bdbdac76SOleksij Rempel
93bdbdac76SOleksij Rempel	if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED)
94bdbdac76SOleksij Rempel		len += nla_total_size(sizeof(u8));
95bdbdac76SOleksij Rempel
96f625aa9bSMichal Kubecek	return len;
97f625aa9bSMichal Kubecek}
98f625aa9bSMichal Kubecek
99f625aa9bSMichal Kubecekstatic int linkmodes_fill_reply(struct sk_buff *skb,
100f625aa9bSMichal Kubecek				const struct ethnl_req_info *req_base,
101f625aa9bSMichal Kubecek				const struct ethnl_reply_data *reply_base)
102f625aa9bSMichal Kubecek{
103f625aa9bSMichal Kubecek	const struct linkmodes_reply_data *data = LINKMODES_REPDATA(reply_base);
104f625aa9bSMichal Kubecek	const struct ethtool_link_ksettings *ksettings = &data->ksettings;
105f625aa9bSMichal Kubecek	const struct ethtool_link_settings *lsettings = &ksettings->base;
106f625aa9bSMichal Kubecek	bool compact = req_base->flags & ETHTOOL_FLAG_COMPACT_BITSETS;
107f625aa9bSMichal Kubecek	int ret;
108f625aa9bSMichal Kubecek
109f625aa9bSMichal Kubecek	if (nla_put_u8(skb, ETHTOOL_A_LINKMODES_AUTONEG, lsettings->autoneg))
110f625aa9bSMichal Kubecek		return -EMSGSIZE;
111f625aa9bSMichal Kubecek
112f625aa9bSMichal Kubecek	ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_OURS,
113f625aa9bSMichal Kubecek			       ksettings->link_modes.advertising,
114f625aa9bSMichal Kubecek			       ksettings->link_modes.supported,
115f625aa9bSMichal Kubecek			       __ETHTOOL_LINK_MODE_MASK_NBITS, link_mode_names,
116f625aa9bSMichal Kubecek			       compact);
117f625aa9bSMichal Kubecek	if (ret < 0)
118f625aa9bSMichal Kubecek		return -EMSGSIZE;
119f625aa9bSMichal Kubecek	if (!data->peer_empty) {
120f625aa9bSMichal Kubecek		ret = ethnl_put_bitset(skb, ETHTOOL_A_LINKMODES_PEER,
121f625aa9bSMichal Kubecek				       ksettings->link_modes.lp_advertising,
122f625aa9bSMichal Kubecek				       NULL, __ETHTOOL_LINK_MODE_MASK_NBITS,
123f625aa9bSMichal Kubecek				       link_mode_names, compact);
124f625aa9bSMichal Kubecek		if (ret < 0)
125f625aa9bSMichal Kubecek			return -EMSGSIZE;
126f625aa9bSMichal Kubecek	}
127f625aa9bSMichal Kubecek
128f625aa9bSMichal Kubecek	if (nla_put_u32(skb, ETHTOOL_A_LINKMODES_SPEED, lsettings->speed) ||
129f625aa9bSMichal Kubecek	    nla_put_u8(skb, ETHTOOL_A_LINKMODES_DUPLEX, lsettings->duplex))
130f625aa9bSMichal Kubecek		return -EMSGSIZE;
131f625aa9bSMichal Kubecek
1327dc33f09SDanielle Ratson	if (ksettings->lanes &&
1337dc33f09SDanielle Ratson	    nla_put_u32(skb, ETHTOOL_A_LINKMODES_LANES, ksettings->lanes))
1347dc33f09SDanielle Ratson		return -EMSGSIZE;
1357dc33f09SDanielle Ratson
136bdbdac76SOleksij Rempel	if (lsettings->master_slave_cfg != MASTER_SLAVE_CFG_UNSUPPORTED &&
137bdbdac76SOleksij Rempel	    nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG,
138bdbdac76SOleksij Rempel		       lsettings->master_slave_cfg))
139bdbdac76SOleksij Rempel		return -EMSGSIZE;
140bdbdac76SOleksij Rempel
141bdbdac76SOleksij Rempel	if (lsettings->master_slave_state != MASTER_SLAVE_STATE_UNSUPPORTED &&
142bdbdac76SOleksij Rempel	    nla_put_u8(skb, ETHTOOL_A_LINKMODES_MASTER_SLAVE_STATE,
143bdbdac76SOleksij Rempel		       lsettings->master_slave_state))
144bdbdac76SOleksij Rempel		return -EMSGSIZE;
145bdbdac76SOleksij Rempel
146f625aa9bSMichal Kubecek	return 0;
147f625aa9bSMichal Kubecek}
148f625aa9bSMichal Kubecek
149f625aa9bSMichal Kubecekconst struct ethnl_request_ops ethnl_linkmodes_request_ops = {
150f625aa9bSMichal Kubecek	.request_cmd		= ETHTOOL_MSG_LINKMODES_GET,
151f625aa9bSMichal Kubecek	.reply_cmd		= ETHTOOL_MSG_LINKMODES_GET_REPLY,
152f625aa9bSMichal Kubecek	.hdr_attr		= ETHTOOL_A_LINKMODES_HEADER,
153f625aa9bSMichal Kubecek	.req_info_size		= sizeof(struct linkmodes_req_info),
154f625aa9bSMichal Kubecek	.reply_data_size	= sizeof(struct linkmodes_reply_data),
155f625aa9bSMichal Kubecek
156f625aa9bSMichal Kubecek	.prepare_data		= linkmodes_prepare_data,
157f625aa9bSMichal Kubecek	.reply_size		= linkmodes_reply_size,
158f625aa9bSMichal Kubecek	.fill_reply		= linkmodes_fill_reply,
159f625aa9bSMichal Kubecek};
160bfbcfe20SMichal Kubecek
161bfbcfe20SMichal Kubecek/* LINKMODES_SET */
162bfbcfe20SMichal Kubecek
163ff419afaSJakub Kicinskiconst struct nla_policy ethnl_linkmodes_set_policy[] = {
164329d9c33SJakub Kicinski	[ETHTOOL_A_LINKMODES_HEADER]		=
165329d9c33SJakub Kicinski		NLA_POLICY_NESTED(ethnl_header_policy),
166bfbcfe20SMichal Kubecek	[ETHTOOL_A_LINKMODES_AUTONEG]		= { .type = NLA_U8 },
167bfbcfe20SMichal Kubecek	[ETHTOOL_A_LINKMODES_OURS]		= { .type = NLA_NESTED },
168bfbcfe20SMichal Kubecek	[ETHTOOL_A_LINKMODES_SPEED]		= { .type = NLA_U32 },
169bfbcfe20SMichal Kubecek	[ETHTOOL_A_LINKMODES_DUPLEX]		= { .type = NLA_U8 },
170bdbdac76SOleksij Rempel	[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG]	= { .type = NLA_U8 },
171012ce4ddSDanielle Ratson	[ETHTOOL_A_LINKMODES_LANES]		= NLA_POLICY_RANGE(NLA_U32, 1, 8),
172bfbcfe20SMichal Kubecek};
173bfbcfe20SMichal Kubecek
174012ce4ddSDanielle Ratson/* Set advertised link modes to all supported modes matching requested speed,
175012ce4ddSDanielle Ratson * lanes and duplex values. Called when autonegotiation is on, speed, lanes or
176012ce4ddSDanielle Ratson * duplex is requested but no link mode change. This is done in userspace with
177012ce4ddSDanielle Ratson * ioctl() interface, move it into kernel for netlink.
178bfbcfe20SMichal Kubecek * Returns true if advertised modes bitmap was modified.
179bfbcfe20SMichal Kubecek */
180bfbcfe20SMichal Kubecekstatic bool ethnl_auto_linkmodes(struct ethtool_link_ksettings *ksettings,
181012ce4ddSDanielle Ratson				 bool req_speed, bool req_lanes, bool req_duplex)
182bfbcfe20SMichal Kubecek{
183bfbcfe20SMichal Kubecek	unsigned long *advertising = ksettings->link_modes.advertising;
184bfbcfe20SMichal Kubecek	unsigned long *supported = ksettings->link_modes.supported;
185bfbcfe20SMichal Kubecek	DECLARE_BITMAP(old_adv, __ETHTOOL_LINK_MODE_MASK_NBITS);
186bfbcfe20SMichal Kubecek	unsigned int i;
187bfbcfe20SMichal Kubecek
188bfbcfe20SMichal Kubecek	bitmap_copy(old_adv, advertising, __ETHTOOL_LINK_MODE_MASK_NBITS);
189bfbcfe20SMichal Kubecek
190bfbcfe20SMichal Kubecek	for (i = 0; i < __ETHTOOL_LINK_MODE_MASK_NBITS; i++) {
191bfbcfe20SMichal Kubecek		const struct link_mode_info *info = &link_mode_params[i];
192bfbcfe20SMichal Kubecek
193bfbcfe20SMichal Kubecek		if (info->speed == SPEED_UNKNOWN)
194bfbcfe20SMichal Kubecek			continue;
195bfbcfe20SMichal Kubecek		if (test_bit(i, supported) &&
196bfbcfe20SMichal Kubecek		    (!req_speed || info->speed == ksettings->base.speed) &&
197012ce4ddSDanielle Ratson		    (!req_lanes || info->lanes == ksettings->lanes) &&
198bfbcfe20SMichal Kubecek		    (!req_duplex || info->duplex == ksettings->base.duplex))
199bfbcfe20SMichal Kubecek			set_bit(i, advertising);
200bfbcfe20SMichal Kubecek		else
201bfbcfe20SMichal Kubecek			clear_bit(i, advertising);
202bfbcfe20SMichal Kubecek	}
203bfbcfe20SMichal Kubecek
204bfbcfe20SMichal Kubecek	return !bitmap_equal(old_adv, advertising,
205bfbcfe20SMichal Kubecek			     __ETHTOOL_LINK_MODE_MASK_NBITS);
206bfbcfe20SMichal Kubecek}
207bfbcfe20SMichal Kubecek
208bdbdac76SOleksij Rempelstatic bool ethnl_validate_master_slave_cfg(u8 cfg)
209bdbdac76SOleksij Rempel{
210bdbdac76SOleksij Rempel	switch (cfg) {
211bdbdac76SOleksij Rempel	case MASTER_SLAVE_CFG_MASTER_PREFERRED:
212bdbdac76SOleksij Rempel	case MASTER_SLAVE_CFG_SLAVE_PREFERRED:
213bdbdac76SOleksij Rempel	case MASTER_SLAVE_CFG_MASTER_FORCE:
214bdbdac76SOleksij Rempel	case MASTER_SLAVE_CFG_SLAVE_FORCE:
215bdbdac76SOleksij Rempel		return true;
216bdbdac76SOleksij Rempel	}
217bdbdac76SOleksij Rempel
218bdbdac76SOleksij Rempel	return false;
219bdbdac76SOleksij Rempel}
220bdbdac76SOleksij Rempel
221189e7a8dSDanielle Ratsonstatic int ethnl_check_linkmodes(struct genl_info *info, struct nlattr **tb)
222189e7a8dSDanielle Ratson{
223012ce4ddSDanielle Ratson	const struct nlattr *master_slave_cfg, *lanes_cfg;
224189e7a8dSDanielle Ratson
225189e7a8dSDanielle Ratson	master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG];
226189e7a8dSDanielle Ratson	if (master_slave_cfg &&
227189e7a8dSDanielle Ratson	    !ethnl_validate_master_slave_cfg(nla_get_u8(master_slave_cfg))) {
228189e7a8dSDanielle Ratson		NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg,
229189e7a8dSDanielle Ratson				    "master/slave value is invalid");
230189e7a8dSDanielle Ratson		return -EOPNOTSUPP;
231189e7a8dSDanielle Ratson	}
232189e7a8dSDanielle Ratson
233012ce4ddSDanielle Ratson	lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES];
234012ce4ddSDanielle Ratson	if (lanes_cfg && !is_power_of_2(nla_get_u32(lanes_cfg))) {
235012ce4ddSDanielle Ratson		NL_SET_ERR_MSG_ATTR(info->extack, lanes_cfg,
236012ce4ddSDanielle Ratson				    "lanes value is invalid");
237012ce4ddSDanielle Ratson		return -EINVAL;
238012ce4ddSDanielle Ratson	}
239012ce4ddSDanielle Ratson
240189e7a8dSDanielle Ratson	return 0;
241189e7a8dSDanielle Ratson}
242189e7a8dSDanielle Ratson
243bfbcfe20SMichal Kubecekstatic int ethnl_update_linkmodes(struct genl_info *info, struct nlattr **tb,
244bfbcfe20SMichal Kubecek				  struct ethtool_link_ksettings *ksettings,
245012ce4ddSDanielle Ratson				  bool *mod, const struct net_device *dev)
246bfbcfe20SMichal Kubecek{
247bfbcfe20SMichal Kubecek	struct ethtool_link_settings *lsettings = &ksettings->base;
248012ce4ddSDanielle Ratson	bool req_speed, req_lanes, req_duplex;
249012ce4ddSDanielle Ratson	const struct nlattr *master_slave_cfg, *lanes_cfg;
250bfbcfe20SMichal Kubecek	int ret;
251bfbcfe20SMichal Kubecek
252bdbdac76SOleksij Rempel	master_slave_cfg = tb[ETHTOOL_A_LINKMODES_MASTER_SLAVE_CFG];
253bdbdac76SOleksij Rempel	if (master_slave_cfg) {
254bdbdac76SOleksij Rempel		if (lsettings->master_slave_cfg == MASTER_SLAVE_CFG_UNSUPPORTED) {
255bdbdac76SOleksij Rempel			NL_SET_ERR_MSG_ATTR(info->extack, master_slave_cfg,
256bdbdac76SOleksij Rempel					    "master/slave configuration not supported by device");
257bdbdac76SOleksij Rempel			return -EOPNOTSUPP;
258bdbdac76SOleksij Rempel		}
259bdbdac76SOleksij Rempel	}
260bdbdac76SOleksij Rempel
261bfbcfe20SMichal Kubecek	*mod = false;
262bfbcfe20SMichal Kubecek	req_speed = tb[ETHTOOL_A_LINKMODES_SPEED];
263012ce4ddSDanielle Ratson	req_lanes = tb[ETHTOOL_A_LINKMODES_LANES];
264bfbcfe20SMichal Kubecek	req_duplex = tb[ETHTOOL_A_LINKMODES_DUPLEX];
265bfbcfe20SMichal Kubecek
266bfbcfe20SMichal Kubecek	ethnl_update_u8(&lsettings->autoneg, tb[ETHTOOL_A_LINKMODES_AUTONEG],
267bfbcfe20SMichal Kubecek			mod);
268012ce4ddSDanielle Ratson
269012ce4ddSDanielle Ratson	lanes_cfg = tb[ETHTOOL_A_LINKMODES_LANES];
270012ce4ddSDanielle Ratson	if (lanes_cfg) {
271012ce4ddSDanielle Ratson		/* If autoneg is off and lanes parameter is not supported by the
272