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 net_device *idev = NULL, *odev = NULL;
249 struct flow_action_entry *act;
250 struct mtk_flow_data data = {};
251 struct mtk_foe_entry foe;
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 if (mtk_is_netsys_v2_or_greater(eth)) {
268 idev = __dev_get_by_index(&init_net, match.key->ingress_ifindex);
269 if (idev && idev->netdev_ops == eth->netdev[0]->netdev_ops) {
270 struct mtk_mac *mac = netdev_priv(idev);
271
272 if (WARN_ON(mac->ppe_idx >= eth->soc->ppe_num))
273 return -EINVAL;
274
275 ppe_index = mac->ppe_idx;
276 }
277 }
278 } else {
279 return -EOPNOTSUPP;
280 }
281
282 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_CONTROL)) {
283 struct flow_match_control match;
284
285 flow_rule_match_control(rule, &match);
286 addr_type = match.key->addr_type;
287
288 if (flow_rule_has_control_flags(match.mask->flags,
289 f->common.extack))
290 return -EOPNOTSUPP;
291 } else {
292 return -EOPNOTSUPP;
293 }
294
295 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) {
296 struct flow_match_basic match;
297
298 flow_rule_match_basic(rule, &match);
299 l4proto = match.key->ip_proto;
300 } else {
301 return -EOPNOTSUPP;
302 }
303
304 switch (addr_type) {
305 case 0:
306 offload_type = MTK_PPE_PKT_TYPE_BRIDGE;
307 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
308 struct flow_match_eth_addrs match;
309
310 flow_rule_match_eth_addrs(rule, &match);
311 memcpy(data.eth.h_dest, match.key->dst, ETH_ALEN);
312 memcpy(data.eth.h_source, match.key->src, ETH_ALEN);
313 } else {
314 return -EOPNOTSUPP;
315 }
316
317 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
318 struct flow_match_vlan match;
319
320 flow_rule_match_vlan(rule, &match);
321
322 if (match.key->vlan_tpid != cpu_to_be16(ETH_P_8021Q))
323 return -EOPNOTSUPP;
324
325 data.vlan_in = match.key->vlan_id;
326 }
327 break;
328 case FLOW_DISSECTOR_KEY_IPV4_ADDRS:
329 offload_type = MTK_PPE_PKT_TYPE_IPV4_HNAPT;
330 break;
331 case FLOW_DISSECTOR_KEY_IPV6_ADDRS:
332 offload_type = MTK_PPE_PKT_TYPE_IPV6_ROUTE_5T;
333 break;
334 default:
335 return -EOPNOTSUPP;
336 }
337
338 flow_action_for_each(i, act, &rule->action) {
339 switch (act->id) {
340 case FLOW_ACTION_MANGLE:
341 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
342 return -EOPNOTSUPP;
343 if (act->mangle.htype == FLOW_ACT_MANGLE_HDR_TYPE_ETH)
344 mtk_flow_offload_mangle_eth(act, &data.eth);
345 break;
346 case FLOW_ACTION_REDIRECT:
347 odev = act->dev;
348 break;
349 case FLOW_ACTION_CSUM:
350 break;
351 case FLOW_ACTION_VLAN_PUSH:
352 if (data.vlan.num == 1 ||
353 act->vlan.proto != htons(ETH_P_8021Q))
354 return -EOPNOTSUPP;
355
356 data.vlan.id = act->vlan.vid;
357 data.vlan.proto = act->vlan.proto;
358 data.vlan.num++;
359 break;
360 case FLOW_ACTION_VLAN_POP:
361 break;
362 case FLOW_ACTION_PPPOE_PUSH:
363 if (data.pppoe.num == 1)
364 return -EOPNOTSUPP;
365
366 data.pppoe.sid = act->pppoe.sid;
367 data.pppoe.num++;
368 break;
369 default:
370 return -EOPNOTSUPP;
371 }
372 }
373
374 if (!is_valid_ether_addr(data.eth.h_source) ||
375 !is_valid_ether_addr(data.eth.h_dest))
376 return -EINVAL;
377
378 err = mtk_foe_entry_prepare(eth, &foe, offload_type, l4proto, 0,
379 data.eth.h_source, data.eth.h_dest);
380 if (err)
381 return err;
382
383 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) {
384 struct flow_match_ports ports;
385
386 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
387 return -EOPNOTSUPP;
388
389 flow_rule_match_ports(rule, &ports);
390 data.src_port = ports.key->src;
391 data.dst_port = ports.key->dst;
392 } else if (offload_type != MTK_PPE_PKT_TYPE_BRIDGE) {
393 return -EOPNOTSUPP;
394 }
395
396 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
397 struct flow_match_ipv4_addrs addrs;
398
399 flow_rule_match_ipv4_addrs(rule, &addrs);
400
401 data.v4.src_addr = addrs.key->src;
402 data.v4.dst_addr = addrs.key->dst;
403
404 mtk_flow_set_ipv4_addr(eth, &foe, &data, false);
405 }
406
407 if (addr_type == FLOW_DISSECTOR_KEY_IPV6_ADDRS) {
408 struct flow_match_ipv6_addrs addrs;
409
410 flow_rule_match_ipv6_addrs(rule, &addrs);
411
412 data.v6.src_addr = addrs.key->src;
413 data.v6.dst_addr = addrs.key->dst;
414
415 mtk_flow_set_ipv6_addr(eth, &foe, &data);
416 }
417
418 flow_action_for_each(i, act, &rule->action) {
419 if (act->id != FLOW_ACTION_MANGLE)
420 continue;
421
422 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
423 return -EOPNOTSUPP;
424
425 switch (act->mangle.htype) {
426 case FLOW_ACT_MANGLE_HDR_TYPE_TCP:
427 case FLOW_ACT_MANGLE_HDR_TYPE_UDP:
428 err = mtk_flow_mangle_ports(act, &data);
429 break;
430 case FLOW_ACT_MANGLE_HDR_TYPE_IP4:
431 err = mtk_flow_mangle_ipv4(act, &data);
432 break;
433 case FLOW_ACT_MANGLE_HDR_TYPE_ETH:
434 /* handled earlier */
435 break;
436 default:
437 return -EOPNOTSUPP;
438 }
439
440 if (err)
441 return err;
442 }
443
444 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
445 err = mtk_flow_set_ipv4_addr(eth, &foe, &data, true);
446 if (err)
447 return err;
448 }
449
450 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
451 foe.bridge.vlan = data.vlan_in;
452
453 if (data.vlan.num == 1) {
454 if (data.vlan.proto != htons(ETH_P_8021Q))
455 return -EOPNOTSUPP;
456
457 mtk_foe_entry_set_vlan(eth, &foe, data.vlan.id);
458 }
459 if (data.pppoe.num == 1)
460 mtk_foe_entry_set_pppoe(eth, &foe, data.pppoe.sid);
461
462 err = mtk_flow_set_output_device(eth, &foe, odev, data.eth.h_dest,
463 &wed_index);
464 if (err)
465 return err;
466
467 if (wed_index >= 0 && (err = mtk_wed_flow_add(wed_index)) < 0)
468 return err;
469
470 entry = kzalloc(sizeof(*entry), GFP_KERNEL);
471 if (!entry)
472 return -ENOMEM;
473
474 entry->cookie = f->cookie;
475 memcpy(&entry->data, &foe, sizeof(entry->data));
476 entry->wed_index = wed_index;
477 entry->ppe_index = ppe_index;
478
479 err = mtk_foe_entry_commit(eth->ppe[entry->ppe_index], entry);
480 if (err < 0)
481 goto free;
482
483 err = rhashtable_insert_fast(ð->flow_table, &entry->node,
484 mtk_flow_ht_params);
485 if (err < 0)
486 goto clear;
487
488 return 0;
489
490clear:
491 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry);
492free:
493 kfree(entry);
494 if (wed_index >= 0)
495 mtk_wed_flow_remove(wed_index);
496 return err;
497}
498
499static int
500mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f)
501{
502 struct mtk_flow_entry *entry;
503
504 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
505 mtk_flow_ht_params);
506 if (!entry)
507 return -ENOENT;
508
509 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry);
510 rhashtable_remove_fast(ð->flow_table, &entry->node,
511 mtk_flow_ht_params);
512 if (entry->wed_index >= 0)
513 mtk_wed_flow_remove(entry->wed_index);
514 kfree(entry);
515
516 return 0;
517}
518
519static int
520mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f)
521{
522 struct mtk_flow_entry *entry;
523 struct mtk_foe_accounting diff;
524 u32 idle;
525
526 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
527 mtk_flow_ht_params);
528 if (!entry)
529 return -ENOENT;
530
531 idle = mtk_foe_entry_idle_time(eth->ppe[entry->ppe_index], entry);
532 f->stats.lastused = jiffies - idle * HZ;
533
534 if (entry->hash != 0xFFFF &&
535 mtk_foe_entry_get_mib(eth->ppe[entry->ppe_index], entry->hash,
536 &diff)) {
537 f->stats.pkts += diff.packets;
538 f->stats.bytes += diff.bytes;
539 }
540
541 return 0;
542}
543
544static DEFINE_MUTEX(mtk_flow_offload_mutex);
545
546int mtk_flow_offload_cmd(struct mtk_eth *eth, struct flow_cls_offload *cls,
547 int ppe_index)
548{
549 int err;
550
551 mutex_lock(&mtk_flow_offload_mutex);
552 switch (cls->command) {
553 case FLOW_CLS_REPLACE:
554 err = mtk_flow_offload_replace(eth, cls, ppe_index);
555 break;
556 case FLOW_CLS_DESTROY:
557 err = mtk_flow_offload_destroy(eth, cls);
558 break;
559 case FLOW_CLS_STATS:
560 err = mtk_flow_offload_stats(eth, cls);
561 break;
562 default:
563 err = -EOPNOTSUPP;
564 break;
565 }
566 mutex_unlock(&mtk_flow_offload_mutex);
567
568 return err;
569}
570
571static int
572mtk_eth_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
573{
574 struct flow_cls_offload *cls = type_data;
575 struct net_device *dev = cb_priv;
576 struct mtk_mac *mac;
577 struct mtk_eth *eth;
578
579 mac = netdev_priv(dev);
580 eth = mac->hw;
581
582 if (!tc_can_offload(dev))
583 return -EOPNOTSUPP;
584
585 if (type != TC_SETUP_CLSFLOWER)
586 return -EOPNOTSUPP;
587
588 return mtk_flow_offload_cmd(eth, cls, 0);
589}
590
591static int
592mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f)
593{
594 struct mtk_mac *mac = netdev_priv(dev);
595 struct mtk_eth *eth = mac->hw;
596 static LIST_HEAD(block_cb_list);
597 struct flow_block_cb *block_cb;
598 flow_setup_cb_t *cb;
599
600 if (!eth->soc->offload_version)
601 return -EOPNOTSUPP;
602
603 if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS)
604 return -EOPNOTSUPP;
605
606 cb = mtk_eth_setup_tc_block_cb;
607 f->driver_block_list = &block_cb_list;
608
609 switch (f->command) {
610 case FLOW_BLOCK_BIND:
611 block_cb = flow_block_cb_lookup(f->block, cb, dev);
612 if (block_cb) {
613 flow_block_cb_incref(block_cb);
614 return 0;
615 }
616 block_cb = flow_block_cb_alloc(cb, dev, dev, NULL);
617 if (IS_ERR(block_cb))
618 return PTR_ERR(block_cb);
619
620 flow_block_cb_incref(block_cb);
621 flow_block_cb_add(block_cb, f);
622 list_add_tail(&block_cb->driver_list, &block_cb_list);
623 return 0;
624 case FLOW_BLOCK_UNBIND:
625 block_cb = flow_block_cb_lookup(f->block, cb, dev);
626 if (!block_cb)
627 return -ENOENT;
628
629 if (!flow_block_cb_decref(block_cb)) {
630 flow_block_cb_remove(block_cb, f);
631 list_del(&block_cb->driver_list);
632 }
633 return 0;
634 default:
635 return -EOPNOTSUPP;
636 }
637}
638
639int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
640 void *type_data)
641{
642 switch (type) {
643 case TC_SETUP_BLOCK:
644 case TC_SETUP_FT:
645 return mtk_eth_setup_tc_block(dev, type_data);
646 default:
647 return -EOPNOTSUPP;
648 }
649}
650
651int mtk_eth_offload_init(struct mtk_eth *eth, u8 id)
652{
653 if (!eth->ppe[id] || !eth->ppe[id]->foe_table)
654 return 0;
655 return rhashtable_init(ð->flow_table, &mtk_flow_ht_params);
656}
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
115 return 0;
116}
117
118
119static int
120mtk_flow_mangle_ports(const struct flow_action_entry *act,
121 struct mtk_flow_data *data)
122{
123 u32 val = ntohl(act->mangle.val);
124
125 switch (act->mangle.offset) {
126 case 0:
127 if (act->mangle.mask == ~htonl(0xffff))
128 data->dst_port = cpu_to_be16(val);
129 else
130 data->src_port = cpu_to_be16(val >> 16);
131 break;
132 case 2:
133 data->dst_port = cpu_to_be16(val);
134 break;
135 default:
136 return -EINVAL;
137 }
138
139 return 0;
140}
141
142static int
143mtk_flow_mangle_ipv4(const struct flow_action_entry *act,
144 struct mtk_flow_data *data)
145{
146 __be32 *dest;
147
148 switch (act->mangle.offset) {
149 case offsetof(struct iphdr, saddr):
150 dest = &data->v4.src_addr;
151 break;
152 case offsetof(struct iphdr, daddr):
153 dest = &data->v4.dst_addr;
154 break;
155 default:
156 return -EINVAL;
157 }
158
159 memcpy(dest, &act->mangle.val, sizeof(u32));
160
161 return 0;
162}
163
164static int
165mtk_flow_get_dsa_port(struct net_device **dev)
166{
167#if IS_ENABLED(CONFIG_NET_DSA)
168 struct dsa_port *dp;
169
170 dp = dsa_port_from_netdev(*dev);
171 if (IS_ERR(dp))
172 return -ENODEV;
173
174 if (dp->cpu_dp->tag_ops->proto != DSA_TAG_PROTO_MTK)
175 return -ENODEV;
176
177 *dev = dsa_port_to_master(dp);
178
179 return dp->index;
180#else
181 return -ENODEV;
182#endif
183}
184
185static int
186mtk_flow_set_output_device(struct mtk_eth *eth, struct mtk_foe_entry *foe,
187 struct net_device *dev, const u8 *dest_mac,
188 int *wed_index)
189{
190 struct mtk_wdma_info info = {};
191 int pse_port, dsa_port, queue;
192
193 if (mtk_flow_get_wdma_info(dev, dest_mac, &info) == 0) {
194 mtk_foe_entry_set_wdma(eth, foe, info.wdma_idx, info.queue,
195 info.bss, info.wcid);
196 if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V2)) {
197 switch (info.wdma_idx) {
198 case 0:
199 pse_port = 8;
200 break;
201 case 1:
202 pse_port = 9;
203 break;
204 default:
205 return -EINVAL;
206 }
207 } else {
208 pse_port = 3;
209 }
210 *wed_index = info.wdma_idx;
211 goto out;
212 }
213
214 dsa_port = mtk_flow_get_dsa_port(&dev);
215
216 if (dev == eth->netdev[0])
217 pse_port = 1;
218 else if (dev == eth->netdev[1])
219 pse_port = 2;
220 else
221 return -EOPNOTSUPP;
222
223 if (dsa_port >= 0) {
224 mtk_foe_entry_set_dsa(eth, foe, dsa_port);
225 queue = 3 + dsa_port;
226 } else {
227 queue = pse_port - 1;
228 }
229 mtk_foe_entry_set_queue(eth, foe, queue);
230
231out:
232 mtk_foe_entry_set_pse_port(eth, foe, pse_port);
233
234 return 0;
235}
236
237static int
238mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
239{
240 struct flow_rule *rule = flow_cls_offload_flow_rule(f);
241 struct flow_action_entry *act;
242 struct mtk_flow_data data = {};
243 struct mtk_foe_entry foe;
244 struct net_device *odev = NULL;
245 struct mtk_flow_entry *entry;
246 int offload_type = 0;
247 int wed_index = -1;
248 u16 addr_type = 0;
249 u8 l4proto = 0;
250 int err = 0;
251 int i;
252
253 if (rhashtable_lookup(ð->flow_table, &f->cookie, mtk_flow_ht_params))
254 return -EEXIST;
255
256 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_META)) {
257 struct flow_match_meta match;
258
259 flow_rule_match_meta(rule, &match);
260 } else {
261 return -EOPNOTSUPP;
262 }
263
264 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_CONTROL)) {
265 struct flow_match_control match;
266
267 flow_rule_match_control(rule, &match);
268 addr_type = match.key->addr_type;
269 } else {
270 return -EOPNOTSUPP;
271 }
272
273 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) {
274 struct flow_match_basic match;
275
276 flow_rule_match_basic(rule, &match);
277 l4proto = match.key->ip_proto;
278 } else {
279 return -EOPNOTSUPP;
280 }
281
282 switch (addr_type) {
283 case 0:
284 offload_type = MTK_PPE_PKT_TYPE_BRIDGE;
285 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
286 struct flow_match_eth_addrs match;
287
288 flow_rule_match_eth_addrs(rule, &match);
289 memcpy(data.eth.h_dest, match.key->dst, ETH_ALEN);
290 memcpy(data.eth.h_source, match.key->src, ETH_ALEN);
291 } else {
292 return -EOPNOTSUPP;
293 }
294
295 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
296 struct flow_match_vlan match;
297
298 flow_rule_match_vlan(rule, &match);
299
300 if (match.key->vlan_tpid != cpu_to_be16(ETH_P_8021Q))
301 return -EOPNOTSUPP;
302
303 data.vlan_in = match.key->vlan_id;
304 }
305 break;
306 case FLOW_DISSECTOR_KEY_IPV4_ADDRS:
307 offload_type = MTK_PPE_PKT_TYPE_IPV4_HNAPT;
308 break;
309 case FLOW_DISSECTOR_KEY_IPV6_ADDRS:
310 offload_type = MTK_PPE_PKT_TYPE_IPV6_ROUTE_5T;
311 break;
312 default:
313 return -EOPNOTSUPP;
314 }
315
316 flow_action_for_each(i, act, &rule->action) {
317 switch (act->id) {
318 case FLOW_ACTION_MANGLE:
319 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
320 return -EOPNOTSUPP;
321 if (act->mangle.htype == FLOW_ACT_MANGLE_HDR_TYPE_ETH)
322 mtk_flow_offload_mangle_eth(act, &data.eth);
323 break;
324 case FLOW_ACTION_REDIRECT:
325 odev = act->dev;
326 break;
327 case FLOW_ACTION_CSUM:
328 break;
329 case FLOW_ACTION_VLAN_PUSH:
330 if (data.vlan.num == 1 ||
331 act->vlan.proto != htons(ETH_P_8021Q))
332 return -EOPNOTSUPP;
333
334 data.vlan.id = act->vlan.vid;
335 data.vlan.proto = act->vlan.proto;
336 data.vlan.num++;
337 break;
338 case FLOW_ACTION_VLAN_POP:
339 break;
340 case FLOW_ACTION_PPPOE_PUSH:
341 if (data.pppoe.num == 1)
342 return -EOPNOTSUPP;
343
344 data.pppoe.sid = act->pppoe.sid;
345 data.pppoe.num++;
346 break;
347 default:
348 return -EOPNOTSUPP;
349 }
350 }
351
352 if (!is_valid_ether_addr(data.eth.h_source) ||
353 !is_valid_ether_addr(data.eth.h_dest))
354 return -EINVAL;
355
356 err = mtk_foe_entry_prepare(eth, &foe, offload_type, l4proto, 0,
357 data.eth.h_source, data.eth.h_dest);
358 if (err)
359 return err;
360
361 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) {
362 struct flow_match_ports ports;
363
364 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
365 return -EOPNOTSUPP;
366
367 flow_rule_match_ports(rule, &ports);
368 data.src_port = ports.key->src;
369 data.dst_port = ports.key->dst;
370 } else if (offload_type != MTK_PPE_PKT_TYPE_BRIDGE) {
371 return -EOPNOTSUPP;
372 }
373
374 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
375 struct flow_match_ipv4_addrs addrs;
376
377 flow_rule_match_ipv4_addrs(rule, &addrs);
378
379 data.v4.src_addr = addrs.key->src;
380 data.v4.dst_addr = addrs.key->dst;
381
382 mtk_flow_set_ipv4_addr(eth, &foe, &data, false);
383 }
384
385 if (addr_type == FLOW_DISSECTOR_KEY_IPV6_ADDRS) {
386 struct flow_match_ipv6_addrs addrs;
387
388 flow_rule_match_ipv6_addrs(rule, &addrs);
389
390 data.v6.src_addr = addrs.key->src;
391 data.v6.dst_addr = addrs.key->dst;
392
393 mtk_flow_set_ipv6_addr(eth, &foe, &data);
394 }
395
396 flow_action_for_each(i, act, &rule->action) {
397 if (act->id != FLOW_ACTION_MANGLE)
398 continue;
399
400 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
401 return -EOPNOTSUPP;
402
403 switch (act->mangle.htype) {
404 case FLOW_ACT_MANGLE_HDR_TYPE_TCP:
405 case FLOW_ACT_MANGLE_HDR_TYPE_UDP:
406 err = mtk_flow_mangle_ports(act, &data);
407 break;
408 case FLOW_ACT_MANGLE_HDR_TYPE_IP4:
409 err = mtk_flow_mangle_ipv4(act, &data);
410 break;
411 case FLOW_ACT_MANGLE_HDR_TYPE_ETH:
412 /* handled earlier */
413 break;
414 default:
415 return -EOPNOTSUPP;
416 }
417
418 if (err)
419 return err;
420 }
421
422 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
423 err = mtk_flow_set_ipv4_addr(eth, &foe, &data, true);
424 if (err)
425 return err;
426 }
427
428 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
429 foe.bridge.vlan = data.vlan_in;
430
431 if (data.vlan.num == 1) {
432 if (data.vlan.proto != htons(ETH_P_8021Q))
433 return -EOPNOTSUPP;
434
435 mtk_foe_entry_set_vlan(eth, &foe, data.vlan.id);
436 }
437 if (data.pppoe.num == 1)
438 mtk_foe_entry_set_pppoe(eth, &foe, data.pppoe.sid);
439
440 err = mtk_flow_set_output_device(eth, &foe, odev, data.eth.h_dest,
441 &wed_index);
442 if (err)
443 return err;
444
445 if (wed_index >= 0 && (err = mtk_wed_flow_add(wed_index)) < 0)
446 return err;
447
448 entry = kzalloc(sizeof(*entry), GFP_KERNEL);
449 if (!entry)
450 return -ENOMEM;
451
452 entry->cookie = f->cookie;
453 memcpy(&entry->data, &foe, sizeof(entry->data));
454 entry->wed_index = wed_index;
455
456 err = mtk_foe_entry_commit(eth->ppe[entry->ppe_index], entry);
457 if (err < 0)
458 goto free;
459
460 err = rhashtable_insert_fast(ð->flow_table, &entry->node,
461 mtk_flow_ht_params);
462 if (err < 0)
463 goto clear;
464
465 return 0;
466
467clear:
468 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry);
469free:
470 kfree(entry);
471 if (wed_index >= 0)
472 mtk_wed_flow_remove(wed_index);
473 return err;
474}
475
476static int
477mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f)
478{
479 struct mtk_flow_entry *entry;
480
481 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
482 mtk_flow_ht_params);
483 if (!entry)
484 return -ENOENT;
485
486 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry);
487 rhashtable_remove_fast(ð->flow_table, &entry->node,
488 mtk_flow_ht_params);
489 if (entry->wed_index >= 0)
490 mtk_wed_flow_remove(entry->wed_index);
491 kfree(entry);
492
493 return 0;
494}
495
496static int
497mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f)
498{
499 struct mtk_flow_entry *entry;
500 u32 idle;
501
502 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
503 mtk_flow_ht_params);
504 if (!entry)
505 return -ENOENT;
506
507 idle = mtk_foe_entry_idle_time(eth->ppe[entry->ppe_index], entry);
508 f->stats.lastused = jiffies - idle * HZ;
509
510 return 0;
511}
512
513static DEFINE_MUTEX(mtk_flow_offload_mutex);
514
515static int
516mtk_eth_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
517{
518 struct flow_cls_offload *cls = type_data;
519 struct net_device *dev = cb_priv;
520 struct mtk_mac *mac = netdev_priv(dev);
521 struct mtk_eth *eth = mac->hw;
522 int err;
523
524 if (!tc_can_offload(dev))
525 return -EOPNOTSUPP;
526
527 if (type != TC_SETUP_CLSFLOWER)
528 return -EOPNOTSUPP;
529
530 mutex_lock(&mtk_flow_offload_mutex);
531 switch (cls->command) {
532 case FLOW_CLS_REPLACE:
533 err = mtk_flow_offload_replace(eth, cls);
534 break;
535 case FLOW_CLS_DESTROY:
536 err = mtk_flow_offload_destroy(eth, cls);
537 break;
538 case FLOW_CLS_STATS:
539 err = mtk_flow_offload_stats(eth, cls);
540 break;
541 default:
542 err = -EOPNOTSUPP;
543 break;
544 }
545 mutex_unlock(&mtk_flow_offload_mutex);
546
547 return err;
548}
549
550static int
551mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f)
552{
553 struct mtk_mac *mac = netdev_priv(dev);
554 struct mtk_eth *eth = mac->hw;
555 static LIST_HEAD(block_cb_list);
556 struct flow_block_cb *block_cb;
557 flow_setup_cb_t *cb;
558
559 if (!eth->soc->offload_version)
560 return -EOPNOTSUPP;
561
562 if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS)
563 return -EOPNOTSUPP;
564
565 cb = mtk_eth_setup_tc_block_cb;
566 f->driver_block_list = &block_cb_list;
567
568 switch (f->command) {
569 case FLOW_BLOCK_BIND:
570 block_cb = flow_block_cb_lookup(f->block, cb, dev);
571 if (block_cb) {
572 flow_block_cb_incref(block_cb);
573 return 0;
574 }
575 block_cb = flow_block_cb_alloc(cb, dev, dev, NULL);
576 if (IS_ERR(block_cb))
577 return PTR_ERR(block_cb);
578
579 flow_block_cb_add(block_cb, f);
580 list_add_tail(&block_cb->driver_list, &block_cb_list);
581 return 0;
582 case FLOW_BLOCK_UNBIND:
583 block_cb = flow_block_cb_lookup(f->block, cb, dev);
584 if (!block_cb)
585 return -ENOENT;
586
587 if (flow_block_cb_decref(block_cb)) {
588 flow_block_cb_remove(block_cb, f);
589 list_del(&block_cb->driver_list);
590 }
591 return 0;
592 default:
593 return -EOPNOTSUPP;
594 }
595}
596
597int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
598 void *type_data)
599{
600 switch (type) {
601 case TC_SETUP_BLOCK:
602 case TC_SETUP_FT:
603 return mtk_eth_setup_tc_block(dev, type_data);
604 default:
605 return -EOPNOTSUPP;
606 }
607}
608
609int mtk_eth_offload_init(struct mtk_eth *eth)
610{
611 return rhashtable_init(ð->flow_table, &mtk_flow_ht_params);
612}