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