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 | |
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(nla: attr); |
43 | break; |
44 | case NLA_U32: |
45 | *dst = nla_get_u32(nla: attr); |
46 | break; |
47 | default: |
48 | WARN_ON_ONCE(1); |
49 | } |
50 | |
51 | *mod = true; |
52 | } |
53 | |
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 | |
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(payload: sizeof(u16)) + /* _VERSION */ |
94 | nla_total_size(payload: sizeof(u8)) + /* _ENABLED */ |
95 | nla_total_size(payload: sizeof(u32)) + /* _NODE_CNT */ |
96 | nla_total_size(payload: sizeof(u32)) + /* _NODE_ID */ |
97 | nla_total_size(payload: sizeof(u32)) + /* _TO_TIMER */ |
98 | nla_total_size(payload: sizeof(u32)) + /* _BURST_COUNT */ |
99 | nla_total_size(payload: sizeof(u32)); /* _BURST_TIMER */ |
100 | } |
101 | |
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, attrtype: ETHTOOL_A_PLCA_VERSION, value: plca->version)) || |
111 | (plca->enabled >= 0 && |
112 | nla_put_u8(skb, attrtype: ETHTOOL_A_PLCA_ENABLED, value: !!plca->enabled)) || |
113 | (plca->node_id >= 0 && |
114 | nla_put_u32(skb, attrtype: ETHTOOL_A_PLCA_NODE_ID, value: plca->node_id)) || |
115 | (plca->node_cnt >= 0 && |
116 | nla_put_u32(skb, attrtype: ETHTOOL_A_PLCA_NODE_CNT, value: plca->node_cnt)) || |
117 | (plca->to_tmr >= 0 && |
118 | nla_put_u32(skb, attrtype: ETHTOOL_A_PLCA_TO_TMR, value: plca->to_tmr)) || |
119 | (plca->burst_cnt >= 0 && |
120 | nla_put_u32(skb, attrtype: ETHTOOL_A_PLCA_BURST_CNT, value: plca->burst_cnt)) || |
121 | (plca->burst_tmr >= 0 && |
122 | nla_put_u32(skb, attrtype: ETHTOOL_A_PLCA_BURST_TMR, value: 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 |
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(dst: &plca_cfg.enabled, tb, attrid: ETHTOOL_A_PLCA_ENABLED, mod: &mod); |
161 | plca_update_sint(dst: &plca_cfg.node_id, tb, attrid: ETHTOOL_A_PLCA_NODE_ID, mod: &mod); |
162 | plca_update_sint(dst: &plca_cfg.node_cnt, tb, attrid: ETHTOOL_A_PLCA_NODE_CNT, mod: &mod); |
163 | plca_update_sint(dst: &plca_cfg.to_tmr, tb, attrid: ETHTOOL_A_PLCA_TO_TMR, mod: &mod); |
164 | plca_update_sint(dst: &plca_cfg.burst_cnt, tb, attrid: ETHTOOL_A_PLCA_BURST_CNT, |
165 | mod: &mod); |
166 | plca_update_sint(dst: &plca_cfg.burst_tmr, tb, attrid: ETHTOOL_A_PLCA_BURST_TMR, |
167 | mod: &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 | |
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 | |
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(payload: sizeof(u8)); /* _STATUS */ |
236 | } |
237 | |
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, attrtype: ETHTOOL_A_PLCA_STATUS, value: !!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 | |