657 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*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 int55mtk_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 int64mtk_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 void73mtk_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 int90mtk_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 int121mtk_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 else131 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 int144mtk_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 int166mtk_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#else182 return -ENODEV;183#endif184}185 186static int187mtk_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 else227 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 int244mtk_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 int500mtk_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 int520mtk_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 int572mtk_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 int592mtk_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}657