1// SPDX-License-Identifier: GPL-2.0-only
2
3#include <linux/phy.h>
4#include <linux/ethtool_netlink.h>
5
6#include "netlink.h"
7#include "common.h"
8
9struct plca_req_info {
10	struct ethnl_req_info		base;
11};
12
13struct plca_reply_data {
14	struct ethnl_reply_data		base;
15	struct phy_plca_cfg		plca_cfg;
16	struct phy_plca_status		plca_st;
17};
18
19// Helpers ------------------------------------------------------------------ //
20
21#define PLCA_REPDATA(__reply_base) \
22	container_of(__reply_base, struct plca_reply_data, base)
23
24// PLCA get configuration message ------------------------------------------- //
25
26const struct nla_policy ethnl_plca_get_cfg_policy[] = {
27	[ETHTOOL_A_PLCA_HEADER]		=
28		NLA_POLICY_NESTED(ethnl_header_policy),
29};
30
31static void plca_update_sint(int *dst, struct nlattr **tb, u32 attrid,
32			     bool *mod)
33{
34	const struct nlattr *attr = tb[attrid];
35
36	if (!attr ||
37	    WARN_ON_ONCE(attrid >= ARRAY_SIZE(ethnl_plca_set_cfg_policy)))
38		return;
39
40	switch (ethnl_plca_set_cfg_policy[attrid].type) {
41	case NLA_U8:
42		*dst = nla_get_u8(attr);
43		break;
44	case NLA_U32:
45		*dst = nla_get_u32(attr);
46		break;
47	default:
48		WARN_ON_ONCE(1);
49	}
50
51	*mod = true;
52}
53
54static int plca_get_cfg_prepare_data(const struct ethnl_req_info *req_base,
55				     struct ethnl_reply_data *reply_base,
56				     const struct genl_info *info)
57{
58	struct plca_reply_data *data = PLCA_REPDATA(reply_base);
59	struct net_device *dev = reply_base->dev;
60	const struct ethtool_phy_ops *ops;
61	int ret;
62
63	// check that the PHY device is available and connected
64	if (!dev->phydev) {
65		ret = -EOPNOTSUPP;
66		goto out;
67	}
68
69	// note: rtnl_lock is held already by ethnl_default_doit
70	ops = ethtool_phy_ops;
71	if (!ops || !ops->get_plca_cfg) {
72		ret = -EOPNOTSUPP;
73		goto out;
74	}
75
76	ret = ethnl_ops_begin(dev);
77	if (ret < 0)
78		goto out;
79
80	memset(&data->plca_cfg, 0xff,
81	       sizeof_field(struct plca_reply_data, plca_cfg));
82
83	ret = ops->get_plca_cfg(dev->phydev, &data->plca_cfg);
84	ethnl_ops_complete(dev);
85
86out:
87	return ret;
88}
89
90static int plca_get_cfg_reply_size(const struct ethnl_req_info *req_base,
91				   const struct ethnl_reply_data *reply_base)
92{
93	return nla_total_size(sizeof(u16)) +	/* _VERSION */
94	       nla_total_size(sizeof(u8)) +	/* _ENABLED */
95	       nla_total_size(sizeof(u32)) +	/* _NODE_CNT */
96	       nla_total_size(sizeof(u32)) +	/* _NODE_ID */
97	       nla_total_size(sizeof(u32)) +	/* _TO_TIMER */
98	       nla_total_size(sizeof(u32)) +	/* _BURST_COUNT */
99	       nla_total_size(sizeof(u32));	/* _BURST_TIMER */
100}
101
102static int plca_get_cfg_fill_reply(struct sk_buff *skb,
103				   const struct ethnl_req_info *req_base,
104				   const struct ethnl_reply_data *reply_base)
105{
106	const struct plca_reply_data *data = PLCA_REPDATA(reply_base);
107	const struct phy_plca_cfg *plca = &data->plca_cfg;
108
109	if ((plca->version >= 0 &&
110	     nla_put_u16(skb, ETHTOOL_A_PLCA_VERSION, plca->version)) ||
111	    (plca->enabled >= 0 &&
112	     nla_put_u8(skb, ETHTOOL_A_PLCA_ENABLED, !!plca->enabled)) ||
113	    (plca->node_id >= 0 &&
114	     nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_ID, plca->node_id)) ||
115	    (plca->node_cnt >= 0 &&
116	     nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_CNT, plca->node_cnt)) ||
117	    (plca->to_tmr >= 0 &&
118	     nla_put_u32(skb, ETHTOOL_A_PLCA_TO_TMR, plca->to_tmr)) ||
119	    (plca->burst_cnt >= 0 &&
120	     nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_CNT, plca->burst_cnt)) ||
121	    (plca->burst_tmr >= 0 &&
122	     nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_TMR, plca->burst_tmr)))
123		return -EMSGSIZE;
124
125	return 0;
126};
127
128// PLCA set configuration message ------------------------------------------- //
129
130const struct nla_policy ethnl_plca_set_cfg_policy[] = {
131	[ETHTOOL_A_PLCA_HEADER]		=
132		NLA_POLICY_NESTED(ethnl_header_policy),
133	[ETHTOOL_A_PLCA_ENABLED]	= NLA_POLICY_MAX(NLA_U8, 1),
134	[ETHTOOL_A_PLCA_NODE_ID]	= NLA_POLICY_MAX(NLA_U32, 255),
135	[ETHTOOL_A_PLCA_NODE_CNT]	= NLA_POLICY_RANGE(NLA_U32, 1, 255),
136	[ETHTOOL_A_PLCA_TO_TMR]		= NLA_POLICY_MAX(NLA_U32, 255),
137	[ETHTOOL_A_PLCA_BURST_CNT]	= NLA_POLICY_MAX(NLA_U32, 255),
138	[ETHTOOL_A_PLCA_BURST_TMR]	= NLA_POLICY_MAX(NLA_U32, 255),
139};
140
141static int
142ethnl_set_plca(struct ethnl_req_info *req_info, struct genl_info *info)
143{
144	struct net_device *dev = req_info->dev;
145	const struct ethtool_phy_ops *ops;
146	struct nlattr **tb = info->attrs;
147	struct phy_plca_cfg plca_cfg;
148	bool mod = false;
149	int ret;
150
151	// check that the PHY device is available and connected
152	if (!dev->phydev)
153		return -EOPNOTSUPP;
154
155	ops = ethtool_phy_ops;
156	if (!ops || !ops->set_plca_cfg)
157		return -EOPNOTSUPP;
158
159	memset(&plca_cfg, 0xff, sizeof(plca_cfg));
160	plca_update_sint(&plca_cfg.enabled, tb, ETHTOOL_A_PLCA_ENABLED, &mod);
161	plca_update_sint(&plca_cfg.node_id, tb, ETHTOOL_A_PLCA_NODE_ID, &mod);
162	plca_update_sint(&plca_cfg.node_cnt, tb, ETHTOOL_A_PLCA_NODE_CNT, &mod);
163	plca_update_sint(&plca_cfg.to_tmr, tb, ETHTOOL_A_PLCA_TO_TMR, &mod);
164	plca_update_sint(&plca_cfg.burst_cnt, tb, ETHTOOL_A_PLCA_BURST_CNT,
165			 &mod);
166	plca_update_sint(&plca_cfg.burst_tmr, tb, ETHTOOL_A_PLCA_BURST_TMR,
167			 &mod);
168	if (!mod)
169		return 0;
170
171	ret = ops->set_plca_cfg(dev->phydev, &plca_cfg, info->extack);
172	return ret < 0 ? ret : 1;
173}
174
175const struct ethnl_request_ops ethnl_plca_cfg_request_ops = {
176	.request_cmd		= ETHTOOL_MSG_PLCA_GET_CFG,
177	.reply_cmd		= ETHTOOL_MSG_PLCA_GET_CFG_REPLY,
178	.hdr_attr		= ETHTOOL_A_PLCA_HEADER,
179	.req_info_size		= sizeof(struct plca_req_info),
180	.reply_data_size	= sizeof(struct plca_reply_data),
181
182	.prepare_data		= plca_get_cfg_prepare_data,
183	.reply_size		= plca_get_cfg_reply_size,
184	.fill_reply		= plca_get_cfg_fill_reply,
185
186	.set			= ethnl_set_plca,
187	.set_ntf_cmd		= ETHTOOL_MSG_PLCA_NTF,
188};
189
190// PLCA get status message -------------------------------------------------- //
191
192const struct nla_policy ethnl_plca_get_status_policy[] = {
193	[ETHTOOL_A_PLCA_HEADER]		=
194		NLA_POLICY_NESTED(ethnl_header_policy),
195};
196
197static int plca_get_status_prepare_data(const struct ethnl_req_info *req_base,
198					struct ethnl_reply_data *reply_base,
199					const struct genl_info *info)
200{
201	struct plca_reply_data *data = PLCA_REPDATA(reply_base);
202	struct net_device *dev = reply_base->dev;
203	const struct ethtool_phy_ops *ops;
204	int ret;
205
206	// check that the PHY device is available and connected
207	if (!dev->phydev) {
208		ret = -EOPNOTSUPP;
209		goto out;
210	}
211
212	// note: rtnl_lock is held already by ethnl_default_doit
213	ops = ethtool_phy_ops;
214	if (!ops || !ops->get_plca_status) {
215		ret = -EOPNOTSUPP;
216		goto out;
217	}
218
219	ret = ethnl_ops_begin(dev);
220	if (ret < 0)
221		goto out;
222
223	memset(&data->plca_st, 0xff,
224	       sizeof_field(struct plca_reply_data, plca_st));
225
226	ret = ops->get_plca_status(dev->phydev, &data->plca_st);
227	ethnl_ops_complete(dev);
228out:
229	return ret;
230}
231
232static int plca_get_status_reply_size(const struct ethnl_req_info *req_base,
233				      const struct ethnl_reply_data *reply_base)
234{
235	return nla_total_size(sizeof(u8));	/* _STATUS */
236}
237
238static int plca_get_status_fill_reply(struct sk_buff *skb,
239				      const struct ethnl_req_info *req_base,
240				      const struct ethnl_reply_data *reply_base)
241{
242	const struct plca_reply_data *data = PLCA_REPDATA(reply_base);
243	const u8 status = data->plca_st.pst;
244
245	if (nla_put_u8(skb, ETHTOOL_A_PLCA_STATUS, !!status))
246		return -EMSGSIZE;
247
248	return 0;
249};
250
251const struct ethnl_request_ops ethnl_plca_status_request_ops = {
252	.request_cmd		= ETHTOOL_MSG_PLCA_GET_STATUS,
253	.reply_cmd		= ETHTOOL_MSG_PLCA_GET_STATUS_REPLY,
254	.hdr_attr		= ETHTOOL_A_PLCA_HEADER,
255	.req_info_size		= sizeof(struct plca_req_info),
256	.reply_data_size	= sizeof(struct plca_reply_data),
257
258	.prepare_data		= plca_get_status_prepare_data,
259	.reply_size		= plca_get_status_reply_size,
260	.fill_reply		= plca_get_status_fill_reply,
261};
262