1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright (C) 2022 Gerhard Engleder <gerhard@engleder-embedded.com> */ |
3 | |
4 | #include "tsnep.h" |
5 | |
6 | #define ETHER_TYPE_FULL_MASK ((__force __be16)~0) |
7 | |
8 | static void tsnep_enable_rule(struct tsnep_adapter *adapter, |
9 | struct tsnep_rxnfc_rule *rule) |
10 | { |
11 | u8 rx_assign; |
12 | void __iomem *addr; |
13 | |
14 | rx_assign = TSNEP_RX_ASSIGN_ACTIVE; |
15 | rx_assign |= (rule->queue_index << TSNEP_RX_ASSIGN_QUEUE_SHIFT) & |
16 | TSNEP_RX_ASSIGN_QUEUE_MASK; |
17 | |
18 | addr = adapter->addr + TSNEP_RX_ASSIGN_ETHER_TYPE + |
19 | TSNEP_RX_ASSIGN_ETHER_TYPE_OFFSET * rule->location; |
20 | iowrite16(rule->filter.ether_type, addr); |
21 | |
22 | /* enable rule after all settings are done */ |
23 | addr = adapter->addr + TSNEP_RX_ASSIGN + |
24 | TSNEP_RX_ASSIGN_OFFSET * rule->location; |
25 | iowrite8(rx_assign, addr); |
26 | } |
27 | |
28 | static void tsnep_disable_rule(struct tsnep_adapter *adapter, |
29 | struct tsnep_rxnfc_rule *rule) |
30 | { |
31 | void __iomem *addr; |
32 | |
33 | addr = adapter->addr + TSNEP_RX_ASSIGN + |
34 | TSNEP_RX_ASSIGN_OFFSET * rule->location; |
35 | iowrite8(0, addr); |
36 | } |
37 | |
38 | static struct tsnep_rxnfc_rule *tsnep_get_rule(struct tsnep_adapter *adapter, |
39 | int location) |
40 | { |
41 | struct tsnep_rxnfc_rule *rule; |
42 | |
43 | list_for_each_entry(rule, &adapter->rxnfc_rules, list) { |
44 | if (rule->location == location) |
45 | return rule; |
46 | if (rule->location > location) |
47 | break; |
48 | } |
49 | |
50 | return NULL; |
51 | } |
52 | |
53 | static void tsnep_add_rule(struct tsnep_adapter *adapter, |
54 | struct tsnep_rxnfc_rule *rule) |
55 | { |
56 | struct tsnep_rxnfc_rule *pred, *cur; |
57 | |
58 | tsnep_enable_rule(adapter, rule); |
59 | |
60 | pred = NULL; |
61 | list_for_each_entry(cur, &adapter->rxnfc_rules, list) { |
62 | if (cur->location >= rule->location) |
63 | break; |
64 | pred = cur; |
65 | } |
66 | |
67 | list_add(new: &rule->list, head: pred ? &pred->list : &adapter->rxnfc_rules); |
68 | adapter->rxnfc_count++; |
69 | } |
70 | |
71 | static void tsnep_delete_rule(struct tsnep_adapter *adapter, |
72 | struct tsnep_rxnfc_rule *rule) |
73 | { |
74 | tsnep_disable_rule(adapter, rule); |
75 | |
76 | list_del(entry: &rule->list); |
77 | adapter->rxnfc_count--; |
78 | |
79 | kfree(objp: rule); |
80 | } |
81 | |
82 | static void tsnep_flush_rules(struct tsnep_adapter *adapter) |
83 | { |
84 | struct tsnep_rxnfc_rule *rule, *tmp; |
85 | |
86 | mutex_lock(&adapter->rxnfc_lock); |
87 | |
88 | list_for_each_entry_safe(rule, tmp, &adapter->rxnfc_rules, list) |
89 | tsnep_delete_rule(adapter, rule); |
90 | |
91 | mutex_unlock(lock: &adapter->rxnfc_lock); |
92 | } |
93 | |
94 | int tsnep_rxnfc_get_rule(struct tsnep_adapter *adapter, |
95 | struct ethtool_rxnfc *cmd) |
96 | { |
97 | struct ethtool_rx_flow_spec *fsp = &cmd->fs; |
98 | struct tsnep_rxnfc_rule *rule = NULL; |
99 | |
100 | cmd->data = adapter->rxnfc_max; |
101 | |
102 | mutex_lock(&adapter->rxnfc_lock); |
103 | |
104 | rule = tsnep_get_rule(adapter, location: fsp->location); |
105 | if (!rule) { |
106 | mutex_unlock(lock: &adapter->rxnfc_lock); |
107 | |
108 | return -ENOENT; |
109 | } |
110 | |
111 | fsp->flow_type = ETHER_FLOW; |
112 | fsp->ring_cookie = rule->queue_index; |
113 | |
114 | if (rule->filter.type == TSNEP_RXNFC_ETHER_TYPE) { |
115 | fsp->h_u.ether_spec.h_proto = htons(rule->filter.ether_type); |
116 | fsp->m_u.ether_spec.h_proto = ETHER_TYPE_FULL_MASK; |
117 | } |
118 | |
119 | mutex_unlock(lock: &adapter->rxnfc_lock); |
120 | |
121 | return 0; |
122 | } |
123 | |
124 | int tsnep_rxnfc_get_all(struct tsnep_adapter *adapter, |
125 | struct ethtool_rxnfc *cmd, |
126 | u32 *rule_locs) |
127 | { |
128 | struct tsnep_rxnfc_rule *rule; |
129 | int count = 0; |
130 | |
131 | cmd->data = adapter->rxnfc_max; |
132 | |
133 | mutex_lock(&adapter->rxnfc_lock); |
134 | |
135 | list_for_each_entry(rule, &adapter->rxnfc_rules, list) { |
136 | if (count == cmd->rule_cnt) { |
137 | mutex_unlock(lock: &adapter->rxnfc_lock); |
138 | |
139 | return -EMSGSIZE; |
140 | } |
141 | |
142 | rule_locs[count] = rule->location; |
143 | count++; |
144 | } |
145 | |
146 | mutex_unlock(lock: &adapter->rxnfc_lock); |
147 | |
148 | cmd->rule_cnt = count; |
149 | |
150 | return 0; |
151 | } |
152 | |
153 | static int tsnep_rxnfc_find_location(struct tsnep_adapter *adapter) |
154 | { |
155 | struct tsnep_rxnfc_rule *tmp; |
156 | int location = 0; |
157 | |
158 | list_for_each_entry(tmp, &adapter->rxnfc_rules, list) { |
159 | if (tmp->location == location) |
160 | location++; |
161 | else |
162 | return location; |
163 | } |
164 | |
165 | if (location >= adapter->rxnfc_max) |
166 | return -ENOSPC; |
167 | |
168 | return location; |
169 | } |
170 | |
171 | static void tsnep_rxnfc_init_rule(struct tsnep_rxnfc_rule *rule, |
172 | const struct ethtool_rx_flow_spec *fsp) |
173 | { |
174 | INIT_LIST_HEAD(list: &rule->list); |
175 | |
176 | rule->queue_index = fsp->ring_cookie; |
177 | rule->location = fsp->location; |
178 | |
179 | rule->filter.type = TSNEP_RXNFC_ETHER_TYPE; |
180 | rule->filter.ether_type = ntohs(fsp->h_u.ether_spec.h_proto); |
181 | } |
182 | |
183 | static int tsnep_rxnfc_check_rule(struct tsnep_adapter *adapter, |
184 | struct tsnep_rxnfc_rule *rule) |
185 | { |
186 | struct net_device *dev = adapter->netdev; |
187 | struct tsnep_rxnfc_rule *tmp; |
188 | |
189 | list_for_each_entry(tmp, &adapter->rxnfc_rules, list) { |
190 | if (!memcmp(p: &rule->filter, q: &tmp->filter, size: sizeof(rule->filter)) && |
191 | tmp->location != rule->location) { |
192 | netdev_dbg(dev, "rule already exists\n" ); |
193 | |
194 | return -EEXIST; |
195 | } |
196 | } |
197 | |
198 | return 0; |
199 | } |
200 | |
201 | int tsnep_rxnfc_add_rule(struct tsnep_adapter *adapter, |
202 | struct ethtool_rxnfc *cmd) |
203 | { |
204 | struct net_device *netdev = adapter->netdev; |
205 | struct ethtool_rx_flow_spec *fsp = |
206 | (struct ethtool_rx_flow_spec *)&cmd->fs; |
207 | struct tsnep_rxnfc_rule *rule, *old_rule; |
208 | int retval; |
209 | |
210 | /* only EtherType is supported */ |
211 | if (fsp->flow_type != ETHER_FLOW || |
212 | !is_zero_ether_addr(addr: fsp->m_u.ether_spec.h_dest) || |
213 | !is_zero_ether_addr(addr: fsp->m_u.ether_spec.h_source) || |
214 | fsp->m_u.ether_spec.h_proto != ETHER_TYPE_FULL_MASK) { |
215 | netdev_dbg(netdev, "only ethernet protocol is supported\n" ); |
216 | |
217 | return -EOPNOTSUPP; |
218 | } |
219 | |
220 | if (fsp->ring_cookie > |
221 | (TSNEP_RX_ASSIGN_QUEUE_MASK >> TSNEP_RX_ASSIGN_QUEUE_SHIFT)) { |
222 | netdev_dbg(netdev, "invalid action\n" ); |
223 | |
224 | return -EINVAL; |
225 | } |
226 | |
227 | if (fsp->location != RX_CLS_LOC_ANY && |
228 | fsp->location >= adapter->rxnfc_max) { |
229 | netdev_dbg(netdev, "invalid location\n" ); |
230 | |
231 | return -EINVAL; |
232 | } |
233 | |
234 | rule = kzalloc(size: sizeof(*rule), GFP_KERNEL); |
235 | if (!rule) |
236 | return -ENOMEM; |
237 | |
238 | mutex_lock(&adapter->rxnfc_lock); |
239 | |
240 | if (fsp->location == RX_CLS_LOC_ANY) { |
241 | retval = tsnep_rxnfc_find_location(adapter); |
242 | if (retval < 0) |
243 | goto failed; |
244 | fsp->location = retval; |
245 | } |
246 | |
247 | tsnep_rxnfc_init_rule(rule, fsp); |
248 | |
249 | retval = tsnep_rxnfc_check_rule(adapter, rule); |
250 | if (retval) |
251 | goto failed; |
252 | |
253 | old_rule = tsnep_get_rule(adapter, location: fsp->location); |
254 | if (old_rule) |
255 | tsnep_delete_rule(adapter, rule: old_rule); |
256 | |
257 | tsnep_add_rule(adapter, rule); |
258 | |
259 | mutex_unlock(lock: &adapter->rxnfc_lock); |
260 | |
261 | return 0; |
262 | |
263 | failed: |
264 | mutex_unlock(lock: &adapter->rxnfc_lock); |
265 | kfree(objp: rule); |
266 | return retval; |
267 | } |
268 | |
269 | int tsnep_rxnfc_del_rule(struct tsnep_adapter *adapter, |
270 | struct ethtool_rxnfc *cmd) |
271 | { |
272 | struct ethtool_rx_flow_spec *fsp = |
273 | (struct ethtool_rx_flow_spec *)&cmd->fs; |
274 | struct tsnep_rxnfc_rule *rule; |
275 | |
276 | mutex_lock(&adapter->rxnfc_lock); |
277 | |
278 | rule = tsnep_get_rule(adapter, location: fsp->location); |
279 | if (!rule) { |
280 | mutex_unlock(lock: &adapter->rxnfc_lock); |
281 | |
282 | return -ENOENT; |
283 | } |
284 | |
285 | tsnep_delete_rule(adapter, rule); |
286 | |
287 | mutex_unlock(lock: &adapter->rxnfc_lock); |
288 | |
289 | return 0; |
290 | } |
291 | |
292 | int tsnep_rxnfc_init(struct tsnep_adapter *adapter) |
293 | { |
294 | int i; |
295 | |
296 | /* disable all rules */ |
297 | for (i = 0; i < adapter->rxnfc_max; |
298 | i += sizeof(u32) / TSNEP_RX_ASSIGN_OFFSET) |
299 | iowrite32(0, adapter->addr + TSNEP_RX_ASSIGN + i); |
300 | |
301 | return 0; |
302 | } |
303 | |
304 | void tsnep_rxnfc_cleanup(struct tsnep_adapter *adapter) |
305 | { |
306 | tsnep_flush_rules(adapter); |
307 | } |
308 | |