1// SPDX-License-Identifier: GPL-2.0-only
2
3#include <linux/phy.h>
4#include <linux/ethtool_netlink.h>
5#include "netlink.h"
6#include "common.h"
7
8/* 802.3 standard allows 100 meters for BaseT cables. However longer
9 * cables might work, depending on the quality of the cables and the
10 * PHY. So allow testing for up to 150 meters.
11 */
12#define MAX_CABLE_LENGTH_CM (150 * 100)
13
14const struct nla_policy ethnl_cable_test_act_policy[] = {
15	[ETHTOOL_A_CABLE_TEST_HEADER]		=
16		NLA_POLICY_NESTED(ethnl_header_policy),
17};
18
19static int ethnl_cable_test_started(struct phy_device *phydev, u8 cmd)
20{
21	struct sk_buff *skb;
22	int err = -ENOMEM;
23	void *ehdr;
24
25	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
26	if (!skb)
27		goto out;
28
29	ehdr = ethnl_bcastmsg_put(skb, cmd);
30	if (!ehdr) {
31		err = -EMSGSIZE;
32		goto out;
33	}
34
35	err = ethnl_fill_reply_header(skb, phydev->attached_dev,
36				      ETHTOOL_A_CABLE_TEST_NTF_HEADER);
37	if (err)
38		goto out;
39
40	err = nla_put_u8(skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS,
41			 ETHTOOL_A_CABLE_TEST_NTF_STATUS_STARTED);
42	if (err)
43		goto out;
44
45	genlmsg_end(skb, ehdr);
46
47	return ethnl_multicast(skb, phydev->attached_dev);
48
49out:
50	nlmsg_free(skb);
51	phydev_err(phydev, "%s: Error %pe\n", __func__, ERR_PTR(err));
52
53	return err;
54}
55
56int ethnl_act_cable_test(struct sk_buff *skb, struct genl_info *info)
57{
58	struct ethnl_req_info req_info = {};
59	const struct ethtool_phy_ops *ops;
60	struct nlattr **tb = info->attrs;
61	struct net_device *dev;
62	int ret;
63
64	ret = ethnl_parse_header_dev_get(&req_info,
65					 tb[ETHTOOL_A_CABLE_TEST_HEADER],
66					 genl_info_net(info), info->extack,
67					 true);
68	if (ret < 0)
69		return ret;
70
71	dev = req_info.dev;
72	if (!dev->phydev) {
73		ret = -EOPNOTSUPP;
74		goto out_dev_put;
75	}
76
77	rtnl_lock();
78	ops = ethtool_phy_ops;
79	if (!ops || !ops->start_cable_test) {
80		ret = -EOPNOTSUPP;
81		goto out_rtnl;
82	}
83
84	ret = ethnl_ops_begin(dev);
85	if (ret < 0)
86		goto out_rtnl;
87
88	ret = ops->start_cable_test(dev->phydev, info->extack);
89
90	ethnl_ops_complete(dev);
91
92	if (!ret)
93		ethnl_cable_test_started(dev->phydev,
94					 ETHTOOL_MSG_CABLE_TEST_NTF);
95
96out_rtnl:
97	rtnl_unlock();
98out_dev_put:
99	ethnl_parse_header_dev_put(&req_info);
100	return ret;
101}
102
103int ethnl_cable_test_alloc(struct phy_device *phydev, u8 cmd)
104{
105	int err = -ENOMEM;
106
107	/* One TDR sample occupies 20 bytes. For a 150 meter cable,
108	 * with four pairs, around 12K is needed.
109	 */
110	phydev->skb = genlmsg_new(SZ_16K, GFP_KERNEL);
111	if (!phydev->skb)
112		goto out;
113
114	phydev->ehdr = ethnl_bcastmsg_put(phydev->skb, cmd);
115	if (!phydev->ehdr) {
116		err = -EMSGSIZE;
117		goto out;
118	}
119
120	err = ethnl_fill_reply_header(phydev->skb, phydev->attached_dev,
121				      ETHTOOL_A_CABLE_TEST_NTF_HEADER);
122	if (err)
123		goto out;
124
125	err = nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS,
126			 ETHTOOL_A_CABLE_TEST_NTF_STATUS_COMPLETED);
127	if (err)
128		goto out;
129
130	phydev->nest = nla_nest_start(phydev->skb,
131				      ETHTOOL_A_CABLE_TEST_NTF_NEST);
132	if (!phydev->nest) {
133		err = -EMSGSIZE;
134		goto out;
135	}
136
137	return 0;
138
139out:
140	nlmsg_free(phydev->skb);
141	phydev->skb = NULL;
142	return err;
143}
144EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc);
145
146void ethnl_cable_test_free(struct phy_device *phydev)
147{
148	nlmsg_free(phydev->skb);
149	phydev->skb = NULL;
150}
151EXPORT_SYMBOL_GPL(ethnl_cable_test_free);
152
153void ethnl_cable_test_finished(struct phy_device *phydev)
154{
155	nla_nest_end(phydev->skb, phydev->nest);
156
157	genlmsg_end(phydev->skb, phydev->ehdr);
158
159	ethnl_multicast(phydev->skb, phydev->attached_dev);
160}
161EXPORT_SYMBOL_GPL(ethnl_cable_test_finished);
162
163int ethnl_cable_test_result(struct phy_device *phydev, u8 pair, u8 result)
164{
165	struct nlattr *nest;
166	int ret = -EMSGSIZE;
167
168	nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_NEST_RESULT);
169	if (!nest)
170		return -EMSGSIZE;
171
172	if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_PAIR, pair))
173		goto err;
174	if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_CODE, result))
175		goto err;
176
177	nla_nest_end(phydev->skb, nest);
178	return 0;
179
180err:
181	nla_nest_cancel(phydev->skb, nest);
182	return ret;
183}
184EXPORT_SYMBOL_GPL(ethnl_cable_test_result);
185
186int ethnl_cable_test_fault_length(struct phy_device *phydev, u8 pair, u32 cm)
187{
188	struct nlattr *nest;
189	int ret = -EMSGSIZE;
190
191	nest = nla_nest_start(phydev->skb,
192			      ETHTOOL_A_CABLE_NEST_FAULT_LENGTH);
193	if (!nest)
194		return -EMSGSIZE;
195
196	if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_PAIR, pair))
197		goto err;
198	if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_CM, cm))
199		goto err;
200
201	nla_nest_end(phydev->skb, nest);
202	return 0;
203
204err:
205	nla_nest_cancel(phydev->skb, nest);
206	return ret;
207}
208EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length);
209
210struct cable_test_tdr_req_info {
211	struct ethnl_req_info		base;
212};
213
214static const struct nla_policy cable_test_tdr_act_cfg_policy[] = {
215	[ETHTOOL_A_CABLE_TEST_TDR_CFG_FIRST]	= { .type = NLA_U32 },
216	[ETHTOOL_A_CABLE_TEST_TDR_CFG_LAST]	= { .type = NLA_U32 },
217	[ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP]	= { .type = NLA_U32 },
218	[ETHTOOL_A_CABLE_TEST_TDR_CFG_PAIR]	= { .type = NLA_U8 },
219};
220
221const struct nla_policy ethnl_cable_test_tdr_act_policy[] = {
222	[ETHTOOL_A_CABLE_TEST_TDR_HEADER]	=
223		NLA_POLICY_NESTED(ethnl_header_policy),
224	[ETHTOOL_A_CABLE_TEST_TDR_CFG]		= { .type = NLA_NESTED },
225};
226
227/* CABLE_TEST_TDR_ACT */
228static int ethnl_act_cable_test_tdr_cfg(const struct nlattr *nest,
229					struct genl_info *info,
230					struct phy_tdr_config *cfg)
231{
232	struct nlattr *tb[ARRAY_SIZE(cable_test_tdr_act_cfg_policy)];
233	int ret;
234
235	cfg->first = 100;
236	cfg->step = 100;
237	cfg->last = MAX_CABLE_LENGTH_CM;
238	cfg->pair = PHY_PAIR_ALL;
239
240	if (!nest)
241		return 0;
242
243	ret = nla_parse_nested(tb,
244			       ARRAY_SIZE(cable_test_tdr_act_cfg_policy) - 1,
245			       nest, cable_test_tdr_act_cfg_policy,
246			       info->extack);
247	if (ret < 0)
248		return ret;
249
250	if (tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_FIRST])
251		cfg->first = nla_get_u32(
252			tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_FIRST]);
253
254	if (tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_LAST])
255		cfg->last = nla_get_u32(tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_LAST]);
256
257	if (tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP])
258		cfg->step = nla_get_u32(tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP]);
259
260	if (tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_PAIR]) {
261		cfg->pair = nla_get_u8(tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_PAIR]);
262		if (cfg->pair > ETHTOOL_A_CABLE_PAIR_D) {
263			NL_SET_ERR_MSG_ATTR(
264				info->extack,
265				tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_PAIR],
266				"invalid pair parameter");
267			return -EINVAL;
268		}
269	}
270
271	if (cfg->first > MAX_CABLE_LENGTH_CM) {
272		NL_SET_ERR_MSG_ATTR(info->extack,
273				    tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_FIRST],
274				    "invalid first parameter");
275		return -EINVAL;
276	}
277
278	if (cfg->last > MAX_CABLE_LENGTH_CM) {
279		NL_SET_ERR_MSG_ATTR(info->extack,
280				    tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_LAST],
281				    "invalid last parameter");
282		return -EINVAL;
283	}
284
285	if (cfg->first > cfg->last) {
286		NL_SET_ERR_MSG(info->extack, "invalid first/last parameter");
287		return -EINVAL;
288	}
289
290	if (!cfg->step) {
291		NL_SET_ERR_MSG_ATTR(info->extack,
292				    tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP],
293				    "invalid step parameter");
294		return -EINVAL;
295	}
296
297	if (cfg->step > (cfg->last - cfg->first)) {
298		NL_SET_ERR_MSG_ATTR(info->extack,
299				    tb[ETHTOOL_A_CABLE_TEST_TDR_CFG_STEP],
300				    "step parameter too big");
301		return -EINVAL;
302	}
303
304	return 0;
305}
306
307int ethnl_act_cable_test_tdr(struct sk_buff *skb, struct genl_info *info)
308{
309	struct ethnl_req_info req_info = {};
310	const struct ethtool_phy_ops *ops;
311	struct nlattr **tb = info->attrs;
312	struct phy_tdr_config cfg;
313	struct net_device *dev;
314	int ret;
315
316	ret = ethnl_parse_header_dev_get(&req_info,
317					 tb[ETHTOOL_A_CABLE_TEST_TDR_HEADER],
318					 genl_info_net(info), info->extack,
319					 true);
320	if (ret < 0)
321		return ret;
322
323	dev = req_info.dev;
324	if (!dev->phydev) {
325		ret = -EOPNOTSUPP;
326		goto out_dev_put;
327	}
328
329	ret = ethnl_act_cable_test_tdr_cfg(tb[ETHTOOL_A_CABLE_TEST_TDR_CFG],
330					   info, &cfg);
331	if (ret)
332		goto out_dev_put;
333
334	rtnl_lock();
335	ops = ethtool_phy_ops;
336	if (!ops || !ops->start_cable_test_tdr) {
337		ret = -EOPNOTSUPP;
338		goto out_rtnl;
339	}
340
341	ret = ethnl_ops_begin(dev);
342	if (ret < 0)
343		goto out_rtnl;
344
345	ret = ops->start_cable_test_tdr(dev->phydev, info->extack, &cfg);
346
347	ethnl_ops_complete(dev);
348
349	if (!ret)
350		ethnl_cable_test_started(dev->phydev,
351					 ETHTOOL_MSG_CABLE_TEST_TDR_NTF);
352
353out_rtnl:
354	rtnl_unlock();
355out_dev_put:
356	ethnl_parse_header_dev_put(&req_info);
357	return ret;
358}
359
360int ethnl_cable_test_amplitude(struct phy_device *phydev,
361			       u8 pair, s16 mV)
362{
363	struct nlattr *nest;
364	int ret = -EMSGSIZE;
365
366	nest = nla_nest_start(phydev->skb,
367			      ETHTOOL_A_CABLE_TDR_NEST_AMPLITUDE);
368	if (!nest)
369		return -EMSGSIZE;
370
371	if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_PAIR, pair))
372		goto err;
373	if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_AMPLITUDE_mV, mV))
374		goto err;
375
376	nla_nest_end(phydev->skb, nest);
377	return 0;
378
379err:
380	nla_nest_cancel(phydev->skb, nest);
381	return ret;
382}
383EXPORT_SYMBOL_GPL(ethnl_cable_test_amplitude);
384
385int ethnl_cable_test_pulse(struct phy_device *phydev, u16 mV)
386{
387	struct nlattr *nest;
388	int ret = -EMSGSIZE;
389
390	nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_PULSE);
391	if (!nest)
392		return -EMSGSIZE;
393
394	if (nla_put_u16(phydev->skb, ETHTOOL_A_CABLE_PULSE_mV, mV))
395		goto err;
396
397	nla_nest_end(phydev->skb, nest);
398	return 0;
399
400err:
401	nla_nest_cancel(phydev->skb, nest);
402	return ret;
403}
404EXPORT_SYMBOL_GPL(ethnl_cable_test_pulse);
405
406int ethnl_cable_test_step(struct phy_device *phydev, u32 first, u32 last,
407			  u32 step)
408{
409	struct nlattr *nest;
410	int ret = -EMSGSIZE;
411
412	nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_TDR_NEST_STEP);
413	if (!nest)
414		return -EMSGSIZE;
415
416	if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_FIRST_DISTANCE,
417			first))
418		goto err;
419
420	if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_LAST_DISTANCE, last))
421		goto err;
422
423	if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_STEP_STEP_DISTANCE, step))
424		goto err;
425
426	nla_nest_end(phydev->skb, nest);
427	return 0;
428
429err:
430	nla_nest_cancel(phydev->skb, nest);
431	return ret;
432}
433EXPORT_SYMBOL_GPL(ethnl_cable_test_step);
434