projects/linux·blob master · refs

383 lines · 10.1 KiB

1// SPDX-License-Identifier: GPL-2.0-only
2//
3// ethtool interface for Ethernet PSE (Power Sourcing Equipment)
4// and PD (Powered Device)
5//
6// Copyright (c) 2022 Pengutronix, Oleksij Rempel <[email protected]>
7//
8
9#include <linux/ethtool_netlink.h>
10#include <linux/ethtool.h>
11#include <linux/export.h>
12#include <linux/phy.h>
13
14#include "common.h"
15#include "linux/pse-pd/pse.h"
16#include "netlink.h"
17
18struct pse_req_info {
19 struct ethnl_req_info base;
20};
21
22struct pse_reply_data {
23 struct ethnl_reply_data base;
24 struct ethtool_pse_control_status status;
25};
26
27#define PSE_REPDATA(__reply_base) \
28 container_of(__reply_base, struct pse_reply_data, base)
29
30/* PSE_GET */
31
32const struct nla_policy ethnl_pse_get_policy[ETHTOOL_A_PSE_HEADER + 1] = {
33 [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy_phy),
34};
35
36static int pse_get_pse_attributes(struct phy_device *phydev,
37 struct netlink_ext_ack *extack,
38 struct pse_reply_data *data)
39{
40 if (!phydev) {
41 NL_SET_ERR_MSG(extack, "No PHY found");
42 return -EOPNOTSUPP;
43 }
44
45 if (!phydev->psec) {
46 NL_SET_ERR_MSG(extack, "No PSE is attached");
47 return -EOPNOTSUPP;
48 }
49
50 memset(&data->status, 0, sizeof(data->status));
51
52 return pse_ethtool_get_status(phydev->psec, extack, &data->status);
53}
54
55static int pse_prepare_data(const struct ethnl_req_info *req_base,
56 struct ethnl_reply_data *reply_base,
57 const struct genl_info *info)
58{
59 struct pse_reply_data *data = PSE_REPDATA(reply_base);
60 struct net_device *dev = reply_base->dev;
61 struct nlattr **tb = info->attrs;
62 struct phy_device *phydev;
63 int ret;
64
65 phydev = ethnl_req_get_phydev(req_base, tb, ETHTOOL_A_PSE_HEADER,
66 info->extack);
67 if (IS_ERR(phydev))
68 return PTR_ERR(phydev);
69
70 ret = ethnl_ops_begin(dev);
71 if (ret < 0)
72 return ret;
73
74 ret = pse_get_pse_attributes(phydev, info->extack, data);
75
76 ethnl_ops_complete(dev);
77
78 return ret;
79}
80
81static int pse_reply_size(const struct ethnl_req_info *req_base,
82 const struct ethnl_reply_data *reply_base)
83{
84 const struct pse_reply_data *data = PSE_REPDATA(reply_base);
85 const struct ethtool_pse_control_status *st = &data->status;
86 int len = 0;
87
88 if (st->pw_d_id)
89 len += nla_total_size(sizeof(u32)); /* _PSE_PW_D_ID */
90 if (st->podl_admin_state > 0)
91 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_ADMIN_STATE */
92 if (st->podl_pw_status > 0)
93 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_PW_D_STATUS */
94 if (st->c33_admin_state > 0)
95 len += nla_total_size(sizeof(u32)); /* _C33_PSE_ADMIN_STATE */
96 if (st->c33_pw_status > 0)
97 len += nla_total_size(sizeof(u32)); /* _C33_PSE_PW_D_STATUS */
98 if (st->c33_pw_class > 0)
99 len += nla_total_size(sizeof(u32)); /* _C33_PSE_PW_CLASS */
100 if (st->c33_actual_pw > 0)
101 len += nla_total_size(sizeof(u32)); /* _C33_PSE_ACTUAL_PW */
102 if (st->c33_ext_state_info.c33_pse_ext_state > 0) {
103 len += nla_total_size(sizeof(u32)); /* _C33_PSE_EXT_STATE */
104 if (st->c33_ext_state_info.__c33_pse_ext_substate > 0)
105 /* _C33_PSE_EXT_SUBSTATE */
106 len += nla_total_size(sizeof(u32));
107 }
108 if (st->c33_avail_pw_limit > 0)
109 /* _C33_AVAIL_PSE_PW_LIMIT */
110 len += nla_total_size(sizeof(u32));
111 if (st->c33_pw_limit_nb_ranges > 0)
112 /* _C33_PSE_PW_LIMIT_RANGES */
113 len += st->c33_pw_limit_nb_ranges *
114 (nla_total_size(0) +
115 nla_total_size(sizeof(u32)) * 2);
116 if (st->prio_max)
117 /* _PSE_PRIO_MAX + _PSE_PRIO */
118 len += nla_total_size(sizeof(u32)) * 2;
119
120 return len;
121}
122
123static int pse_put_pw_limit_ranges(struct sk_buff *skb,
124 const struct ethtool_pse_control_status *st)
125{
126 const struct ethtool_c33_pse_pw_limit_range *pw_limit_ranges;
127 int i;
128
129 pw_limit_ranges = st->c33_pw_limit_ranges;
130 for (i = 0; i < st->c33_pw_limit_nb_ranges; i++) {
131 struct nlattr *nest;
132
133 nest = nla_nest_start(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_RANGES);
134 if (!nest)
135 return -EMSGSIZE;
136
137 if (nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_MIN,
138 pw_limit_ranges->min) ||
139 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_MAX,
140 pw_limit_ranges->max)) {
141 nla_nest_cancel(skb, nest);
142 return -EMSGSIZE;
143 }
144 nla_nest_end(skb, nest);
145 pw_limit_ranges++;
146 }
147
148 return 0;
149}
150
151static int pse_fill_reply(struct sk_buff *skb,
152 const struct ethnl_req_info *req_base,
153 const struct ethnl_reply_data *reply_base)
154{
155 const struct pse_reply_data *data = PSE_REPDATA(reply_base);
156 const struct ethtool_pse_control_status *st = &data->status;
157
158 if (st->pw_d_id &&
159 nla_put_u32(skb, ETHTOOL_A_PSE_PW_D_ID,
160 st->pw_d_id))
161 return -EMSGSIZE;
162
163 if (st->podl_admin_state > 0 &&
164 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_ADMIN_STATE,
165 st->podl_admin_state))
166 return -EMSGSIZE;
167
168 if (st->podl_pw_status > 0 &&
169 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_PW_D_STATUS,
170 st->podl_pw_status))
171 return -EMSGSIZE;
172
173 if (st->c33_admin_state > 0 &&
174 nla_put_u32(skb, ETHTOOL_A_C33_PSE_ADMIN_STATE,
175 st->c33_admin_state))
176 return -EMSGSIZE;
177
178 if (st->c33_pw_status > 0 &&
179 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_D_STATUS,
180 st->c33_pw_status))
181 return -EMSGSIZE;
182
183 if (st->c33_pw_class > 0 &&
184 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_CLASS,
185 st->c33_pw_class))
186 return -EMSGSIZE;
187
188 if (st->c33_actual_pw > 0 &&
189 nla_put_u32(skb, ETHTOOL_A_C33_PSE_ACTUAL_PW,
190 st->c33_actual_pw))
191 return -EMSGSIZE;
192
193 if (st->c33_ext_state_info.c33_pse_ext_state > 0) {
194 if (nla_put_u32(skb, ETHTOOL_A_C33_PSE_EXT_STATE,
195 st->c33_ext_state_info.c33_pse_ext_state))
196 return -EMSGSIZE;
197
198 if (st->c33_ext_state_info.__c33_pse_ext_substate > 0 &&
199 nla_put_u32(skb, ETHTOOL_A_C33_PSE_EXT_SUBSTATE,
200 st->c33_ext_state_info.__c33_pse_ext_substate))
201 return -EMSGSIZE;
202 }
203
204 if (st->c33_avail_pw_limit > 0 &&
205 nla_put_u32(skb, ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT,
206 st->c33_avail_pw_limit))
207 return -EMSGSIZE;
208
209 if (st->c33_pw_limit_nb_ranges > 0 &&
210 pse_put_pw_limit_ranges(skb, st))
211 return -EMSGSIZE;
212
213 if (st->prio_max &&
214 (nla_put_u32(skb, ETHTOOL_A_PSE_PRIO_MAX, st->prio_max) ||
215 nla_put_u32(skb, ETHTOOL_A_PSE_PRIO, st->prio)))
216 return -EMSGSIZE;
217
218 return 0;
219}
220
221static void pse_cleanup_data(struct ethnl_reply_data *reply_base)
222{
223 const struct pse_reply_data *data = PSE_REPDATA(reply_base);
224
225 kfree(data->status.c33_pw_limit_ranges);
226}
227
228/* PSE_SET */
229
230const struct nla_policy ethnl_pse_set_policy[ETHTOOL_A_PSE_MAX + 1] = {
231 [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy_phy),
232 [ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] =
233 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED,
234 ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED),
235 [ETHTOOL_A_C33_PSE_ADMIN_CONTROL] =
236 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED,
237 ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED),
238 [ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT] = { .type = NLA_U32 },
239 [ETHTOOL_A_PSE_PRIO] = { .type = NLA_U32 },
240};
241
242static int
243ethnl_set_pse_validate(struct phy_device *phydev, struct genl_info *info)
244{
245 struct nlattr **tb = info->attrs;
246
247 if (IS_ERR_OR_NULL(phydev)) {
248 NL_SET_ERR_MSG(info->extack, "No PHY is attached");
249 return -EOPNOTSUPP;
250 }
251
252 if (!phydev->psec) {
253 NL_SET_ERR_MSG(info->extack, "No PSE is attached");
254 return -EOPNOTSUPP;
255 }
256
257 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] &&
258 !pse_has_podl(phydev->psec)) {
259 NL_SET_ERR_MSG_ATTR(info->extack,
260 tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL],
261 "setting PoDL PSE admin control not supported");
262 return -EOPNOTSUPP;
263 }
264 if (tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL] &&
265 !pse_has_c33(phydev->psec)) {
266 NL_SET_ERR_MSG_ATTR(info->extack,
267 tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL],
268 "setting C33 PSE admin control not supported");
269 return -EOPNOTSUPP;
270 }
271
272 return 0;
273}
274
275static int
276ethnl_set_pse(struct ethnl_req_info *req_info, struct genl_info *info)
277{
278 struct nlattr **tb = info->attrs;
279 struct phy_device *phydev;
280 int ret;
281
282 phydev = ethnl_req_get_phydev(req_info, tb, ETHTOOL_A_PSE_HEADER,
283 info->extack);
284 ret = ethnl_set_pse_validate(phydev, info);
285 if (ret)
286 return ret;
287
288 if (tb[ETHTOOL_A_PSE_PRIO]) {
289 unsigned int prio;
290
291 prio = nla_get_u32(tb[ETHTOOL_A_PSE_PRIO]);
292 ret = pse_ethtool_set_prio(phydev->psec, info->extack, prio);
293 if (ret)
294 return ret;
295 }
296
297 if (tb[ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT]) {
298 unsigned int pw_limit;
299
300 pw_limit = nla_get_u32(tb[ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT]);
301 ret = pse_ethtool_set_pw_limit(phydev->psec, info->extack,
302 pw_limit);
303 if (ret)
304 return ret;
305 }
306
307 /* These values are already validated by the ethnl_pse_set_policy */
308 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] ||
309 tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]) {
310 struct pse_control_config config = {};
311
312 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL])
313 config.podl_admin_control = nla_get_u32(tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]);
314 if (tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL])
315 config.c33_admin_control = nla_get_u32(tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]);
316
317 /* pse_ethtool_set_config() will do nothing if the config
318 * is zero
319 */
320 ret = pse_ethtool_set_config(phydev->psec, info->extack,
321 &config);
322 if (ret)
323 return ret;
324 }
325
326 /* Return errno or zero - PSE has no notification */
327 return ret;
328}
329
330const struct ethnl_request_ops ethnl_pse_request_ops = {
331 .request_cmd = ETHTOOL_MSG_PSE_GET,
332 .reply_cmd = ETHTOOL_MSG_PSE_GET_REPLY,
333 .hdr_attr = ETHTOOL_A_PSE_HEADER,
334 .req_info_size = sizeof(struct pse_req_info),
335 .reply_data_size = sizeof(struct pse_reply_data),
336
337 .prepare_data = pse_prepare_data,
338 .reply_size = pse_reply_size,
339 .fill_reply = pse_fill_reply,
340 .cleanup_data = pse_cleanup_data,
341
342 .set = ethnl_set_pse,
343 /* PSE has no notification */
344};
345
346void ethnl_pse_send_ntf(struct net_device *netdev, unsigned long notifs)
347{
348 void *reply_payload;
349 struct sk_buff *skb;
350 int reply_len;
351 int ret;
352
353 ASSERT_RTNL();
354
355 if (!netdev || !notifs)
356 return;
357
358 reply_len = ethnl_reply_header_size() +
359 nla_total_size(sizeof(u32)); /* _PSE_NTF_EVENTS */
360
361 skb = genlmsg_new(reply_len, GFP_KERNEL);
362 if (!skb)
363 return;
364
365 reply_payload = ethnl_bcastmsg_put(skb, ETHTOOL_MSG_PSE_NTF);
366 if (!reply_payload)
367 goto err_skb;
368
369 ret = ethnl_fill_reply_header(skb, netdev, ETHTOOL_A_PSE_NTF_HEADER);
370 if (ret < 0)
371 goto err_skb;
372
373 if (nla_put_uint(skb, ETHTOOL_A_PSE_NTF_EVENTS, notifs))
374 goto err_skb;
375
376 genlmsg_end(skb, reply_payload);
377 ethnl_multicast(skb, netdev);
378 return;
379
380err_skb:
381 nlmsg_free(skb);
382}
383EXPORT_SYMBOL_GPL(ethnl_pse_send_ntf);