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