Commit | Line | Data |
---|---|---|
7f59fb32 MK |
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 | }; | |
9 | ||
10 | struct pause_reply_data { | |
11 | struct ethnl_reply_data base; | |
12 | struct ethtool_pauseparam pauseparam; | |
9a27a330 | 13 | struct ethtool_pause_stats pausestat; |
7f59fb32 MK |
14 | }; |
15 | ||
16 | #define PAUSE_REPDATA(__reply_base) \ | |
17 | container_of(__reply_base, struct pause_reply_data, base) | |
18 | ||
ff419afa | 19 | const struct nla_policy ethnl_pause_get_policy[] = { |
329d9c33 | 20 | [ETHTOOL_A_PAUSE_HEADER] = |
a0de1cd3 | 21 | NLA_POLICY_NESTED(ethnl_header_policy_stats), |
7f59fb32 MK |
22 | }; |
23 | ||
24 | static int pause_prepare_data(const struct ethnl_req_info *req_base, | |
25 | struct ethnl_reply_data *reply_base, | |
26 | struct genl_info *info) | |
27 | { | |
28 | struct pause_reply_data *data = PAUSE_REPDATA(reply_base); | |
29 | struct net_device *dev = reply_base->dev; | |
30 | int ret; | |
31 | ||
32 | if (!dev->ethtool_ops->get_pauseparam) | |
33 | return -EOPNOTSUPP; | |
9a27a330 | 34 | |
16756d3e JK |
35 | ethtool_stats_init((u64 *)&data->pausestat, |
36 | sizeof(data->pausestat) / 8); | |
37 | ||
7f59fb32 MK |
38 | ret = ethnl_ops_begin(dev); |
39 | if (ret < 0) | |
40 | return ret; | |
41 | dev->ethtool_ops->get_pauseparam(dev, &data->pauseparam); | |
9a27a330 | 42 | if (req_base->flags & ETHTOOL_FLAG_STATS && |
16756d3e | 43 | dev->ethtool_ops->get_pause_stats) |
9a27a330 | 44 | dev->ethtool_ops->get_pause_stats(dev, &data->pausestat); |
7f59fb32 MK |
45 | ethnl_ops_complete(dev); |
46 | ||
47 | return 0; | |
48 | } | |
49 | ||
50 | static int pause_reply_size(const struct ethnl_req_info *req_base, | |
51 | const struct ethnl_reply_data *reply_base) | |
52 | { | |
9a27a330 | 53 | int n = nla_total_size(sizeof(u8)) + /* _PAUSE_AUTONEG */ |
7f59fb32 MK |
54 | nla_total_size(sizeof(u8)) + /* _PAUSE_RX */ |
55 | nla_total_size(sizeof(u8)); /* _PAUSE_TX */ | |
9a27a330 JK |
56 | |
57 | if (req_base->flags & ETHTOOL_FLAG_STATS) | |
58 | n += nla_total_size(0) + /* _PAUSE_STATS */ | |
1aabe578 | 59 | nla_total_size_64bit(sizeof(u64)) * ETHTOOL_PAUSE_STAT_CNT; |
9a27a330 JK |
60 | return n; |
61 | } | |
62 | ||
63 | static int ethtool_put_stat(struct sk_buff *skb, u64 val, u16 attrtype, | |
64 | u16 padtype) | |
65 | { | |
66 | if (val == ETHTOOL_STAT_NOT_SET) | |
67 | return 0; | |
68 | if (nla_put_u64_64bit(skb, attrtype, val, padtype)) | |
69 | return -EMSGSIZE; | |
70 | ||
71 | return 0; | |
72 | } | |
73 | ||
74 | static int pause_put_stats(struct sk_buff *skb, | |
75 | const struct ethtool_pause_stats *pause_stats) | |
76 | { | |
77 | const u16 pad = ETHTOOL_A_PAUSE_STAT_PAD; | |
78 | struct nlattr *nest; | |
79 | ||
80 | nest = nla_nest_start(skb, ETHTOOL_A_PAUSE_STATS); | |
81 | if (!nest) | |
82 | return -EMSGSIZE; | |
83 | ||
84 | if (ethtool_put_stat(skb, pause_stats->tx_pause_frames, | |
85 | ETHTOOL_A_PAUSE_STAT_TX_FRAMES, pad) || | |
86 | ethtool_put_stat(skb, pause_stats->rx_pause_frames, | |
87 | ETHTOOL_A_PAUSE_STAT_RX_FRAMES, pad)) | |
88 | goto err_cancel; | |
89 | ||
90 | nla_nest_end(skb, nest); | |
91 | return 0; | |
92 | ||
93 | err_cancel: | |
94 | nla_nest_cancel(skb, nest); | |
95 | return -EMSGSIZE; | |
7f59fb32 MK |
96 | } |
97 | ||
98 | static int pause_fill_reply(struct sk_buff *skb, | |
99 | const struct ethnl_req_info *req_base, | |
100 | const struct ethnl_reply_data *reply_base) | |
101 | { | |
102 | const struct pause_reply_data *data = PAUSE_REPDATA(reply_base); | |
103 | const struct ethtool_pauseparam *pauseparam = &data->pauseparam; | |
104 | ||
105 | if (nla_put_u8(skb, ETHTOOL_A_PAUSE_AUTONEG, !!pauseparam->autoneg) || | |
106 | nla_put_u8(skb, ETHTOOL_A_PAUSE_RX, !!pauseparam->rx_pause) || | |
107 | nla_put_u8(skb, ETHTOOL_A_PAUSE_TX, !!pauseparam->tx_pause)) | |
108 | return -EMSGSIZE; | |
109 | ||
9a27a330 JK |
110 | if (req_base->flags & ETHTOOL_FLAG_STATS && |
111 | pause_put_stats(skb, &data->pausestat)) | |
112 | return -EMSGSIZE; | |
113 | ||
7f59fb32 MK |
114 | return 0; |
115 | } | |
116 | ||
117 | const struct ethnl_request_ops ethnl_pause_request_ops = { | |
118 | .request_cmd = ETHTOOL_MSG_PAUSE_GET, | |
119 | .reply_cmd = ETHTOOL_MSG_PAUSE_GET_REPLY, | |
120 | .hdr_attr = ETHTOOL_A_PAUSE_HEADER, | |
7f59fb32 MK |
121 | .req_info_size = sizeof(struct pause_req_info), |
122 | .reply_data_size = sizeof(struct pause_reply_data), | |
7f59fb32 MK |
123 | |
124 | .prepare_data = pause_prepare_data, | |
125 | .reply_size = pause_reply_size, | |
126 | .fill_reply = pause_fill_reply, | |
127 | }; | |
3ab87993 MK |
128 | |
129 | /* PAUSE_SET */ | |
130 | ||
ff419afa | 131 | const struct nla_policy ethnl_pause_set_policy[] = { |
329d9c33 JK |
132 | [ETHTOOL_A_PAUSE_HEADER] = |
133 | NLA_POLICY_NESTED(ethnl_header_policy), | |
3ab87993 MK |
134 | [ETHTOOL_A_PAUSE_AUTONEG] = { .type = NLA_U8 }, |
135 | [ETHTOOL_A_PAUSE_RX] = { .type = NLA_U8 }, | |
136 | [ETHTOOL_A_PAUSE_TX] = { .type = NLA_U8 }, | |
137 | }; | |
138 | ||
139 | int ethnl_set_pause(struct sk_buff *skb, struct genl_info *info) | |
140 | { | |
3ab87993 MK |
141 | struct ethtool_pauseparam params = {}; |
142 | struct ethnl_req_info req_info = {}; | |
5028588b | 143 | struct nlattr **tb = info->attrs; |
3ab87993 MK |
144 | const struct ethtool_ops *ops; |
145 | struct net_device *dev; | |
146 | bool mod = false; | |
147 | int ret; | |
148 | ||
3ab87993 MK |
149 | ret = ethnl_parse_header_dev_get(&req_info, |
150 | tb[ETHTOOL_A_PAUSE_HEADER], | |
151 | genl_info_net(info), info->extack, | |
152 | true); | |
153 | if (ret < 0) | |
154 | return ret; | |
155 | dev = req_info.dev; | |
156 | ops = dev->ethtool_ops; | |
157 | ret = -EOPNOTSUPP; | |
158 | if (!ops->get_pauseparam || !ops->set_pauseparam) | |
159 | goto out_dev; | |
160 | ||
161 | rtnl_lock(); | |
162 | ret = ethnl_ops_begin(dev); | |
163 | if (ret < 0) | |
164 | goto out_rtnl; | |
165 | ops->get_pauseparam(dev, ¶ms); | |
166 | ||
167 | ethnl_update_bool32(¶ms.autoneg, tb[ETHTOOL_A_PAUSE_AUTONEG], &mod); | |
168 | ethnl_update_bool32(¶ms.rx_pause, tb[ETHTOOL_A_PAUSE_RX], &mod); | |
169 | ethnl_update_bool32(¶ms.tx_pause, tb[ETHTOOL_A_PAUSE_TX], &mod); | |
170 | ret = 0; | |
171 | if (!mod) | |
172 | goto out_ops; | |
173 | ||
174 | ret = dev->ethtool_ops->set_pauseparam(dev, ¶ms); | |
bf37faa3 MK |
175 | if (ret < 0) |
176 | goto out_ops; | |
177 | ethtool_notify(dev, ETHTOOL_MSG_PAUSE_NTF, NULL); | |
3ab87993 MK |
178 | |
179 | out_ops: | |
180 | ethnl_ops_complete(dev); | |
181 | out_rtnl: | |
182 | rtnl_unlock(); | |
183 | out_dev: | |
34ac17ec | 184 | ethnl_parse_header_dev_put(&req_info); |
3ab87993 MK |
185 | return ret; |
186 | } |