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 u16 id;
38 __be16 proto;
39 u8 num;
40 } vlan;
41 struct {
42 u16 sid;
43 u8 num;
44 } pppoe;
45 };
46
47 static 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
54 static int
mtk_flow_set_ipv4_addr(struct mtk_eth * eth,struct mtk_foe_entry * foe,struct mtk_flow_data * data,bool egress)55 mtk_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
63 static int
mtk_flow_set_ipv6_addr(struct mtk_eth * eth,struct mtk_foe_entry * foe,struct mtk_flow_data * data)64 mtk_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
72 static void
mtk_flow_offload_mangle_eth(const struct flow_action_entry * act,void * eth)73 mtk_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
89 static int
mtk_flow_get_wdma_info(struct net_device * dev,const u8 * addr,struct mtk_wdma_info * info)90 mtk_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
119 static int
mtk_flow_mangle_ports(const struct flow_action_entry * act,struct mtk_flow_data * data)120 mtk_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
142 static int
mtk_flow_mangle_ipv4(const struct flow_action_entry * act,struct mtk_flow_data * data)143 mtk_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
164 static int
mtk_flow_get_dsa_port(struct net_device ** dev)165 mtk_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
185 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)186 mtk_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;
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 if (dsa_port >= 0)
216 mtk_foe_entry_set_dsa(eth, foe, dsa_port);
217
218 if (dev == eth->netdev[0])
219 pse_port = 1;
220 else if (dev == eth->netdev[1])
221 pse_port = 2;
222 else
223 return -EOPNOTSUPP;
224
225 out:
226 mtk_foe_entry_set_pse_port(eth, foe, pse_port);
227
228 return 0;
229 }
230
231 static int
mtk_flow_offload_replace(struct mtk_eth * eth,struct flow_cls_offload * f)232 mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
233 {
234 struct flow_rule *rule = flow_cls_offload_flow_rule(f);
235 struct flow_action_entry *act;
236 struct mtk_flow_data data = {};
237 struct mtk_foe_entry foe;
238 struct net_device *odev = NULL;
239 struct mtk_flow_entry *entry;
240 int offload_type = 0;
241 int wed_index = -1;
242 u16 addr_type = 0;
243 u8 l4proto = 0;
244 int err = 0;
245 int i;
246
247 if (rhashtable_lookup(ð->flow_table, &f->cookie, mtk_flow_ht_params))
248 return -EEXIST;
249
250 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_META)) {
251 struct flow_match_meta match;
252
253 flow_rule_match_meta(rule, &match);
254 } else {
255 return -EOPNOTSUPP;
256 }
257
258 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_CONTROL)) {
259 struct flow_match_control match;
260
261 flow_rule_match_control(rule, &match);
262 addr_type = match.key->addr_type;
263 } else {
264 return -EOPNOTSUPP;
265 }
266
267 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) {
268 struct flow_match_basic match;
269
270 flow_rule_match_basic(rule, &match);
271 l4proto = match.key->ip_proto;
272 } else {
273 return -EOPNOTSUPP;
274 }
275
276 switch (addr_type) {
277 case 0:
278 offload_type = MTK_PPE_PKT_TYPE_BRIDGE;
279 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
280 struct flow_match_eth_addrs match;
281
282 flow_rule_match_eth_addrs(rule, &match);
283 memcpy(data.eth.h_dest, match.key->dst, ETH_ALEN);
284 memcpy(data.eth.h_source, match.key->src, ETH_ALEN);
285 } else {
286 return -EOPNOTSUPP;
287 }
288
289 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
290 struct flow_match_vlan match;
291
292 flow_rule_match_vlan(rule, &match);
293
294 if (match.key->vlan_tpid != cpu_to_be16(ETH_P_8021Q))
295 return -EOPNOTSUPP;
296
297 data.vlan_in = match.key->vlan_id;
298 }
299 break;
300 case FLOW_DISSECTOR_KEY_IPV4_ADDRS:
301 offload_type = MTK_PPE_PKT_TYPE_IPV4_HNAPT;
302 break;
303 case FLOW_DISSECTOR_KEY_IPV6_ADDRS:
304 offload_type = MTK_PPE_PKT_TYPE_IPV6_ROUTE_5T;
305 break;
306 default:
307 return -EOPNOTSUPP;
308 }
309
310 flow_action_for_each(i, act, &rule->action) {
311 switch (act->id) {
312 case FLOW_ACTION_MANGLE:
313 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
314 return -EOPNOTSUPP;
315 if (act->mangle.htype == FLOW_ACT_MANGLE_HDR_TYPE_ETH)
316 mtk_flow_offload_mangle_eth(act, &data.eth);
317 break;
318 case FLOW_ACTION_REDIRECT:
319 odev = act->dev;
320 break;
321 case FLOW_ACTION_CSUM:
322 break;
323 case FLOW_ACTION_VLAN_PUSH:
324 if (data.vlan.num == 1 ||
325 act->vlan.proto != htons(ETH_P_8021Q))
326 return -EOPNOTSUPP;
327
328 data.vlan.id = act->vlan.vid;
329 data.vlan.proto = act->vlan.proto;
330 data.vlan.num++;
331 break;
332 case FLOW_ACTION_VLAN_POP:
333 break;
334 case FLOW_ACTION_PPPOE_PUSH:
335 if (data.pppoe.num == 1)
336 return -EOPNOTSUPP;
337
338 data.pppoe.sid = act->pppoe.sid;
339 data.pppoe.num++;
340 break;
341 default:
342 return -EOPNOTSUPP;
343 }
344 }
345
346 if (!is_valid_ether_addr(data.eth.h_source) ||
347 !is_valid_ether_addr(data.eth.h_dest))
348 return -EINVAL;
349
350 err = mtk_foe_entry_prepare(eth, &foe, offload_type, l4proto, 0,
351 data.eth.h_source, data.eth.h_dest);
352 if (err)
353 return err;
354
355 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) {
356 struct flow_match_ports ports;
357
358 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
359 return -EOPNOTSUPP;
360
361 flow_rule_match_ports(rule, &ports);
362 data.src_port = ports.key->src;
363 data.dst_port = ports.key->dst;
364 } else if (offload_type != MTK_PPE_PKT_TYPE_BRIDGE) {
365 return -EOPNOTSUPP;
366 }
367
368 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
369 struct flow_match_ipv4_addrs addrs;
370
371 flow_rule_match_ipv4_addrs(rule, &addrs);
372
373 data.v4.src_addr = addrs.key->src;
374 data.v4.dst_addr = addrs.key->dst;
375
376 mtk_flow_set_ipv4_addr(eth, &foe, &data, false);
377 }
378
379 if (addr_type == FLOW_DISSECTOR_KEY_IPV6_ADDRS) {
380 struct flow_match_ipv6_addrs addrs;
381
382 flow_rule_match_ipv6_addrs(rule, &addrs);
383
384 data.v6.src_addr = addrs.key->src;
385 data.v6.dst_addr = addrs.key->dst;
386
387 mtk_flow_set_ipv6_addr(eth, &foe, &data);
388 }
389
390 flow_action_for_each(i, act, &rule->action) {
391 if (act->id != FLOW_ACTION_MANGLE)
392 continue;
393
394 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
395 return -EOPNOTSUPP;
396
397 switch (act->mangle.htype) {
398 case FLOW_ACT_MANGLE_HDR_TYPE_TCP:
399 case FLOW_ACT_MANGLE_HDR_TYPE_UDP:
400 err = mtk_flow_mangle_ports(act, &data);
401 break;
402 case FLOW_ACT_MANGLE_HDR_TYPE_IP4:
403 err = mtk_flow_mangle_ipv4(act, &data);
404 break;
405 case FLOW_ACT_MANGLE_HDR_TYPE_ETH:
406 /* handled earlier */
407 break;
408 default:
409 return -EOPNOTSUPP;
410 }
411
412 if (err)
413 return err;
414 }
415
416 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
417 err = mtk_flow_set_ipv4_addr(eth, &foe, &data, true);
418 if (err)
419 return err;
420 }
421
422 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE)
423 foe.bridge.vlan = data.vlan_in;
424
425 if (data.vlan.num == 1) {
426 if (data.vlan.proto != htons(ETH_P_8021Q))
427 return -EOPNOTSUPP;
428
429 mtk_foe_entry_set_vlan(eth, &foe, data.vlan.id);
430 }
431 if (data.pppoe.num == 1)
432 mtk_foe_entry_set_pppoe(eth, &foe, data.pppoe.sid);
433
434 err = mtk_flow_set_output_device(eth, &foe, odev, data.eth.h_dest,
435 &wed_index);
436 if (err)
437 return err;
438
439 if (wed_index >= 0 && (err = mtk_wed_flow_add(wed_index)) < 0)
440 return err;
441
442 entry = kzalloc(sizeof(*entry), GFP_KERNEL);
443 if (!entry)
444 return -ENOMEM;
445
446 entry->cookie = f->cookie;
447 memcpy(&entry->data, &foe, sizeof(entry->data));
448 entry->wed_index = wed_index;
449
450 err = mtk_foe_entry_commit(eth->ppe[entry->ppe_index], entry);
451 if (err < 0)
452 goto free;
453
454 err = rhashtable_insert_fast(ð->flow_table, &entry->node,
455 mtk_flow_ht_params);
456 if (err < 0)
457 goto clear;
458
459 return 0;
460
461 clear:
462 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry);
463 free:
464 kfree(entry);
465 if (wed_index >= 0)
466 mtk_wed_flow_remove(wed_index);
467 return err;
468 }
469
470 static int
mtk_flow_offload_destroy(struct mtk_eth * eth,struct flow_cls_offload * f)471 mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f)
472 {
473 struct mtk_flow_entry *entry;
474
475 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
476 mtk_flow_ht_params);
477 if (!entry)
478 return -ENOENT;
479
480 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry);
481 rhashtable_remove_fast(ð->flow_table, &entry->node,
482 mtk_flow_ht_params);
483 if (entry->wed_index >= 0)
484 mtk_wed_flow_remove(entry->wed_index);
485 kfree(entry);
486
487 return 0;
488 }
489
490 static int
mtk_flow_offload_stats(struct mtk_eth * eth,struct flow_cls_offload * f)491 mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f)
492 {
493 struct mtk_flow_entry *entry;
494 u32 idle;
495
496 entry = rhashtable_lookup(ð->flow_table, &f->cookie,
497 mtk_flow_ht_params);
498 if (!entry)
499 return -ENOENT;
500
501 idle = mtk_foe_entry_idle_time(eth->ppe[entry->ppe_index], entry);
502 f->stats.lastused = jiffies - idle * HZ;
503
504 return 0;
505 }
506
507 static DEFINE_MUTEX(mtk_flow_offload_mutex);
508
509 static int
mtk_eth_setup_tc_block_cb(enum tc_setup_type type,void * type_data,void * cb_priv)510 mtk_eth_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
511 {
512 struct flow_cls_offload *cls = type_data;
513 struct net_device *dev = cb_priv;
514 struct mtk_mac *mac = netdev_priv(dev);
515 struct mtk_eth *eth = mac->hw;
516 int err;
517
518 if (!tc_can_offload(dev))
519 return -EOPNOTSUPP;
520
521 if (type != TC_SETUP_CLSFLOWER)
522 return -EOPNOTSUPP;
523
524 mutex_lock(&mtk_flow_offload_mutex);
525 switch (cls->command) {
526 case FLOW_CLS_REPLACE:
527 err = mtk_flow_offload_replace(eth, cls);
528 break;
529 case FLOW_CLS_DESTROY:
530 err = mtk_flow_offload_destroy(eth, cls);
531 break;
532 case FLOW_CLS_STATS:
533 err = mtk_flow_offload_stats(eth, cls);
534 break;
535 default:
536 err = -EOPNOTSUPP;
537 break;
538 }
539 mutex_unlock(&mtk_flow_offload_mutex);
540
541 return err;
542 }
543
544 static int
mtk_eth_setup_tc_block(struct net_device * dev,struct flow_block_offload * f)545 mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f)
546 {
547 struct mtk_mac *mac = netdev_priv(dev);
548 struct mtk_eth *eth = mac->hw;
549 static LIST_HEAD(block_cb_list);
550 struct flow_block_cb *block_cb;
551 flow_setup_cb_t *cb;
552
553 if (!eth->soc->offload_version)
554 return -EOPNOTSUPP;
555
556 if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS)
557 return -EOPNOTSUPP;
558
559 cb = mtk_eth_setup_tc_block_cb;
560 f->driver_block_list = &block_cb_list;
561
562 switch (f->command) {
563 case FLOW_BLOCK_BIND:
564 block_cb = flow_block_cb_lookup(f->block, cb, dev);
565 if (block_cb) {
566 flow_block_cb_incref(block_cb);
567 return 0;
568 }
569 block_cb = flow_block_cb_alloc(cb, dev, dev, NULL);
570 if (IS_ERR(block_cb))
571 return PTR_ERR(block_cb);
572
573 flow_block_cb_add(block_cb, f);
574 list_add_tail(&block_cb->driver_list, &block_cb_list);
575 return 0;
576 case FLOW_BLOCK_UNBIND:
577 block_cb = flow_block_cb_lookup(f->block, cb, dev);
578 if (!block_cb)
579 return -ENOENT;
580
581 if (flow_block_cb_decref(block_cb)) {
582 flow_block_cb_remove(block_cb, f);
583 list_del(&block_cb->driver_list);
584 }
585 return 0;
586 default:
587 return -EOPNOTSUPP;
588 }
589 }
590
mtk_eth_setup_tc(struct net_device * dev,enum tc_setup_type type,void * type_data)591 int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
592 void *type_data)
593 {
594 switch (type) {
595 case TC_SETUP_BLOCK:
596 case TC_SETUP_FT:
597 return mtk_eth_setup_tc_block(dev, type_data);
598 default:
599 return -EOPNOTSUPP;
600 }
601 }
602
mtk_eth_offload_init(struct mtk_eth * eth)603 int mtk_eth_offload_init(struct mtk_eth *eth)
604 {
605 return rhashtable_init(ð->flow_table, &mtk_flow_ht_params);
606 }
607
