Loading...
1// SPDX-License-Identifier: GPL-2.0-only
2/*
3 * Copyright (C) 2020 Felix Fietkau <nbd@nbd.name>
4 */
5
6#include <linux/if_ether.h>
7#include <linux/rhashtable.h>
8#include <linux/ip.h>
9#include <linux/ipv6.h>
10#include <net/flow_offload.h>
11#include <net/pkt_cls.h>
12#include <net/dsa.h>
13#include "mtk_eth_soc.h"
14#include "mtk_wed.h"
15
16struct mtk_flow_data {
17 struct ethhdr eth;
18
19 union {
20 struct {
21 __be32 src_addr;
22 __be32 dst_addr;
23 } v4;
24
25 struct {
26 struct in6_addr src_addr;
27 struct in6_addr dst_addr;
28 } v6;
29 };
30
31 __be16 src_port;
32 __be16 dst_port;
33
34 u16 vlan_in;
35
36 struct {
37 u16 id;
38 __be16 proto;
39 u8 num;
40 } vlan;
41 struct {
42 u16 sid;
43 u8 num;
44 } pppoe;
45};
46
47static const struct rhashtable_params mtk_flow_ht_params = {
48 .head_offset = offsetof(struct mtk_flow_entry, node),
49 .key_offset = offsetof(struct mtk_flow_entry, cookie),
50 .key_len = sizeof(unsigned long),
51 .automatic_shrinking = true,
52};
53
54static int
55mtk_flow_set_ipv4_addr(struct mtk_eth *eth, struct mtk_foe_entry *foe,
56 struct mtk_flow_data *data, bool egress)
57{
58 return mtk_foe_entry_set_ipv4_tuple(eth, foe, egress,
59 data->v4.src_addr, data->src_port,
60 data->v4.dst_addr, data->dst_port);
61}
62
63static int
64mtk_flow_set_ipv6_addr(struct mtk_eth *eth, struct mtk_foe_entry *foe,
65 struct mtk_flow_data *data)
66{
67 return mtk_foe_entry_set_ipv6_tuple(eth, foe,
68 data->v6.src_addr.s6_addr32, data->src_port,
69 data->v6.dst_addr.s6_addr32, data->dst_port);
70}
71
72static void
73mtk_flow_offload_mangle_eth(const struct flow_action_entry *act, void *eth)
74{
75 void *dest = eth + act->mangle.offset;
76 const void *src = &act->mangle.val;
77
78 if (act->mangle.offset > 8)
79 return;
80
81 if (act->mangle.mask == 0xffff) {
82 src += 2;
83 dest += 2;
84 }
85
86 memcpy(dest, src, act->mangle.mask ? 2 : 4);
87}
88
89static int
90mtk_flow_get_wdma_info(struct net_device *dev, const u8 *addr, struct mtk_wdma_info *info)
91{
92 struct net_device_path_stack stack;
93 struct net_device_path *path;
94 int err;
95
96 if (!dev)
97 return -ENODEV;
98
99 if (!IS_ENABLED(CONFIG_NET_MEDIATEK_SOC_WED))
100 return -1;
101
102 err = dev_fill_forward_path(dev, addr, &stack);
103 if (err)
104 return err;
105
106 path = &stack.path[stack.num_paths - 1];
107 if (path->type != DEV_PATH_MTK_WDMA)
108 return -1;
109
110 info->wdma_idx = path->mtk_wdma.wdma_idx;
111 info->queue = path->mtk_wdma.queue;
112 info->bss = path->mtk_wdma.bss;
113 info->wcid = path->mtk_wdma.wcid;
114 info->amsdu = path->mtk_wdma.amsdu;
115
116 return 0;
117}
118
119
120static int
121mtk_flow_mangle_ports(const struct flow_action_entry *act,
122 struct mtk_flow_data *data)
123{
124 u32 val = ntohl(act->mangle.val);
125
126 switch (act->mangle.offset) {
127 case 0:
128 if (act->mangle.mask == ~htonl(0xffff))
129 data->dst_port = cpu_to_be16(val);
130 else
131 data->src_port = cpu_to_be16(val >> 16);
132 break;
133 case 2:
134 data->dst_port = cpu_to_be16(val);
135 break;
136 default:
137 return -EINVAL;
138 }
139
140 return 0;
141}
142
143static int
144mtk_flow_mangle_ipv4(const struct flow_action_entry *act,
145 struct mtk_flow_data *data)
146{
147 __be32 *dest;
148
149 switch (act->mangle.offset) {
150 case offsetof(struct iphdr, saddr):
151 dest = &data->v4.src_addr;
152 break;
153 case offsetof(struct iphdr, daddr):
154 dest = &data->v4.dst_addr;
155 break;
156 default:
157 return -EINVAL;
158 }
159
160 memcpy(dest, &act->mangle.val, sizeof(u32));
161
162 return 0;
163}
164
165static int
166mtk_flow_get_dsa_port(struct net_device **dev)
167{
168#if IS_ENABLED(CONFIG_NET_DSA)
169 struct dsa_port *dp;
170
171 dp = dsa_port_from_netdev(*dev);
172 if (IS_ERR(dp))
173 return -ENODEV;
174
175 if (dp->cpu_dp->tag_ops->proto != DSA_TAG_PROTO_MTK)
176 return -ENODEV;
177
178 *dev = dsa_port_to_conduit(dp);
179
180 return dp->index;
181#else
182 return -ENODEV;
183#endif
184}
185
186static int
187mtk_flow_set_output_device(struct mtk_eth *eth, struct mtk_foe_entry *foe,
188 struct net_device *dev, const u8 *dest_mac,
189 int *wed_index)
190{
191 struct mtk_wdma_info info = {};
192 int pse_port, dsa_port, queue;
193
194 if (mtk_flow_get_wdma_info(dev, dest_mac, &info) == 0) {
195 mtk_foe_entry_set_wdma(eth, foe, info.wdma_idx, info.queue,
196 info.bss, info.wcid, info.amsdu);
197 if (mtk_is_netsys_v2_or_greater(eth)) {
198 switch (info.wdma_idx) {
199 case 0:
200 pse_port = PSE_WDMA0_PORT;
201 break;
202 case 1:
203 pse_port = PSE_WDMA1_PORT;
204 break;
205 case 2:
206 pse_port = PSE_WDMA2_PORT;
207 break;
208 default:
209 return -EINVAL;
210 }
211 } else {
212 pse_port = 3;
213 }
214 *wed_index = info.wdma_idx;
215 goto out;
216 }
217
218 dsa_port = mtk_flow_get_dsa_port(&dev);
219
220 if (dev == eth->netdev[0])
221 pse_port = PSE_GDM1_PORT;
222 else if (dev == eth->netdev[1])
223 pse_port = PSE_GDM2_PORT;
224 else if (dev == eth->netdev[2])
225 pse_port = PSE_GDM3_PORT;
226 else
227 return -EOPNOTSUPP;
228
229 if (dsa_port >= 0) {
230 mtk_foe_entry_set_dsa(eth, foe, dsa_port);
231 queue = 3 + dsa_port;
232 } else {
233 queue = pse_port - 1;
234 }
235 mtk_foe_entry_set_queue(eth, foe, queue);
236
237out:
238 mtk_foe_entry_set_pse_port(eth, foe, pse_port);
239
240 return 0;
241}
242
243static int
244mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f,
245 int ppe_index)
246{
247 struct flow_rule *rule = flow_cls_offload_flow_rule(f);
248 struct flow_action_entry *act;
249 struct mtk_flow_data data = {};
250 struct mtk_foe_entry foe;
251 struct net_device *odev = NULL;
252 struct mtk_flow_entry *entry;
253 int offload_type = 0;
254 int wed_index = -1;
255 u16 addr_type = 0;
256 u8 l4proto = 0;
257 int err = 0;
258 int i;
259
260 if (rhashtable_lookup(ð->flow_table, &f->cookie, mtk_flow_ht_params))
261 return -EEXIST;
262
263 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_META)) {
264 struct flow_match_meta match;
265
266 flow_rule_match_meta(rule, &match);
267 } else {
268 return -EOPNOTSUPP;
269 }
270
271 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_CONTROL)) {
272 struct flow_match_control match;
273
274 flow_rule_match_control(rule, &match);
275 addr_type = match.key->addr_type;
276 } else {
277 return -EOPNOTSUPP;
278 }
279
280 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) {
281 struct flow_match_basic match;
282
283 flow_rule_match_basic(rule, &match);
284 l4proto = match.key->ip_proto;
285 } else {
286 return -EOPNOTSUPP;
287 }
288
289 switch (addr_type) {
290 case 0:
291 offload_type = MTK_PPE_PKT_TYPE_BRIDGE;
292 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
293 struct flow_match_eth_addrs match;
294
295 flow_rule_match_eth_addrs(rule, &match);
296 memcpy(data.eth.h_dest, match.key->dst, ETH_ALEN);
297 memcpy(data.eth.h_source, match.key->src, ETH_ALEN);
298 } else {
299 return -EOPNOTSUPP;
300 }
301
302 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
303 struct flow_match_vlan match;
304
305 flow_rule_match_vlan(rule, &match);
306
307 if (match.key->vlan_tpid != cpu_to_be16(ETH_P_8021Q))
308 return -EOPNOTSUPP;
309
310 data.vlan_in = match.key->vlan_id;
311 }
312 break;
313 case FLOW_DISSECTOR_KEY_IPV4_ADDRS:
314 offload_type = MTK_PPE_PKT_TYPE_IPV4_HNAPT;
315 break;
316 case FLOW_DISSECTOR_KEY_IPV6_ADDRS:
317 offload_type = MTK_PPE_PKT_TYPE_IPV6_ROUTE_5T;
318 break;
319 default:
320 return -EOPNOTSUPP;
321 }
322
323 flow_action_for_each(i, act, &rule->action) {
324 switch (act->id) {
325 case FLOW_ACTION_MANGLE:
326 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
327 return -EOPNOTSUPP;
328 if (act->mangle.htype == FLOW_ACT_MANGLE_HDR_TYPE_ETH)
329 mtk_flow_offload_mangle_eth(act, &data.eth);
330 break;
331 case FLOW_ACTION_REDIRECT:
332 odev = act->dev;
333 break;
334 case FLOW_ACTION_CSUM:
335 break;
336 case FLOW_ACTION_VLAN_PUSH:
337 if (data.vlan.num == 1 ||
338 act->vlan.proto != htons(ETH_P_8021Q))
339 return -EOPNOTSUPP;
340
341 data.vlan.id = act->vlan.vid;
342 data.vlan.proto = act->vlan.proto;
343 data.vlan.num++;
344 break;
345 case FLOW_ACTION_VLAN_POP:
346 break;
347 case FLOW_ACTION_PPPOE_PUSH:
348 if (data.pppoe.num == 1)
349 return -EOPNOTSUPP;
350
351 data.pppoe.sid = act->pppoe.sid;
352 data.pppoe.num++;
353 break;
354 default:
355 return -EOPNOTSUPP;
356 }
357 }
358
359 if (!is_valid_ether_addr(data.eth.h_source) ||
360 !is_valid_ether_addr(data.eth.h_dest))
361 return -EINVAL;
362
363 err = mtk_foe_entry_prepare(eth, &foe, offload_type, l4proto, 0,
364 data.eth.h_source, data.eth.h_dest);
365 if (err)
366 return err;
367
368 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) {
369 struct flow_match_ports ports;
370
371 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
372 return -EOPNOTSUPP;
373
374 flow_rule_match_ports(rule, &ports);
375 data.src_port = ports.key->src;
376 data.dst_port = ports.key->dst;
377 } else if (offload_type != MTK_PPE_PKT_TYPE_BRIDGE) {
378 return -EOPNOTSUPP;
379 }
380
381 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
382 struct flow_match_ipv4_addrs addrs;
383
384 flow_rule_match_ipv4_addrs(rule, &addrs);
385
386 data.v4.src_addr = addrs.key->src;
387 data.v4.dst_addr = addrs.key->dst;
388
389 mtk_flow_set_ipv4_addr(eth, &foe, &data, false);
390 }
391
392 if (addr_type == FLOW_DISSECTOR_KEY_IPV6_ADDRS) {
393 struct flow_match_ipv6_addrs addrs;
394
395 flow_rule_match_ipv6_addrs(rule, &addrs);
396
397 data.v6.src_addr = addrs.key->src;
398 data.v6.dst_addr = addrs.key->dst;
399
400 mtk_flow_set_ipv6_addr(eth, &foe, &data);
401 }
402
403 flow_action_for_each(i, act, &rule->action) {
404 if (act->id != FLOW_ACTION_MANGLE)
405 continue;
406
407 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
408 return -EOPNOTSUPP;
409
410 switch (act->mangle.htype) {
411 case FLOW_ACT_MANGLE_HDR_TYPE_TCP:
412 case FLOW_ACT_MANGLE_HDR_TYPE_UDP:
413 err = mtk_flow_mangle_ports(act, &data);
414 break;
415 case FLOW_ACT_MANGLE_HDR_TYPE_IP4:
416 err = mtk_flow_mangle_ipv4(act, &data);
417 break;
418 case FLOW_ACT_MANGLE_HDR_TYPE_ETH:
419 /* handled earlier */
420 break;
421 default:
422 return -EOPNOTSUPP;
423 }
424
425 if (err)
426 return err;
427 }
428
429 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
430 err = mtk_flow_set_ipv4_addr(eth, &foe, &data, true);
431 if (err)
432 return err;
433 }
434
435 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
436 foe.bridge.vlan = data.vlan_in;
437
438 if (data.vlan.num == 1) {
439 if (data.vlan.proto != htons(ETH_P_8021Q))
440 return -EOPNOTSUPP;
441
442 mtk_foe_entry_set_vlan(eth, &foe, data.vlan.id);
443 }
444 if (data.pppoe.num == 1)
445 mtk_foe_entry_set_pppoe(eth, &foe, data.pppoe.sid);
446
447 err = mtk_flow_set_output_device(eth, &foe, odev, data.eth.h_dest,
448 &wed_index);
449 if (err)
450 return err;
451
452 if (wed_index >= 0 && (err = mtk_wed_flow_add(wed_index)) < 0)
453 return err;
454
455 entry = kzalloc(sizeof(*entry), GFP_KERNEL);
456 if (!entry)
457 return -ENOMEM;
458
459 entry->cookie = f->cookie;
460 memcpy(&entry->data, &foe, sizeof(entry->data));
461 entry->wed_index = wed_index;
462 entry->ppe_index = ppe_index;
463
464 err = mtk_foe_entry_commit(eth->ppe[entry->ppe_index], entry);
465 if (err < 0)
466 goto free;
467
468 err = rhashtable_insert_fast(ð->flow_table, &entry->node,
469 mtk_flow_ht_params);
470 if (err < 0)
471 goto clear;
472
473 return 0;
474
475clear:
476 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry);
477free:
478 kfree(entry);
479 if (wed_index >= 0)
480 mtk_wed_flow_remove(wed_index);
481 return err;
482}
483
484static int
485mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f)
486{
487 struct mtk_flow_entry *entry;
488
489 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
490 mtk_flow_ht_params);
491 if (!entry)
492 return -ENOENT;
493
494 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry);
495 rhashtable_remove_fast(ð->flow_table, &entry->node,
496 mtk_flow_ht_params);
497 if (entry->wed_index >= 0)
498 mtk_wed_flow_remove(entry->wed_index);
499 kfree(entry);
500
501 return 0;
502}
503
504static int
505mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f)
506{
507 struct mtk_flow_entry *entry;
508 struct mtk_foe_accounting diff;
509 u32 idle;
510
511 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
512 mtk_flow_ht_params);
513 if (!entry)
514 return -ENOENT;
515
516 idle = mtk_foe_entry_idle_time(eth->ppe[entry->ppe_index], entry);
517 f->stats.lastused = jiffies - idle * HZ;
518
519 if (entry->hash != 0xFFFF &&
520 mtk_foe_entry_get_mib(eth->ppe[entry->ppe_index], entry->hash,
521 &diff)) {
522 f->stats.pkts += diff.packets;
523 f->stats.bytes += diff.bytes;
524 }
525
526 return 0;
527}
528
529static DEFINE_MUTEX(mtk_flow_offload_mutex);
530
531int mtk_flow_offload_cmd(struct mtk_eth *eth, struct flow_cls_offload *cls,
532 int ppe_index)
533{
534 int err;
535
536 mutex_lock(&mtk_flow_offload_mutex);
537 switch (cls->command) {
538 case FLOW_CLS_REPLACE:
539 err = mtk_flow_offload_replace(eth, cls, ppe_index);
540 break;
541 case FLOW_CLS_DESTROY:
542 err = mtk_flow_offload_destroy(eth, cls);
543 break;
544 case FLOW_CLS_STATS:
545 err = mtk_flow_offload_stats(eth, cls);
546 break;
547 default:
548 err = -EOPNOTSUPP;
549 break;
550 }
551 mutex_unlock(&mtk_flow_offload_mutex);
552
553 return err;
554}
555
556static int
557mtk_eth_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
558{
559 struct flow_cls_offload *cls = type_data;
560 struct net_device *dev = cb_priv;
561 struct mtk_mac *mac;
562 struct mtk_eth *eth;
563
564 mac = netdev_priv(dev);
565 eth = mac->hw;
566
567 if (!tc_can_offload(dev))
568 return -EOPNOTSUPP;
569
570 if (type != TC_SETUP_CLSFLOWER)
571 return -EOPNOTSUPP;
572
573 return mtk_flow_offload_cmd(eth, cls, 0);
574}
575
576static int
577mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f)
578{
579 struct mtk_mac *mac = netdev_priv(dev);
580 struct mtk_eth *eth = mac->hw;
581 static LIST_HEAD(block_cb_list);
582 struct flow_block_cb *block_cb;
583 flow_setup_cb_t *cb;
584
585 if (!eth->soc->offload_version)
586 return -EOPNOTSUPP;
587
588 if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS)
589 return -EOPNOTSUPP;
590
591 cb = mtk_eth_setup_tc_block_cb;
592 f->driver_block_list = &block_cb_list;
593
594 switch (f->command) {
595 case FLOW_BLOCK_BIND:
596 block_cb = flow_block_cb_lookup(f->block, cb, dev);
597 if (block_cb) {
598 flow_block_cb_incref(block_cb);
599 return 0;
600 }
601 block_cb = flow_block_cb_alloc(cb, dev, dev, NULL);
602 if (IS_ERR(block_cb))
603 return PTR_ERR(block_cb);
604
605 flow_block_cb_incref(block_cb);
606 flow_block_cb_add(block_cb, f);
607 list_add_tail(&block_cb->driver_list, &block_cb_list);
608 return 0;
609 case FLOW_BLOCK_UNBIND:
610 block_cb = flow_block_cb_lookup(f->block, cb, dev);
611 if (!block_cb)
612 return -ENOENT;
613
614 if (!flow_block_cb_decref(block_cb)) {
615 flow_block_cb_remove(block_cb, f);
616 list_del(&block_cb->driver_list);
617 }
618 return 0;
619 default:
620 return -EOPNOTSUPP;
621 }
622}
623
624int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
625 void *type_data)
626{
627 switch (type) {
628 case TC_SETUP_BLOCK:
629 case TC_SETUP_FT:
630 return mtk_eth_setup_tc_block(dev, type_data);
631 default:
632 return -EOPNOTSUPP;
633 }
634}
635
636int mtk_eth_offload_init(struct mtk_eth *eth)
637{
638 return rhashtable_init(ð->flow_table, &mtk_flow_ht_params);
639}
1// SPDX-License-Identifier: GPL-2.0-only
2/*
3 * Copyright (C) 2020 Felix Fietkau <nbd@nbd.name>
4 */
5
6#include <linux/if_ether.h>
7#include <linux/rhashtable.h>
8#include <linux/ip.h>
9#include <net/flow_offload.h>
10#include <net/pkt_cls.h>
11#include <net/dsa.h>
12#include "mtk_eth_soc.h"
13
14struct mtk_flow_data {
15 struct ethhdr eth;
16
17 union {
18 struct {
19 __be32 src_addr;
20 __be32 dst_addr;
21 } v4;
22 };
23
24 __be16 src_port;
25 __be16 dst_port;
26
27 struct {
28 u16 id;
29 __be16 proto;
30 u8 num;
31 } vlan;
32 struct {
33 u16 sid;
34 u8 num;
35 } pppoe;
36};
37
38struct mtk_flow_entry {
39 struct rhash_head node;
40 unsigned long cookie;
41 u16 hash;
42};
43
44static const struct rhashtable_params mtk_flow_ht_params = {
45 .head_offset = offsetof(struct mtk_flow_entry, node),
46 .key_offset = offsetof(struct mtk_flow_entry, cookie),
47 .key_len = sizeof(unsigned long),
48 .automatic_shrinking = true,
49};
50
51static u32
52mtk_eth_timestamp(struct mtk_eth *eth)
53{
54 return mtk_r32(eth, 0x0010) & MTK_FOE_IB1_BIND_TIMESTAMP;
55}
56
57static int
58mtk_flow_set_ipv4_addr(struct mtk_foe_entry *foe, struct mtk_flow_data *data,
59 bool egress)
60{
61 return mtk_foe_entry_set_ipv4_tuple(foe, egress,
62 data->v4.src_addr, data->src_port,
63 data->v4.dst_addr, data->dst_port);
64}
65
66static void
67mtk_flow_offload_mangle_eth(const struct flow_action_entry *act, void *eth)
68{
69 void *dest = eth + act->mangle.offset;
70 const void *src = &act->mangle.val;
71
72 if (act->mangle.offset > 8)
73 return;
74
75 if (act->mangle.mask == 0xffff) {
76 src += 2;
77 dest += 2;
78 }
79
80 memcpy(dest, src, act->mangle.mask ? 2 : 4);
81}
82
83
84static int
85mtk_flow_mangle_ports(const struct flow_action_entry *act,
86 struct mtk_flow_data *data)
87{
88 u32 val = ntohl(act->mangle.val);
89
90 switch (act->mangle.offset) {
91 case 0:
92 if (act->mangle.mask == ~htonl(0xffff))
93 data->dst_port = cpu_to_be16(val);
94 else
95 data->src_port = cpu_to_be16(val >> 16);
96 break;
97 case 2:
98 data->dst_port = cpu_to_be16(val);
99 break;
100 default:
101 return -EINVAL;
102 }
103
104 return 0;
105}
106
107static int
108mtk_flow_mangle_ipv4(const struct flow_action_entry *act,
109 struct mtk_flow_data *data)
110{
111 __be32 *dest;
112
113 switch (act->mangle.offset) {
114 case offsetof(struct iphdr, saddr):
115 dest = &data->v4.src_addr;
116 break;
117 case offsetof(struct iphdr, daddr):
118 dest = &data->v4.dst_addr;
119 break;
120 default:
121 return -EINVAL;
122 }
123
124 memcpy(dest, &act->mangle.val, sizeof(u32));
125
126 return 0;
127}
128
129static int
130mtk_flow_get_dsa_port(struct net_device **dev)
131{
132#if IS_ENABLED(CONFIG_NET_DSA)
133 struct dsa_port *dp;
134
135 dp = dsa_port_from_netdev(*dev);
136 if (IS_ERR(dp))
137 return -ENODEV;
138
139 if (dp->cpu_dp->tag_ops->proto != DSA_TAG_PROTO_MTK)
140 return -ENODEV;
141
142 *dev = dp->cpu_dp->master;
143
144 return dp->index;
145#else
146 return -ENODEV;
147#endif
148}
149
150static int
151mtk_flow_set_output_device(struct mtk_eth *eth, struct mtk_foe_entry *foe,
152 struct net_device *dev)
153{
154 int pse_port, dsa_port;
155
156 dsa_port = mtk_flow_get_dsa_port(&dev);
157 if (dsa_port >= 0)
158 mtk_foe_entry_set_dsa(foe, dsa_port);
159
160 if (dev == eth->netdev[0])
161 pse_port = 1;
162 else if (dev == eth->netdev[1])
163 pse_port = 2;
164 else
165 return -EOPNOTSUPP;
166
167 mtk_foe_entry_set_pse_port(foe, pse_port);
168
169 return 0;
170}
171
172static int
173mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
174{
175 struct flow_rule *rule = flow_cls_offload_flow_rule(f);
176 struct flow_action_entry *act;
177 struct mtk_flow_data data = {};
178 struct mtk_foe_entry foe;
179 struct net_device *odev = NULL;
180 struct mtk_flow_entry *entry;
181 int offload_type = 0;
182 u16 addr_type = 0;
183 u32 timestamp;
184 u8 l4proto = 0;
185 int err = 0;
186 int hash;
187 int i;
188
189 if (rhashtable_lookup(ð->flow_table, &f->cookie, mtk_flow_ht_params))
190 return -EEXIST;
191
192 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_META)) {
193 struct flow_match_meta match;
194
195 flow_rule_match_meta(rule, &match);
196 } else {
197 return -EOPNOTSUPP;
198 }
199
200 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_CONTROL)) {
201 struct flow_match_control match;
202
203 flow_rule_match_control(rule, &match);
204 addr_type = match.key->addr_type;
205 } else {
206 return -EOPNOTSUPP;
207 }
208
209 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) {
210 struct flow_match_basic match;
211
212 flow_rule_match_basic(rule, &match);
213 l4proto = match.key->ip_proto;
214 } else {
215 return -EOPNOTSUPP;
216 }
217
218 flow_action_for_each(i, act, &rule->action) {
219 switch (act->id) {
220 case FLOW_ACTION_MANGLE:
221 if (act->mangle.htype == FLOW_ACT_MANGLE_HDR_TYPE_ETH)
222 mtk_flow_offload_mangle_eth(act, &data.eth);
223 break;
224 case FLOW_ACTION_REDIRECT:
225 odev = act->dev;
226 break;
227 case FLOW_ACTION_CSUM:
228 break;
229 case FLOW_ACTION_VLAN_PUSH:
230 if (data.vlan.num == 1 ||
231 act->vlan.proto != htons(ETH_P_8021Q))
232 return -EOPNOTSUPP;
233
234 data.vlan.id = act->vlan.vid;
235 data.vlan.proto = act->vlan.proto;
236 data.vlan.num++;
237 break;
238 case FLOW_ACTION_VLAN_POP:
239 break;
240 case FLOW_ACTION_PPPOE_PUSH:
241 if (data.pppoe.num == 1)
242 return -EOPNOTSUPP;
243
244 data.pppoe.sid = act->pppoe.sid;
245 data.pppoe.num++;
246 break;
247 default:
248 return -EOPNOTSUPP;
249 }
250 }
251
252 switch (addr_type) {
253 case FLOW_DISSECTOR_KEY_IPV4_ADDRS:
254 offload_type = MTK_PPE_PKT_TYPE_IPV4_HNAPT;
255 break;
256 default:
257 return -EOPNOTSUPP;
258 }
259
260 if (!is_valid_ether_addr(data.eth.h_source) ||
261 !is_valid_ether_addr(data.eth.h_dest))
262 return -EINVAL;
263
264 err = mtk_foe_entry_prepare(&foe, offload_type, l4proto, 0,
265 data.eth.h_source,
266 data.eth.h_dest);
267 if (err)
268 return err;
269
270 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) {
271 struct flow_match_ports ports;
272
273 flow_rule_match_ports(rule, &ports);
274 data.src_port = ports.key->src;
275 data.dst_port = ports.key->dst;
276 } else {
277 return -EOPNOTSUPP;
278 }
279
280 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
281 struct flow_match_ipv4_addrs addrs;
282
283 flow_rule_match_ipv4_addrs(rule, &addrs);
284
285 data.v4.src_addr = addrs.key->src;
286 data.v4.dst_addr = addrs.key->dst;
287
288 mtk_flow_set_ipv4_addr(&foe, &data, false);
289 }
290
291 flow_action_for_each(i, act, &rule->action) {
292 if (act->id != FLOW_ACTION_MANGLE)
293 continue;
294
295 switch (act->mangle.htype) {
296 case FLOW_ACT_MANGLE_HDR_TYPE_TCP:
297 case FLOW_ACT_MANGLE_HDR_TYPE_UDP:
298 err = mtk_flow_mangle_ports(act, &data);
299 break;
300 case FLOW_ACT_MANGLE_HDR_TYPE_IP4:
301 err = mtk_flow_mangle_ipv4(act, &data);
302 break;
303 case FLOW_ACT_MANGLE_HDR_TYPE_ETH:
304 /* handled earlier */
305 break;
306 default:
307 return -EOPNOTSUPP;
308 }
309
310 if (err)
311 return err;
312 }
313
314 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
315 err = mtk_flow_set_ipv4_addr(&foe, &data, true);
316 if (err)
317 return err;
318 }
319
320 if (data.vlan.num == 1) {
321 if (data.vlan.proto != htons(ETH_P_8021Q))
322 return -EOPNOTSUPP;
323
324 mtk_foe_entry_set_vlan(&foe, data.vlan.id);
325 }
326 if (data.pppoe.num == 1)
327 mtk_foe_entry_set_pppoe(&foe, data.pppoe.sid);
328
329 err = mtk_flow_set_output_device(eth, &foe, odev);
330 if (err)
331 return err;
332
333 entry = kzalloc(sizeof(*entry), GFP_KERNEL);
334 if (!entry)
335 return -ENOMEM;
336
337 entry->cookie = f->cookie;
338 timestamp = mtk_eth_timestamp(eth);
339 hash = mtk_foe_entry_commit(ð->ppe, &foe, timestamp);
340 if (hash < 0) {
341 err = hash;
342 goto free;
343 }
344
345 entry->hash = hash;
346 err = rhashtable_insert_fast(ð->flow_table, &entry->node,
347 mtk_flow_ht_params);
348 if (err < 0)
349 goto clear_flow;
350
351 return 0;
352clear_flow:
353 mtk_foe_entry_clear(ð->ppe, hash);
354free:
355 kfree(entry);
356 return err;
357}
358
359static int
360mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f)
361{
362 struct mtk_flow_entry *entry;
363
364 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
365 mtk_flow_ht_params);
366 if (!entry)
367 return -ENOENT;
368
369 mtk_foe_entry_clear(ð->ppe, entry->hash);
370 rhashtable_remove_fast(ð->flow_table, &entry->node,
371 mtk_flow_ht_params);
372 kfree(entry);
373
374 return 0;
375}
376
377static int
378mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f)
379{
380 struct mtk_flow_entry *entry;
381 int timestamp;
382 u32 idle;
383
384 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
385 mtk_flow_ht_params);
386 if (!entry)
387 return -ENOENT;
388
389 timestamp = mtk_foe_entry_timestamp(ð->ppe, entry->hash);
390 if (timestamp < 0)
391 return -ETIMEDOUT;
392
393 idle = mtk_eth_timestamp(eth) - timestamp;
394 f->stats.lastused = jiffies - idle * HZ;
395
396 return 0;
397}
398
399static DEFINE_MUTEX(mtk_flow_offload_mutex);
400
401static int
402mtk_eth_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
403{
404 struct flow_cls_offload *cls = type_data;
405 struct net_device *dev = cb_priv;
406 struct mtk_mac *mac = netdev_priv(dev);
407 struct mtk_eth *eth = mac->hw;
408 int err;
409
410 if (!tc_can_offload(dev))
411 return -EOPNOTSUPP;
412
413 if (type != TC_SETUP_CLSFLOWER)
414 return -EOPNOTSUPP;
415
416 mutex_lock(&mtk_flow_offload_mutex);
417 switch (cls->command) {
418 case FLOW_CLS_REPLACE:
419 err = mtk_flow_offload_replace(eth, cls);
420 break;
421 case FLOW_CLS_DESTROY:
422 err = mtk_flow_offload_destroy(eth, cls);
423 break;
424 case FLOW_CLS_STATS:
425 err = mtk_flow_offload_stats(eth, cls);
426 break;
427 default:
428 err = -EOPNOTSUPP;
429 break;
430 }
431 mutex_unlock(&mtk_flow_offload_mutex);
432
433 return err;
434}
435
436static int
437mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f)
438{
439 struct mtk_mac *mac = netdev_priv(dev);
440 struct mtk_eth *eth = mac->hw;
441 static LIST_HEAD(block_cb_list);
442 struct flow_block_cb *block_cb;
443 flow_setup_cb_t *cb;
444
445 if (!eth->ppe.foe_table)
446 return -EOPNOTSUPP;
447
448 if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS)
449 return -EOPNOTSUPP;
450
451 cb = mtk_eth_setup_tc_block_cb;
452 f->driver_block_list = &block_cb_list;
453
454 switch (f->command) {
455 case FLOW_BLOCK_BIND:
456 block_cb = flow_block_cb_lookup(f->block, cb, dev);
457 if (block_cb) {
458 flow_block_cb_incref(block_cb);
459 return 0;
460 }
461 block_cb = flow_block_cb_alloc(cb, dev, dev, NULL);
462 if (IS_ERR(block_cb))
463 return PTR_ERR(block_cb);
464
465 flow_block_cb_add(block_cb, f);
466 list_add_tail(&block_cb->driver_list, &block_cb_list);
467 return 0;
468 case FLOW_BLOCK_UNBIND:
469 block_cb = flow_block_cb_lookup(f->block, cb, dev);
470 if (!block_cb)
471 return -ENOENT;
472
473 if (flow_block_cb_decref(block_cb)) {
474 flow_block_cb_remove(block_cb, f);
475 list_del(&block_cb->driver_list);
476 }
477 return 0;
478 default:
479 return -EOPNOTSUPP;
480 }
481}
482
483int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
484 void *type_data)
485{
486 if (type == TC_SETUP_FT)
487 return mtk_eth_setup_tc_block(dev, type_data);
488
489 return -EOPNOTSUPP;
490}
491
492int mtk_eth_offload_init(struct mtk_eth *eth)
493{
494 if (!eth->ppe.foe_table)
495 return 0;
496
497 return rhashtable_init(ð->flow_table, &mtk_flow_ht_params);
498}