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