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(&rule->list, 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(&rule->list);
 77	adapter->rxnfc_count--;
 78
 79	kfree(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(&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, 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
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(&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
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(&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(&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
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(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
263failed:
264	mutex_unlock(&adapter->rxnfc_lock);
265	kfree(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, 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
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}