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