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
tsnep_enable_rule(struct tsnep_adapter * adapter,struct tsnep_rxnfc_rule * rule)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
tsnep_disable_rule(struct tsnep_adapter * adapter,struct tsnep_rxnfc_rule * rule)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
tsnep_get_rule(struct tsnep_adapter * adapter,int location)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
tsnep_add_rule(struct tsnep_adapter * adapter,struct tsnep_rxnfc_rule * rule)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(&rule->list, pred ? &pred->list : &adapter->rxnfc_rules);
68 adapter->rxnfc_count++;
69 }
70
tsnep_delete_rule(struct tsnep_adapter * adapter,struct tsnep_rxnfc_rule * rule)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(&rule->list);
77 adapter->rxnfc_count--;
78
79 kfree(rule);
80 }
81
tsnep_flush_rules(struct tsnep_adapter * adapter)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(&adapter->rxnfc_lock);
92 }
93
tsnep_rxnfc_get_rule(struct tsnep_adapter * adapter,struct ethtool_rxnfc * cmd)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, fsp->location);
105 if (!rule) {
106 mutex_unlock(&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(&adapter->rxnfc_lock);
120
121 return 0;
122 }
123
tsnep_rxnfc_get_all(struct tsnep_adapter * adapter,struct ethtool_rxnfc * cmd,u32 * rule_locs)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(&adapter->rxnfc_lock);
138
139 return -EMSGSIZE;
140 }
141
142 rule_locs[count] = rule->location;
143 count++;
144 }
145
146 mutex_unlock(&adapter->rxnfc_lock);
147
148 cmd->rule_cnt = count;
149
150 return 0;
151 }
152
tsnep_rxnfc_find_location(struct tsnep_adapter * adapter)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
tsnep_rxnfc_init_rule(struct tsnep_rxnfc_rule * rule,const struct ethtool_rx_flow_spec * fsp)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(&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
tsnep_rxnfc_check_rule(struct tsnep_adapter * adapter,struct tsnep_rxnfc_rule * rule)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(&rule->filter, &tmp->filter, 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
tsnep_rxnfc_add_rule(struct tsnep_adapter * adapter,struct ethtool_rxnfc * cmd)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(fsp->m_u.ether_spec.h_dest) ||
213 !is_zero_ether_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(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, fsp->location);
254 if (old_rule)
255 tsnep_delete_rule(adapter, old_rule);
256
257 tsnep_add_rule(adapter, rule);
258
259 mutex_unlock(&adapter->rxnfc_lock);
260
261 return 0;
262
263 failed:
264 mutex_unlock(&adapter->rxnfc_lock);
265 kfree(rule);
266 return retval;
267 }
268
tsnep_rxnfc_del_rule(struct tsnep_adapter * adapter,struct ethtool_rxnfc * cmd)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, fsp->location);
279 if (!rule) {
280 mutex_unlock(&adapter->rxnfc_lock);
281
282 return -ENOENT;
283 }
284
285 tsnep_delete_rule(adapter, rule);
286
287 mutex_unlock(&adapter->rxnfc_lock);
288
289 return 0;
290 }
291
tsnep_rxnfc_init(struct tsnep_adapter * adapter)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
tsnep_rxnfc_cleanup(struct tsnep_adapter * adapter)304 void tsnep_rxnfc_cleanup(struct tsnep_adapter *adapter)
305 {
306 tsnep_flush_rules(adapter);
307 }
308