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 <kernel@pengutronix.de> |
7 | // |
8 | |
9 | #include "common.h" |
10 | #include "linux/pse-pd/pse.h" |
11 | #include "netlink.h" |
12 | #include <linux/ethtool_netlink.h> |
13 | #include <linux/ethtool.h> |
14 | #include <linux/phy.h> |
15 | |
16 | struct pse_req_info { |
17 | struct ethnl_req_info base; |
18 | }; |
19 | |
20 | struct pse_reply_data { |
21 | struct ethnl_reply_data base; |
22 | struct pse_control_status status; |
23 | }; |
24 | |
25 | #define PSE_REPDATA(__reply_base) \ |
26 | container_of(__reply_base, struct pse_reply_data, base) |
27 | |
28 | /* PSE_GET */ |
29 | |
30 | const struct nla_policy ethnl_pse_get_policy[ETHTOOL_A_PSE_HEADER + 1] = { |
31 | [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy), |
32 | }; |
33 | |
34 | static int pse_get_pse_attributes(struct net_device *dev, |
35 | struct netlink_ext_ack *extack, |
36 | struct pse_reply_data *data) |
37 | { |
38 | struct phy_device *phydev = dev->phydev; |
39 | |
40 | if (!phydev) { |
41 | NL_SET_ERR_MSG(extack, "No PHY is attached" ); |
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(psec: phydev->psec, extack, status: &data->status); |
53 | } |
54 | |
55 | static 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 | int ret; |
62 | |
63 | ret = ethnl_ops_begin(dev); |
64 | if (ret < 0) |
65 | return ret; |
66 | |
67 | ret = pse_get_pse_attributes(dev, extack: info->extack, data); |
68 | |
69 | ethnl_ops_complete(dev); |
70 | |
71 | return ret; |
72 | } |
73 | |
74 | static int pse_reply_size(const struct ethnl_req_info *req_base, |
75 | const struct ethnl_reply_data *reply_base) |
76 | { |
77 | const struct pse_reply_data *data = PSE_REPDATA(reply_base); |
78 | const struct pse_control_status *st = &data->status; |
79 | int len = 0; |
80 | |
81 | if (st->podl_admin_state > 0) |
82 | len += nla_total_size(payload: sizeof(u32)); /* _PODL_PSE_ADMIN_STATE */ |
83 | if (st->podl_pw_status > 0) |
84 | len += nla_total_size(payload: sizeof(u32)); /* _PODL_PSE_PW_D_STATUS */ |
85 | |
86 | return len; |
87 | } |
88 | |
89 | static int pse_fill_reply(struct sk_buff *skb, |
90 | const struct ethnl_req_info *req_base, |
91 | const struct ethnl_reply_data *reply_base) |
92 | { |
93 | const struct pse_reply_data *data = PSE_REPDATA(reply_base); |
94 | const struct pse_control_status *st = &data->status; |
95 | |
96 | if (st->podl_admin_state > 0 && |
97 | nla_put_u32(skb, attrtype: ETHTOOL_A_PODL_PSE_ADMIN_STATE, |
98 | value: st->podl_admin_state)) |
99 | return -EMSGSIZE; |
100 | |
101 | if (st->podl_pw_status > 0 && |
102 | nla_put_u32(skb, attrtype: ETHTOOL_A_PODL_PSE_PW_D_STATUS, |
103 | value: st->podl_pw_status)) |
104 | return -EMSGSIZE; |
105 | |
106 | return 0; |
107 | } |
108 | |
109 | /* PSE_SET */ |
110 | |
111 | const struct nla_policy ethnl_pse_set_policy[ETHTOOL_A_PSE_MAX + 1] = { |
112 | [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy), |
113 | [ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] = |
114 | NLA_POLICY_RANGE(NLA_U32, ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED, |
115 | ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED), |
116 | }; |
117 | |
118 | static int |
119 | ethnl_set_pse_validate(struct ethnl_req_info *req_info, struct genl_info *info) |
120 | { |
121 | return !!info->attrs[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]; |
122 | } |
123 | |
124 | static int |
125 | ethnl_set_pse(struct ethnl_req_info *req_info, struct genl_info *info) |
126 | { |
127 | struct net_device *dev = req_info->dev; |
128 | struct pse_control_config config = {}; |
129 | struct nlattr **tb = info->attrs; |
130 | struct phy_device *phydev; |
131 | |
132 | /* this values are already validated by the ethnl_pse_set_policy */ |
133 | config.admin_cotrol = nla_get_u32(nla: tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]); |
134 | |
135 | phydev = dev->phydev; |
136 | if (!phydev) { |
137 | NL_SET_ERR_MSG(info->extack, "No PHY is attached" ); |
138 | return -EOPNOTSUPP; |
139 | } |
140 | |
141 | if (!phydev->psec) { |
142 | NL_SET_ERR_MSG(info->extack, "No PSE is attached" ); |
143 | return -EOPNOTSUPP; |
144 | } |
145 | |
146 | /* Return errno directly - PSE has no notification */ |
147 | return pse_ethtool_set_config(psec: phydev->psec, extack: info->extack, config: &config); |
148 | } |
149 | |
150 | const struct ethnl_request_ops ethnl_pse_request_ops = { |
151 | .request_cmd = ETHTOOL_MSG_PSE_GET, |
152 | .reply_cmd = ETHTOOL_MSG_PSE_GET_REPLY, |
153 | .hdr_attr = ETHTOOL_A_PSE_HEADER, |
154 | .req_info_size = sizeof(struct pse_req_info), |
155 | .reply_data_size = sizeof(struct pse_reply_data), |
156 | |
157 | .prepare_data = pse_prepare_data, |
158 | .reply_size = pse_reply_size, |
159 | .fill_reply = pse_fill_reply, |
160 | |
161 | .set_validate = ethnl_set_pse_validate, |
162 | .set = ethnl_set_pse, |
163 | /* PSE has no notification */ |
164 | }; |
165 | |