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