1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | |
3 | #include "netlink.h" |
4 | #include "common.h" |
5 | |
6 | struct pause_req_info { |
7 | struct ethnl_req_info base; |
8 | enum ethtool_mac_stats_src src; |
9 | }; |
10 | |
11 | #define PAUSE_REQINFO(__req_base) \ |
12 | container_of(__req_base, struct pause_req_info, base) |
13 | |
14 | struct pause_reply_data { |
15 | struct ethnl_reply_data base; |
16 | struct ethtool_pauseparam pauseparam; |
17 | struct ethtool_pause_stats pausestat; |
18 | }; |
19 | |
20 | #define PAUSE_REPDATA(__reply_base) \ |
21 | container_of(__reply_base, struct pause_reply_data, base) |
22 | |
23 | const struct nla_policy ethnl_pause_get_policy[] = { |
24 | [ETHTOOL_A_PAUSE_HEADER] = |
25 | NLA_POLICY_NESTED(ethnl_header_policy_stats), |
26 | [ETHTOOL_A_PAUSE_STATS_SRC] = |
27 | NLA_POLICY_MAX(NLA_U32, ETHTOOL_MAC_STATS_SRC_PMAC), |
28 | }; |
29 | |
30 | static int pause_parse_request(struct ethnl_req_info *req_base, |
31 | struct nlattr **tb, |
32 | struct netlink_ext_ack *extack) |
33 | { |
34 | enum ethtool_mac_stats_src src = ETHTOOL_MAC_STATS_SRC_AGGREGATE; |
35 | struct pause_req_info *req_info = PAUSE_REQINFO(req_base); |
36 | |
37 | if (tb[ETHTOOL_A_PAUSE_STATS_SRC]) { |
38 | if (!(req_base->flags & ETHTOOL_FLAG_STATS)) { |
39 | NL_SET_ERR_MSG_MOD(extack, |
40 | "ETHTOOL_FLAG_STATS must be set when requesting a source of stats" ); |
41 | return -EINVAL; |
42 | } |
43 | |
44 | src = nla_get_u32(nla: tb[ETHTOOL_A_PAUSE_STATS_SRC]); |
45 | } |
46 | |
47 | req_info->src = src; |
48 | |
49 | return 0; |
50 | } |
51 | |
52 | static int pause_prepare_data(const struct ethnl_req_info *req_base, |
53 | struct ethnl_reply_data *reply_base, |
54 | const struct genl_info *info) |
55 | { |
56 | const struct pause_req_info *req_info = PAUSE_REQINFO(req_base); |
57 | struct pause_reply_data *data = PAUSE_REPDATA(reply_base); |
58 | enum ethtool_mac_stats_src src = req_info->src; |
59 | struct net_device *dev = reply_base->dev; |
60 | int ret; |
61 | |
62 | if (!dev->ethtool_ops->get_pauseparam) |
63 | return -EOPNOTSUPP; |
64 | |
65 | ethtool_stats_init(stats: (u64 *)&data->pausestat, |
66 | n: sizeof(data->pausestat) / 8); |
67 | data->pausestat.src = src; |
68 | |
69 | ret = ethnl_ops_begin(dev); |
70 | if (ret < 0) |
71 | return ret; |
72 | |
73 | if ((src == ETHTOOL_MAC_STATS_SRC_EMAC || |
74 | src == ETHTOOL_MAC_STATS_SRC_PMAC) && |
75 | !__ethtool_dev_mm_supported(dev)) { |
76 | NL_SET_ERR_MSG_MOD(info->extack, |
77 | "Device does not support MAC merge layer" ); |
78 | ethnl_ops_complete(dev); |
79 | return -EOPNOTSUPP; |
80 | } |
81 | |
82 | dev->ethtool_ops->get_pauseparam(dev, &data->pauseparam); |
83 | if (req_base->flags & ETHTOOL_FLAG_STATS && |
84 | dev->ethtool_ops->get_pause_stats) |
85 | dev->ethtool_ops->get_pause_stats(dev, &data->pausestat); |
86 | |
87 | ethnl_ops_complete(dev); |
88 | |
89 | return 0; |
90 | } |
91 | |
92 | static int pause_reply_size(const struct ethnl_req_info *req_base, |
93 | const struct ethnl_reply_data *reply_base) |
94 | { |
95 | int n = nla_total_size(payload: sizeof(u8)) + /* _PAUSE_AUTONEG */ |
96 | nla_total_size(payload: sizeof(u8)) + /* _PAUSE_RX */ |
97 | nla_total_size(payload: sizeof(u8)); /* _PAUSE_TX */ |
98 | |
99 | if (req_base->flags & ETHTOOL_FLAG_STATS) |
100 | n += nla_total_size(payload: 0) + /* _PAUSE_STATS */ |
101 | nla_total_size(payload: sizeof(u32)) + /* _PAUSE_STATS_SRC */ |
102 | nla_total_size_64bit(payload: sizeof(u64)) * ETHTOOL_PAUSE_STAT_CNT; |
103 | return n; |
104 | } |
105 | |
106 | static int ethtool_put_stat(struct sk_buff *skb, u64 val, u16 attrtype, |
107 | u16 padtype) |
108 | { |
109 | if (val == ETHTOOL_STAT_NOT_SET) |
110 | return 0; |
111 | if (nla_put_u64_64bit(skb, attrtype, value: val, padattr: padtype)) |
112 | return -EMSGSIZE; |
113 | |
114 | return 0; |
115 | } |
116 | |
117 | static int pause_put_stats(struct sk_buff *skb, |
118 | const struct ethtool_pause_stats *pause_stats) |
119 | { |
120 | const u16 pad = ETHTOOL_A_PAUSE_STAT_PAD; |
121 | struct nlattr *nest; |
122 | |
123 | if (nla_put_u32(skb, attrtype: ETHTOOL_A_PAUSE_STATS_SRC, value: pause_stats->src)) |
124 | return -EMSGSIZE; |
125 | |
126 | nest = nla_nest_start(skb, attrtype: ETHTOOL_A_PAUSE_STATS); |
127 | if (!nest) |
128 | return -EMSGSIZE; |
129 | |
130 | if (ethtool_put_stat(skb, val: pause_stats->tx_pause_frames, |
131 | attrtype: ETHTOOL_A_PAUSE_STAT_TX_FRAMES, padtype: pad) || |
132 | ethtool_put_stat(skb, val: pause_stats->rx_pause_frames, |
133 | attrtype: ETHTOOL_A_PAUSE_STAT_RX_FRAMES, padtype: pad)) |
134 | goto err_cancel; |
135 | |
136 | nla_nest_end(skb, start: nest); |
137 | return 0; |
138 | |
139 | err_cancel: |
140 | nla_nest_cancel(skb, start: nest); |
141 | return -EMSGSIZE; |
142 | } |
143 | |
144 | static int pause_fill_reply(struct sk_buff *skb, |
145 | const struct ethnl_req_info *req_base, |
146 | const struct ethnl_reply_data *reply_base) |
147 | { |
148 | const struct pause_reply_data *data = PAUSE_REPDATA(reply_base); |
149 | const struct ethtool_pauseparam *pauseparam = &data->pauseparam; |
150 | |
151 | if (nla_put_u8(skb, attrtype: ETHTOOL_A_PAUSE_AUTONEG, value: !!pauseparam->autoneg) || |
152 | nla_put_u8(skb, attrtype: ETHTOOL_A_PAUSE_RX, value: !!pauseparam->rx_pause) || |
153 | nla_put_u8(skb, attrtype: ETHTOOL_A_PAUSE_TX, value: !!pauseparam->tx_pause)) |
154 | return -EMSGSIZE; |
155 | |
156 | if (req_base->flags & ETHTOOL_FLAG_STATS && |
157 | pause_put_stats(skb, pause_stats: &data->pausestat)) |
158 | return -EMSGSIZE; |
159 | |
160 | return 0; |
161 | } |
162 | |
163 | /* PAUSE_SET */ |
164 | |
165 | const struct nla_policy ethnl_pause_set_policy[] = { |
166 | [ETHTOOL_A_PAUSE_HEADER] = |
167 | NLA_POLICY_NESTED(ethnl_header_policy), |
168 | [ETHTOOL_A_PAUSE_AUTONEG] = { .type = NLA_U8 }, |
169 | [ETHTOOL_A_PAUSE_RX] = { .type = NLA_U8 }, |
170 | [ETHTOOL_A_PAUSE_TX] = { .type = NLA_U8 }, |
171 | }; |
172 | |
173 | static int |
174 | ethnl_set_pause_validate(struct ethnl_req_info *req_info, |
175 | struct genl_info *info) |
176 | { |
177 | const struct ethtool_ops *ops = req_info->dev->ethtool_ops; |
178 | |
179 | return ops->get_pauseparam && ops->set_pauseparam ? 1 : -EOPNOTSUPP; |
180 | } |
181 | |
182 | static int |
183 | ethnl_set_pause(struct ethnl_req_info *req_info, struct genl_info *info) |
184 | { |
185 | struct net_device *dev = req_info->dev; |
186 | struct ethtool_pauseparam params = {}; |
187 | struct nlattr **tb = info->attrs; |
188 | bool mod = false; |
189 | int ret; |
190 | |
191 | dev->ethtool_ops->get_pauseparam(dev, ¶ms); |
192 | |
193 | ethnl_update_bool32(dst: ¶ms.autoneg, attr: tb[ETHTOOL_A_PAUSE_AUTONEG], mod: &mod); |
194 | ethnl_update_bool32(dst: ¶ms.rx_pause, attr: tb[ETHTOOL_A_PAUSE_RX], mod: &mod); |
195 | ethnl_update_bool32(dst: ¶ms.tx_pause, attr: tb[ETHTOOL_A_PAUSE_TX], mod: &mod); |
196 | if (!mod) |
197 | return 0; |
198 | |
199 | ret = dev->ethtool_ops->set_pauseparam(dev, ¶ms); |
200 | return ret < 0 ? ret : 1; |
201 | } |
202 | |
203 | const struct ethnl_request_ops ethnl_pause_request_ops = { |
204 | .request_cmd = ETHTOOL_MSG_PAUSE_GET, |
205 | .reply_cmd = ETHTOOL_MSG_PAUSE_GET_REPLY, |
206 | .hdr_attr = ETHTOOL_A_PAUSE_HEADER, |
207 | .req_info_size = sizeof(struct pause_req_info), |
208 | .reply_data_size = sizeof(struct pause_reply_data), |
209 | |
210 | .parse_request = pause_parse_request, |
211 | .prepare_data = pause_prepare_data, |
212 | .reply_size = pause_reply_size, |
213 | .fill_reply = pause_fill_reply, |
214 | |
215 | .set_validate = ethnl_set_pause_validate, |
216 | .set = ethnl_set_pause, |
217 | .set_ntf_cmd = ETHTOOL_MSG_PAUSE_NTF, |
218 | }; |
219 | |