Commit | Line | Data |
---|---|---|
11ca3c42 AL |
1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | ||
3 | #include <linux/phy.h> | |
1dd3f212 | 4 | #include <linux/ethtool_netlink.h> |
11ca3c42 AL |
5 | #include "netlink.h" |
6 | #include "common.h" | |
7 | ||
8 | /* CABLE_TEST_ACT */ | |
9 | ||
10 | static const struct nla_policy | |
11 | cable_test_act_policy[ETHTOOL_A_CABLE_TEST_MAX + 1] = { | |
12 | [ETHTOOL_A_CABLE_TEST_UNSPEC] = { .type = NLA_REJECT }, | |
13 | [ETHTOOL_A_CABLE_TEST_HEADER] = { .type = NLA_NESTED }, | |
14 | }; | |
15 | ||
1a644de2 | 16 | static int ethnl_cable_test_started(struct phy_device *phydev, u8 cmd) |
9896a457 AL |
17 | { |
18 | struct sk_buff *skb; | |
19 | int err = -ENOMEM; | |
20 | void *ehdr; | |
21 | ||
22 | skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); | |
23 | if (!skb) | |
24 | goto out; | |
25 | ||
1a644de2 | 26 | ehdr = ethnl_bcastmsg_put(skb, cmd); |
9896a457 AL |
27 | if (!ehdr) { |
28 | err = -EMSGSIZE; | |
29 | goto out; | |
30 | } | |
31 | ||
32 | err = ethnl_fill_reply_header(skb, phydev->attached_dev, | |
33 | ETHTOOL_A_CABLE_TEST_NTF_HEADER); | |
34 | if (err) | |
35 | goto out; | |
36 | ||
37 | err = nla_put_u8(skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS, | |
38 | ETHTOOL_A_CABLE_TEST_NTF_STATUS_STARTED); | |
39 | if (err) | |
40 | goto out; | |
41 | ||
42 | genlmsg_end(skb, ehdr); | |
43 | ||
44 | return ethnl_multicast(skb, phydev->attached_dev); | |
45 | ||
46 | out: | |
47 | nlmsg_free(skb); | |
48 | phydev_err(phydev, "%s: Error %pe\n", __func__, ERR_PTR(err)); | |
49 | ||
50 | return err; | |
51 | } | |
52 | ||
11ca3c42 AL |
53 | int ethnl_act_cable_test(struct sk_buff *skb, struct genl_info *info) |
54 | { | |
55 | struct nlattr *tb[ETHTOOL_A_CABLE_TEST_MAX + 1]; | |
56 | struct ethnl_req_info req_info = {}; | |
57 | struct net_device *dev; | |
58 | int ret; | |
59 | ||
60 | ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, | |
61 | ETHTOOL_A_CABLE_TEST_MAX, | |
62 | cable_test_act_policy, info->extack); | |
63 | if (ret < 0) | |
64 | return ret; | |
65 | ||
66 | ret = ethnl_parse_header_dev_get(&req_info, | |
67 | tb[ETHTOOL_A_CABLE_TEST_HEADER], | |
68 | genl_info_net(info), info->extack, | |
69 | true); | |
70 | if (ret < 0) | |
71 | return ret; | |
72 | ||
73 | dev = req_info.dev; | |
74 | if (!dev->phydev) { | |
75 | ret = -EOPNOTSUPP; | |
76 | goto out_dev_put; | |
77 | } | |
78 | ||
79 | rtnl_lock(); | |
80 | ret = ethnl_ops_begin(dev); | |
81 | if (ret < 0) | |
82 | goto out_rtnl; | |
83 | ||
84 | ret = phy_start_cable_test(dev->phydev, info->extack); | |
85 | ||
86 | ethnl_ops_complete(dev); | |
9896a457 AL |
87 | |
88 | if (!ret) | |
1a644de2 AL |
89 | ethnl_cable_test_started(dev->phydev, |
90 | ETHTOOL_MSG_CABLE_TEST_NTF); | |
9896a457 | 91 | |
11ca3c42 AL |
92 | out_rtnl: |
93 | rtnl_unlock(); | |
94 | out_dev_put: | |
95 | dev_put(dev); | |
96 | return ret; | |
97 | } | |
1dd3f212 | 98 | |
1a644de2 | 99 | int ethnl_cable_test_alloc(struct phy_device *phydev, u8 cmd) |
1dd3f212 AL |
100 | { |
101 | int err = -ENOMEM; | |
102 | ||
103 | phydev->skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); | |
104 | if (!phydev->skb) | |
105 | goto out; | |
106 | ||
1a644de2 | 107 | phydev->ehdr = ethnl_bcastmsg_put(phydev->skb, cmd); |
1dd3f212 AL |
108 | if (!phydev->ehdr) { |
109 | err = -EMSGSIZE; | |
110 | goto out; | |
111 | } | |
112 | ||
113 | err = ethnl_fill_reply_header(phydev->skb, phydev->attached_dev, | |
114 | ETHTOOL_A_CABLE_TEST_NTF_HEADER); | |
115 | if (err) | |
116 | goto out; | |
117 | ||
118 | err = nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_TEST_NTF_STATUS, | |
119 | ETHTOOL_A_CABLE_TEST_NTF_STATUS_COMPLETED); | |
120 | if (err) | |
121 | goto out; | |
122 | ||
123 | phydev->nest = nla_nest_start(phydev->skb, | |
124 | ETHTOOL_A_CABLE_TEST_NTF_NEST); | |
1e2dc145 AL |
125 | if (!phydev->nest) { |
126 | err = -EMSGSIZE; | |
1dd3f212 | 127 | goto out; |
1e2dc145 | 128 | } |
1dd3f212 AL |
129 | |
130 | return 0; | |
131 | ||
132 | out: | |
133 | nlmsg_free(phydev->skb); | |
1e2dc145 | 134 | phydev->skb = NULL; |
1dd3f212 AL |
135 | return err; |
136 | } | |
137 | EXPORT_SYMBOL_GPL(ethnl_cable_test_alloc); | |
138 | ||
139 | void ethnl_cable_test_free(struct phy_device *phydev) | |
140 | { | |
141 | nlmsg_free(phydev->skb); | |
1e2dc145 | 142 | phydev->skb = NULL; |
1dd3f212 AL |
143 | } |
144 | EXPORT_SYMBOL_GPL(ethnl_cable_test_free); | |
145 | ||
146 | void ethnl_cable_test_finished(struct phy_device *phydev) | |
147 | { | |
148 | nla_nest_end(phydev->skb, phydev->nest); | |
149 | ||
150 | genlmsg_end(phydev->skb, phydev->ehdr); | |
151 | ||
152 | ethnl_multicast(phydev->skb, phydev->attached_dev); | |
153 | } | |
154 | EXPORT_SYMBOL_GPL(ethnl_cable_test_finished); | |
1e2dc145 AL |
155 | |
156 | int ethnl_cable_test_result(struct phy_device *phydev, u8 pair, u8 result) | |
157 | { | |
158 | struct nlattr *nest; | |
159 | int ret = -EMSGSIZE; | |
160 | ||
161 | nest = nla_nest_start(phydev->skb, ETHTOOL_A_CABLE_NEST_RESULT); | |
162 | if (!nest) | |
163 | return -EMSGSIZE; | |
164 | ||
165 | if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_PAIR, pair)) | |
166 | goto err; | |
167 | if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_RESULT_CODE, result)) | |
168 | goto err; | |
169 | ||
170 | nla_nest_end(phydev->skb, nest); | |
171 | return 0; | |
172 | ||
173 | err: | |
174 | nla_nest_cancel(phydev->skb, nest); | |
175 | return ret; | |
176 | } | |
177 | EXPORT_SYMBOL_GPL(ethnl_cable_test_result); | |
178 | ||
179 | int ethnl_cable_test_fault_length(struct phy_device *phydev, u8 pair, u32 cm) | |
180 | { | |
181 | struct nlattr *nest; | |
182 | int ret = -EMSGSIZE; | |
183 | ||
184 | nest = nla_nest_start(phydev->skb, | |
185 | ETHTOOL_A_CABLE_NEST_FAULT_LENGTH); | |
186 | if (!nest) | |
187 | return -EMSGSIZE; | |
188 | ||
189 | if (nla_put_u8(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_PAIR, pair)) | |
190 | goto err; | |
191 | if (nla_put_u32(phydev->skb, ETHTOOL_A_CABLE_FAULT_LENGTH_CM, cm)) | |
192 | goto err; | |
193 | ||
194 | nla_nest_end(phydev->skb, nest); | |
195 | return 0; | |
196 | ||
197 | err: | |
198 | nla_nest_cancel(phydev->skb, nest); | |
199 | return ret; | |
200 | } | |
201 | EXPORT_SYMBOL_GPL(ethnl_cable_test_fault_length); | |
1a644de2 AL |
202 | |
203 | static const struct nla_policy | |
204 | cable_test_tdr_act_policy[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1] = { | |
205 | [ETHTOOL_A_CABLE_TEST_TDR_UNSPEC] = { .type = NLA_REJECT }, | |
206 | [ETHTOOL_A_CABLE_TEST_TDR_HEADER] = { .type = NLA_NESTED }, | |
207 | }; | |
208 | ||
209 | int ethnl_act_cable_test_tdr(struct sk_buff *skb, struct genl_info *info) | |
210 | { | |
211 | struct nlattr *tb[ETHTOOL_A_CABLE_TEST_TDR_MAX + 1]; | |
212 | struct ethnl_req_info req_info = {}; | |
213 | struct net_device *dev; | |
214 | int ret; | |
215 | ||
216 | ret = nlmsg_parse(info->nlhdr, GENL_HDRLEN, tb, | |
217 | ETHTOOL_A_CABLE_TEST_TDR_MAX, | |
218 | cable_test_tdr_act_policy, info->extack); | |
219 | if (ret < 0) | |
220 | return ret; | |
221 | ||
222 | ret = ethnl_parse_header_dev_get(&req_info, | |
223 | tb[ETHTOOL_A_CABLE_TEST_TDR_HEADER], | |
224 | genl_info_net(info), info->extack, | |
225 | true); | |
226 | if (ret < 0) | |
227 | return ret; | |
228 | ||
229 | dev = req_info.dev; | |
230 | if (!dev->phydev) { | |
231 | ret = -EOPNOTSUPP; | |
232 | goto out_dev_put; | |
233 | } | |
234 | ||
235 | rtnl_lock(); | |
236 | ret = ethnl_ops_begin(dev); | |
237 | if (ret < 0) | |
238 | goto out_rtnl; | |
239 | ||
240 | ret = phy_start_cable_test_tdr(dev->phydev, info->extack); | |
241 | ||
242 | ethnl_ops_complete(dev); | |
243 | ||
244 | if (!ret) | |
245 | ethnl_cable_test_started(dev->phydev, | |
246 | ETHTOOL_MSG_CABLE_TEST_TDR_NTF); | |
247 | ||
248 | out_rtnl: | |
249 | rtnl_unlock(); | |
250 | out_dev_put: | |
251 | dev_put(dev); | |
252 | return ret; | |
253 | } |