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
8static 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
28static 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
38static 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
53static 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
71static 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
82static 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
94int 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
124int 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
153static 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
171static 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
183static 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
201int 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
263failed:
264 mutex_unlock(lock: &adapter->rxnfc_lock);
265 kfree(objp: rule);
266 return retval;
267}
268
269int 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
292int 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
304void tsnep_rxnfc_cleanup(struct tsnep_adapter *adapter)
305{
306 tsnep_flush_rules(adapter);
307}
308

source code of linux/drivers/net/ethernet/engleder/tsnep_rxnfc.c