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