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 
14 const struct nla_policy ethnl_cable_test_act_policy[] = {
15 	[ETHTOOL_A_CABLE_TEST_HEADER]		=
16 		NLA_POLICY_NESTED(ethnl_header_policy),
17 };
18 
ethnl_cable_test_started(struct phy_device * phydev,u8 cmd)19 static 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 
49 out:
50 	nlmsg_free(skb);
51 	phydev_err(phydev, "%s: Error %pe\n", __func__, ERR_PTR(err));
52 
53 	return err;
54 }
55 
ethnl_act_cable_test(struct sk_buff * skb,struct genl_info * info)56 int 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 
96 out_rtnl:
97 	rtnl_unlock();
98 out_dev_put:
99 	ethnl_parse_header_dev_put(&req_info);
100 	return ret;
101 }
102 
ethnl_cable_test_alloc(struct phy_device * phydev,u8 cmd)103 int 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 
139 out:
140 	nlmsg_free(phydev->skb);
141 	phydev->skb = NULL;
142 	return err;
143 }
144 EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc);
145 
ethnl_cable_test_free(struct phy_device * phydev)146 void ethnl_cable_test_free(struct phy_device *phydev)
147 {
148 	nlmsg_free(phydev->skb);
149 	phydev->skb = NULL;
150 }
151 EXPORT_SYMBOL_GPL(ethnl_cable_test_free);
152 
ethnl_cable_test_finished(struct phy_device * phydev)153 void 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 }
161 EXPORT_SYMBOL_GPL(ethnl_cable_test_finished);
162 
ethnl_cable_test_result(struct phy_device * phydev,u8 pair,u8 result)163 int 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 
180 err:
181 	nla_nest_cancel(phydev->skb, nest);
182 	return ret;
183 }
184 EXPORT_SYMBOL_GPL(ethnl_cable_test_result);
185 
ethnl_cable_test_fault_length(struct phy_device * phydev,u8 pair,u32 cm)186 int 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 
204 err:
205 	nla_nest_cancel(phydev->skb, nest);
206 	return ret;
207 }
208 EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length);
209 
210 struct cable_test_tdr_req_info {
211 	struct ethnl_req_info		base;
212 };
213 
214 static 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 
221 const 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 */
ethnl_act_cable_test_tdr_cfg(const struct nlattr * nest,struct genl_info * info,struct phy_tdr_config * cfg)228 static 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 
ethnl_act_cable_test_tdr(struct sk_buff * skb,struct genl_info * info)307 int 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 
353 out_rtnl:
354 	rtnl_unlock();
355 out_dev_put:
356 	ethnl_parse_header_dev_put(&req_info);
357 	return ret;
358 }
359 
ethnl_cable_test_amplitude(struct phy_device * phydev,u8 pair,s16 mV)360 int 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 
379 err:
380 	nla_nest_cancel(phydev->skb, nest);
381 	return ret;
382 }
383 EXPORT_SYMBOL_GPL(ethnl_cable_test_amplitude);
384 
ethnl_cable_test_pulse(struct phy_device * phydev,u16 mV)385 int 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 
400 err:
401 	nla_nest_cancel(phydev->skb, nest);
402 	return ret;
403 }
404 EXPORT_SYMBOL_GPL(ethnl_cable_test_pulse);
405 
ethnl_cable_test_step(struct phy_device * phydev,u32 first,u32 last,u32 step)406 int 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 
429 err:
430 	nla_nest_cancel(phydev->skb, nest);
431 	return ret;
432 }
433 EXPORT_SYMBOL_GPL(ethnl_cable_test_step);
434