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