projects/linux·blob master · refs

271 lines · 7.8 KiB

1// SPDX-License-Identifier: GPL-2.0-only
2
3#include <linux/phy.h>
4#include <linux/ethtool_netlink.h>
5
6#include "common.h"
7#include "netlink.h"
8
9struct plca_req_info {
10 struct ethnl_req_info base;
11};
12
13struct plca_reply_data {
14 struct ethnl_reply_data base;
15 struct phy_plca_cfg plca_cfg;
16 struct phy_plca_status plca_st;
17};
18
19// Helpers ------------------------------------------------------------------ //
20
21#define PLCA_REPDATA(__reply_base) \
22 container_of(__reply_base, struct plca_reply_data, base)
23
24// PLCA get configuration message ------------------------------------------- //
25
26const struct nla_policy ethnl_plca_get_cfg_policy[] = {
27 [ETHTOOL_A_PLCA_HEADER] =
28 NLA_POLICY_NESTED(ethnl_header_policy_phy),
29};
30
31static void plca_update_sint(int *dst, struct nlattr **tb, u32 attrid,
32 bool *mod)
33{
34 const struct nlattr *attr = tb[attrid];
35
36 if (!attr ||
37 WARN_ON_ONCE(attrid >= ARRAY_SIZE(ethnl_plca_set_cfg_policy)))
38 return;
39
40 switch (ethnl_plca_set_cfg_policy[attrid].type) {
41 case NLA_U8:
42 *dst = nla_get_u8(attr);
43 break;
44 case NLA_U32:
45 *dst = nla_get_u32(attr);
46 break;
47 default:
48 WARN_ON_ONCE(1);
49 }
50
51 *mod = true;
52}
53
54static int plca_get_cfg_prepare_data(const struct ethnl_req_info *req_base,
55 struct ethnl_reply_data *reply_base,
56 const struct genl_info *info)
57{
58 struct plca_reply_data *data = PLCA_REPDATA(reply_base);
59 struct net_device *dev = reply_base->dev;
60 const struct ethtool_phy_ops *ops;
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_PLCA_HEADER,
66 info->extack);
67 // check that the PHY device is available and connected
68 if (IS_ERR_OR_NULL(phydev)) {
69 ret = -EOPNOTSUPP;
70 goto out;
71 }
72
73 // note: rtnl_lock is held already by ethnl_default_doit
74 ops = ethtool_phy_ops;
75 if (!ops || !ops->get_plca_cfg) {
76 ret = -EOPNOTSUPP;
77 goto out;
78 }
79
80 ret = ethnl_ops_begin(dev);
81 if (ret < 0)
82 goto out;
83
84 memset(&data->plca_cfg, 0xff,
85 sizeof_field(struct plca_reply_data, plca_cfg));
86
87 ret = ops->get_plca_cfg(phydev, &data->plca_cfg);
88 ethnl_ops_complete(dev);
89
90out:
91 return ret;
92}
93
94static int plca_get_cfg_reply_size(const struct ethnl_req_info *req_base,
95 const struct ethnl_reply_data *reply_base)
96{
97 return nla_total_size(sizeof(u16)) + /* _VERSION */
98 nla_total_size(sizeof(u8)) + /* _ENABLED */
99 nla_total_size(sizeof(u32)) + /* _NODE_CNT */
100 nla_total_size(sizeof(u32)) + /* _NODE_ID */
101 nla_total_size(sizeof(u32)) + /* _TO_TIMER */
102 nla_total_size(sizeof(u32)) + /* _BURST_COUNT */
103 nla_total_size(sizeof(u32)); /* _BURST_TIMER */
104}
105
106static int plca_get_cfg_fill_reply(struct sk_buff *skb,
107 const struct ethnl_req_info *req_base,
108 const struct ethnl_reply_data *reply_base)
109{
110 const struct plca_reply_data *data = PLCA_REPDATA(reply_base);
111 const struct phy_plca_cfg *plca = &data->plca_cfg;
112
113 if ((plca->version >= 0 &&
114 nla_put_u16(skb, ETHTOOL_A_PLCA_VERSION, plca->version)) ||
115 (plca->enabled >= 0 &&
116 nla_put_u8(skb, ETHTOOL_A_PLCA_ENABLED, !!plca->enabled)) ||
117 (plca->node_id >= 0 &&
118 nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_ID, plca->node_id)) ||
119 (plca->node_cnt >= 0 &&
120 nla_put_u32(skb, ETHTOOL_A_PLCA_NODE_CNT, plca->node_cnt)) ||
121 (plca->to_tmr >= 0 &&
122 nla_put_u32(skb, ETHTOOL_A_PLCA_TO_TMR, plca->to_tmr)) ||
123 (plca->burst_cnt >= 0 &&
124 nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_CNT, plca->burst_cnt)) ||
125 (plca->burst_tmr >= 0 &&
126 nla_put_u32(skb, ETHTOOL_A_PLCA_BURST_TMR, plca->burst_tmr)))
127 return -EMSGSIZE;
128
129 return 0;
130};
131
132// PLCA set configuration message ------------------------------------------- //
133
134const struct nla_policy ethnl_plca_set_cfg_policy[] = {
135 [ETHTOOL_A_PLCA_HEADER] =
136 NLA_POLICY_NESTED(ethnl_header_policy_phy),
137 [ETHTOOL_A_PLCA_ENABLED] = NLA_POLICY_MAX(NLA_U8, 1),
138 [ETHTOOL_A_PLCA_NODE_ID] = NLA_POLICY_MAX(NLA_U32, 255),
139 [ETHTOOL_A_PLCA_NODE_CNT] = NLA_POLICY_RANGE(NLA_U32, 1, 255),
140 [ETHTOOL_A_PLCA_TO_TMR] = NLA_POLICY_MAX(NLA_U32, 255),
141 [ETHTOOL_A_PLCA_BURST_CNT] = NLA_POLICY_MAX(NLA_U32, 255),
142 [ETHTOOL_A_PLCA_BURST_TMR] = NLA_POLICY_MAX(NLA_U32, 255),
143};
144
145static int
146ethnl_set_plca(struct ethnl_req_info *req_info, struct genl_info *info)
147{
148 const struct ethtool_phy_ops *ops;
149 struct nlattr **tb = info->attrs;
150 struct phy_plca_cfg plca_cfg;
151 struct phy_device *phydev;
152 bool mod = false;
153 int ret;
154
155 phydev = ethnl_req_get_phydev(req_info, tb, ETHTOOL_A_PLCA_HEADER,
156 info->extack);
157 // check that the PHY device is available and connected
158 if (IS_ERR_OR_NULL(phydev))
159 return -EOPNOTSUPP;
160
161 ops = ethtool_phy_ops;
162 if (!ops || !ops->set_plca_cfg)
163 return -EOPNOTSUPP;
164
165 memset(&plca_cfg, 0xff, sizeof(plca_cfg));
166 plca_update_sint(&plca_cfg.enabled, tb, ETHTOOL_A_PLCA_ENABLED, &mod);
167 plca_update_sint(&plca_cfg.node_id, tb, ETHTOOL_A_PLCA_NODE_ID, &mod);
168 plca_update_sint(&plca_cfg.node_cnt, tb, ETHTOOL_A_PLCA_NODE_CNT, &mod);
169 plca_update_sint(&plca_cfg.to_tmr, tb, ETHTOOL_A_PLCA_TO_TMR, &mod);
170 plca_update_sint(&plca_cfg.burst_cnt, tb, ETHTOOL_A_PLCA_BURST_CNT,
171 &mod);
172 plca_update_sint(&plca_cfg.burst_tmr, tb, ETHTOOL_A_PLCA_BURST_TMR,
173 &mod);
174 if (!mod)
175 return 0;
176
177 ret = ops->set_plca_cfg(phydev, &plca_cfg, info->extack);
178 return ret < 0 ? ret : 1;
179}
180
181const struct ethnl_request_ops ethnl_plca_cfg_request_ops = {
182 .request_cmd = ETHTOOL_MSG_PLCA_GET_CFG,
183 .reply_cmd = ETHTOOL_MSG_PLCA_GET_CFG_REPLY,
184 .hdr_attr = ETHTOOL_A_PLCA_HEADER,
185 .req_info_size = sizeof(struct plca_req_info),
186 .reply_data_size = sizeof(struct plca_reply_data),
187
188 .prepare_data = plca_get_cfg_prepare_data,
189 .reply_size = plca_get_cfg_reply_size,
190 .fill_reply = plca_get_cfg_fill_reply,
191
192 .set = ethnl_set_plca,
193 .set_ntf_cmd = ETHTOOL_MSG_PLCA_NTF,
194};
195
196// PLCA get status message -------------------------------------------------- //
197
198const struct nla_policy ethnl_plca_get_status_policy[] = {
199 [ETHTOOL_A_PLCA_HEADER] =
200 NLA_POLICY_NESTED(ethnl_header_policy_phy),
201};
202
203static int plca_get_status_prepare_data(const struct ethnl_req_info *req_base,
204 struct ethnl_reply_data *reply_base,
205 const struct genl_info *info)
206{
207 struct plca_reply_data *data = PLCA_REPDATA(reply_base);
208 struct net_device *dev = reply_base->dev;
209 const struct ethtool_phy_ops *ops;
210 struct nlattr **tb = info->attrs;
211 struct phy_device *phydev;
212 int ret;
213
214 phydev = ethnl_req_get_phydev(req_base, tb, ETHTOOL_A_PLCA_HEADER,
215 info->extack);
216 // check that the PHY device is available and connected
217 if (IS_ERR_OR_NULL(phydev)) {
218 ret = -EOPNOTSUPP;
219 goto out;
220 }
221
222 // note: rtnl_lock is held already by ethnl_default_doit
223 ops = ethtool_phy_ops;
224 if (!ops || !ops->get_plca_status) {
225 ret = -EOPNOTSUPP;
226 goto out;
227 }
228
229 ret = ethnl_ops_begin(dev);
230 if (ret < 0)
231 goto out;
232
233 memset(&data->plca_st, 0xff,
234 sizeof_field(struct plca_reply_data, plca_st));
235
236 ret = ops->get_plca_status(phydev, &data->plca_st);
237 ethnl_ops_complete(dev);
238out:
239 return ret;
240}
241
242static int plca_get_status_reply_size(const struct ethnl_req_info *req_base,
243 const struct ethnl_reply_data *reply_base)
244{
245 return nla_total_size(sizeof(u8)); /* _STATUS */
246}
247
248static int plca_get_status_fill_reply(struct sk_buff *skb,
249 const struct ethnl_req_info *req_base,
250 const struct ethnl_reply_data *reply_base)
251{
252 const struct plca_reply_data *data = PLCA_REPDATA(reply_base);
253 const u8 status = data->plca_st.pst;
254
255 if (nla_put_u8(skb, ETHTOOL_A_PLCA_STATUS, !!status))
256 return -EMSGSIZE;
257
258 return 0;
259};
260
261const struct ethnl_request_ops ethnl_plca_status_request_ops = {
262 .request_cmd = ETHTOOL_MSG_PLCA_GET_STATUS,
263 .reply_cmd = ETHTOOL_MSG_PLCA_GET_STATUS_REPLY,
264 .hdr_attr = ETHTOOL_A_PLCA_HEADER,
265 .req_info_size = sizeof(struct plca_req_info),
266 .reply_data_size = sizeof(struct plca_reply_data),
267
268 .prepare_data = plca_get_status_prepare_data,
269 .reply_size = plca_get_status_reply_size,
270 .fill_reply = plca_get_status_fill_reply,
271};