target/linux/generic/backport-5.10/610-v5.13-33-net-ethernet-mtk_eth_soc-add-flow-offloading-support.patch

   1 From: Felix Fietkau <nbd@nbd.name>
   2 Date: Wed, 24 Mar 2021 02:30:54 +0100
   3 Subject: [PATCH] net: ethernet: mtk_eth_soc: add flow offloading support
   4
   5 This adds support for offloading IPv4 routed flows, including SNAT/DNAT,
   6 one VLAN, PPPoE and DSA.
   7
   8 Signed-off-by: Felix Fietkau <nbd@nbd.name>
   9 Signed-off-by: Pablo Neira Ayuso <pablo@netfilter.org>
  10 ---
  11  create mode 100644 drivers/net/ethernet/mediatek/mtk_ppe_offload.c
  12
  13 --- a/drivers/net/ethernet/mediatek/Makefile
  14 +++ b/drivers/net/ethernet/mediatek/Makefile
  15 @@ -4,5 +4,5 @@
  16  #
  17
  18  obj-$(CONFIG_NET_MEDIATEK_SOC) += mtk_eth.o
  19 -mtk_eth-y := mtk_eth_soc.o mtk_sgmii.o mtk_eth_path.o mtk_ppe.o mtk_ppe_debugfs.o
  20 +mtk_eth-y := mtk_eth_soc.o mtk_sgmii.o mtk_eth_path.o mtk_ppe.o mtk_ppe_debugfs.o mtk_ppe_offload.o
  21  obj-$(CONFIG_NET_MEDIATEK_STAR_EMAC) += mtk_star_emac.o
  22 --- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
  23 +++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
  24 @@ -2854,6 +2854,7 @@ static const struct net_device_ops mtk_n
  25  #ifdef CONFIG_NET_POLL_CONTROLLER
  26         .ndo_poll_controller    = mtk_poll_controller,
  27  #endif
  28 +       .ndo_setup_tc           = mtk_eth_setup_tc,
  29  };
  30
  31  static int mtk_add_mac(struct mtk_eth *eth, struct device_node *np)
  32 @@ -3112,6 +3113,10 @@ static int mtk_probe(struct platform_dev
  33                                    eth->base + MTK_ETH_PPE_BASE, 2);
  34                 if (err)
  35                         goto err_free_dev;
  36 +
  37 +               err = mtk_eth_offload_init(eth);
  38 +               if (err)
  39 +                       goto err_free_dev;
  40         }
  41
  42         for (i = 0; i < MTK_MAX_DEVS; i++) {
  43 --- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
  44 +++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
  45 @@ -15,6 +15,7 @@
  46  #include <linux/u64_stats_sync.h>
  47  #include <linux/refcount.h>
  48  #include <linux/phylink.h>
  49 +#include <linux/rhashtable.h>
  50  #include "mtk_ppe.h"
  51
  52  #define MTK_QDMA_PAGE_SIZE     2048
  53 @@ -40,7 +41,8 @@
  54                                  NETIF_F_HW_VLAN_CTAG_RX | \
  55                                  NETIF_F_SG | NETIF_F_TSO | \
  56                                  NETIF_F_TSO6 | \
  57 -                                NETIF_F_IPV6_CSUM)
  58 +                                NETIF_F_IPV6_CSUM |\
  59 +                                NETIF_F_HW_TC)
  60  #define MTK_HW_FEATURES_MT7628 (NETIF_F_SG | NETIF_F_RXCSUM)
  61  #define NEXT_DESP_IDX(X, Y)    (((X) + 1) & ((Y) - 1))
  62
  63 @@ -929,6 +931,7 @@ struct mtk_eth {
  64         int                             ip_align;
  65
  66         struct mtk_ppe                  ppe;
  67 +       struct rhashtable               flow_table;
  68  };
  69
  70  /* struct mtk_mac -    the structure that holds the info about the MACs of the
  71 @@ -973,4 +976,9 @@ int mtk_gmac_sgmii_path_setup(struct mtk
  72  int mtk_gmac_gephy_path_setup(struct mtk_eth *eth, int mac_id);
  73  int mtk_gmac_rgmii_path_setup(struct mtk_eth *eth, int mac_id);
  74
  75 +int mtk_eth_offload_init(struct mtk_eth *eth);
  76 +int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
  77 +                    void *type_data);
  78 +
  79 +
  80  #endif /* MTK_ETH_H */
  81 --- /dev/null
  82 +++ b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
  83 @@ -0,0 +1,485 @@
  84 +// SPDX-License-Identifier: GPL-2.0-only
  85 +/*
  86 + *  Copyright (C) 2020 Felix Fietkau <nbd@nbd.name>
  87 + */
  88 +
  89 +#include <linux/if_ether.h>
  90 +#include <linux/rhashtable.h>
  91 +#include <linux/if_ether.h>
  92 +#include <linux/ip.h>
  93 +#include <net/flow_offload.h>
  94 +#include <net/pkt_cls.h>
  95 +#include <net/dsa.h>
  96 +#include "mtk_eth_soc.h"
  97 +
  98 +struct mtk_flow_data {
  99 +       struct ethhdr eth;
 100 +
 101 +       union {
 102 +               struct {
 103 +                       __be32 src_addr;
 104 +                       __be32 dst_addr;
 105 +               } v4;
 106 +       };
 107 +
 108 +       __be16 src_port;
 109 +       __be16 dst_port;
 110 +
 111 +       struct {
 112 +               u16 id;
 113 +               __be16 proto;
 114 +               u8 num;
 115 +       } vlan;
 116 +       struct {
 117 +               u16 sid;
 118 +               u8 num;
 119 +       } pppoe;
 120 +};
 121 +
 122 +struct mtk_flow_entry {
 123 +       struct rhash_head node;
 124 +       unsigned long cookie;
 125 +       u16 hash;
 126 +};
 127 +
 128 +static const struct rhashtable_params mtk_flow_ht_params = {
 129 +       .head_offset = offsetof(struct mtk_flow_entry, node),
 130 +       .head_offset = offsetof(struct mtk_flow_entry, cookie),
 131 +       .key_len = sizeof(unsigned long),
 132 +       .automatic_shrinking = true,
 133 +};
 134 +
 135 +static u32
 136 +mtk_eth_timestamp(struct mtk_eth *eth)
 137 +{
 138 +       return mtk_r32(eth, 0x0010) & MTK_FOE_IB1_BIND_TIMESTAMP;
 139 +}
 140 +
 141 +static int
 142 +mtk_flow_set_ipv4_addr(struct mtk_foe_entry *foe, struct mtk_flow_data *data,
 143 +                      bool egress)
 144 +{
 145 +       return mtk_foe_entry_set_ipv4_tuple(foe, egress,
 146 +                                           data->v4.src_addr, data->src_port,
 147 +                                           data->v4.dst_addr, data->dst_port);
 148 +}
 149 +
 150 +static void
 151 +mtk_flow_offload_mangle_eth(const struct flow_action_entry *act, void *eth)
 152 +{
 153 +       void *dest = eth + act->mangle.offset;
 154 +       const void *src = &act->mangle.val;
 155 +
 156 +       if (act->mangle.offset > 8)
 157 +               return;
 158 +
 159 +       if (act->mangle.mask == 0xffff) {
 160 +               src += 2;
 161 +               dest += 2;
 162 +       }
 163 +
 164 +       memcpy(dest, src, act->mangle.mask ? 2 : 4);
 165 +}
 166 +
 167 +
 168 +static int
 169 +mtk_flow_mangle_ports(const struct flow_action_entry *act,
 170 +                     struct mtk_flow_data *data)
 171 +{
 172 +       u32 val = ntohl(act->mangle.val);
 173 +
 174 +       switch (act->mangle.offset) {
 175 +       case 0:
 176 +               if (act->mangle.mask == ~htonl(0xffff))
 177 +                       data->dst_port = cpu_to_be16(val);
 178 +               else
 179 +                       data->src_port = cpu_to_be16(val >> 16);
 180 +               break;
 181 +       case 2:
 182 +               data->dst_port = cpu_to_be16(val);
 183 +               break;
 184 +       default:
 185 +               return -EINVAL;
 186 +       }
 187 +
 188 +       return 0;
 189 +}
 190 +
 191 +static int
 192 +mtk_flow_mangle_ipv4(const struct flow_action_entry *act,
 193 +                    struct mtk_flow_data *data)
 194 +{
 195 +       __be32 *dest;
 196 +
 197 +       switch (act->mangle.offset) {
 198 +       case offsetof(struct iphdr, saddr):
 199 +               dest = &data->v4.src_addr;
 200 +               break;
 201 +       case offsetof(struct iphdr, daddr):
 202 +               dest = &data->v4.dst_addr;
 203 +               break;
 204 +       default:
 205 +               return -EINVAL;
 206 +       }
 207 +
 208 +       memcpy(dest, &act->mangle.val, sizeof(u32));
 209 +
 210 +       return 0;
 211 +}
 212 +
 213 +static int
 214 +mtk_flow_get_dsa_port(struct net_device **dev)
 215 +{
 216 +#if IS_ENABLED(CONFIG_NET_DSA)
 217 +       struct dsa_port *dp;
 218 +
 219 +       dp = dsa_port_from_netdev(*dev);
 220 +       if (IS_ERR(dp))
 221 +               return -ENODEV;
 222 +
 223 +       if (dp->cpu_dp->tag_ops->proto != DSA_TAG_PROTO_MTK)
 224 +               return -ENODEV;
 225 +
 226 +       *dev = dp->cpu_dp->master;
 227 +
 228 +       return dp->index;
 229 +#else
 230 +       return -ENODEV;
 231 +#endif
 232 +}
 233 +
 234 +static int
 235 +mtk_flow_set_output_device(struct mtk_eth *eth, struct mtk_foe_entry *foe,
 236 +                          struct net_device *dev)
 237 +{
 238 +       int pse_port, dsa_port;
 239 +
 240 +       dsa_port = mtk_flow_get_dsa_port(&dev);
 241 +       if (dsa_port >= 0)
 242 +               mtk_foe_entry_set_dsa(foe, dsa_port);
 243 +
 244 +       if (dev == eth->netdev[0])
 245 +               pse_port = 1;
 246 +       else if (dev == eth->netdev[1])
 247 +               pse_port = 2;
 248 +       else
 249 +               return -EOPNOTSUPP;
 250 +
 251 +       mtk_foe_entry_set_pse_port(foe, pse_port);
 252 +
 253 +       return 0;
 254 +}
 255 +
 256 +static int
 257 +mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
 258 +{
 259 +       struct flow_rule *rule = flow_cls_offload_flow_rule(f);
 260 +       struct flow_action_entry *act;
 261 +       struct mtk_flow_data data = {};
 262 +       struct mtk_foe_entry foe;
 263 +       struct net_device *odev = NULL;
 264 +       struct mtk_flow_entry *entry;
 265 +       int offload_type = 0;
 266 +       u16 addr_type = 0;
 267 +       u32 timestamp;
 268 +       u8 l4proto = 0;
 269 +       int err = 0;
 270 +       int hash;
 271 +       int i;
 272 +
 273 +       if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_META)) {
 274 +               struct flow_match_meta match;
 275 +
 276 +               flow_rule_match_meta(rule, &match);
 277 +       } else {
 278 +               return -EOPNOTSUPP;
 279 +       }
 280 +
 281 +       if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_CONTROL)) {
 282 +               struct flow_match_control match;
 283 +
 284 +               flow_rule_match_control(rule, &match);
 285 +               addr_type = match.key->addr_type;
 286 +       } else {
 287 +               return -EOPNOTSUPP;
 288 +       }
 289 +
 290 +       if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) {
 291 +               struct flow_match_basic match;
 292 +
 293 +               flow_rule_match_basic(rule, &match);
 294 +               l4proto = match.key->ip_proto;
 295 +       } else {
 296 +               return -EOPNOTSUPP;
 297 +       }
 298 +
 299 +       flow_action_for_each(i, act, &rule->action) {
 300 +               switch (act->id) {
 301 +               case FLOW_ACTION_MANGLE:
 302 +                       if (act->mangle.htype == FLOW_ACT_MANGLE_HDR_TYPE_ETH)
 303 +                               mtk_flow_offload_mangle_eth(act, &data.eth);
 304 +                       break;
 305 +               case FLOW_ACTION_REDIRECT:
 306 +                       odev = act->dev;
 307 +                       break;
 308 +               case FLOW_ACTION_CSUM:
 309 +                       break;
 310 +               case FLOW_ACTION_VLAN_PUSH:
 311 +                       if (data.vlan.num == 1 ||
 312 +                           act->vlan.proto != htons(ETH_P_8021Q))
 313 +                               return -EOPNOTSUPP;
 314 +
 315 +                       data.vlan.id = act->vlan.vid;
 316 +                       data.vlan.proto = act->vlan.proto;
 317 +                       data.vlan.num++;
 318 +                       break;
 319 +               case FLOW_ACTION_PPPOE_PUSH:
 320 +                       if (data.pppoe.num == 1)
 321 +                               return -EOPNOTSUPP;
 322 +
 323 +                       data.pppoe.sid = act->pppoe.sid;
 324 +                       data.pppoe.num++;
 325 +                       break;
 326 +               default:
 327 +                       return -EOPNOTSUPP;
 328 +               }
 329 +       }
 330 +
 331 +       switch (addr_type) {
 332 +       case FLOW_DISSECTOR_KEY_IPV4_ADDRS:
 333 +               offload_type = MTK_PPE_PKT_TYPE_IPV4_HNAPT;
 334 +               break;
 335 +       default:
 336 +               return -EOPNOTSUPP;
 337 +       }
 338 +
 339 +       if (!is_valid_ether_addr(data.eth.h_source) ||
 340 +           !is_valid_ether_addr(data.eth.h_dest))
 341 +               return -EINVAL;
 342 +
 343 +       err = mtk_foe_entry_prepare(&foe, offload_type, l4proto, 0,
 344 +                                   data.eth.h_source,
 345 +                                   data.eth.h_dest);
 346 +       if (err)
 347 +               return err;
 348 +
 349 +       if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) {
 350 +               struct flow_match_ports ports;
 351 +
 352 +               flow_rule_match_ports(rule, &ports);
 353 +               data.src_port = ports.key->src;
 354 +               data.dst_port = ports.key->dst;
 355 +       } else {
 356 +               return -EOPNOTSUPP;
 357 +       }
 358 +
 359 +       if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
 360 +               struct flow_match_ipv4_addrs addrs;
 361 +
 362 +               flow_rule_match_ipv4_addrs(rule, &addrs);
 363 +
 364 +               data.v4.src_addr = addrs.key->src;
 365 +               data.v4.dst_addr = addrs.key->dst;
 366 +
 367 +               mtk_flow_set_ipv4_addr(&foe, &data, false);
 368 +       }
 369 +
 370 +       flow_action_for_each(i, act, &rule->action) {
 371 +               if (act->id != FLOW_ACTION_MANGLE)
 372 +                       continue;
 373 +
 374 +               switch (act->mangle.htype) {
 375 +               case FLOW_ACT_MANGLE_HDR_TYPE_TCP:
 376 +               case FLOW_ACT_MANGLE_HDR_TYPE_UDP:
 377 +                       err = mtk_flow_mangle_ports(act, &data);
 378 +                       break;
 379 +               case FLOW_ACT_MANGLE_HDR_TYPE_IP4:
 380 +                       err = mtk_flow_mangle_ipv4(act, &data);
 381 +                       break;
 382 +               case FLOW_ACT_MANGLE_HDR_TYPE_ETH:
 383 +                       /* handled earlier */
 384 +                       break;
 385 +               default:
 386 +                       return -EOPNOTSUPP;
 387 +               }
 388 +
 389 +               if (err)
 390 +                       return err;
 391 +       }
 392 +
 393 +       if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
 394 +               err = mtk_flow_set_ipv4_addr(&foe, &data, true);
 395 +               if (err)
 396 +                       return err;
 397 +       }
 398 +
 399 +       if (data.vlan.num == 1) {
 400 +               if (data.vlan.proto != htons(ETH_P_8021Q))
 401 +                       return -EOPNOTSUPP;
 402 +
 403 +               mtk_foe_entry_set_vlan(&foe, data.vlan.id);
 404 +       }
 405 +       if (data.pppoe.num == 1)
 406 +               mtk_foe_entry_set_pppoe(&foe, data.pppoe.sid);
 407 +
 408 +       err = mtk_flow_set_output_device(eth, &foe, odev);
 409 +       if (err)
 410 +               return err;
 411 +
 412 +       entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 413 +       if (!entry)
 414 +               return -ENOMEM;
 415 +
 416 +       entry->cookie = f->cookie;
 417 +       timestamp = mtk_eth_timestamp(eth);
 418 +       hash = mtk_foe_entry_commit(&eth->ppe, &foe, timestamp);
 419 +       if (hash < 0) {
 420 +               err = hash;
 421 +               goto free;
 422 +       }
 423 +
 424 +       entry->hash = hash;
 425 +       err = rhashtable_insert_fast(&eth->flow_table, &entry->node,
 426 +                                    mtk_flow_ht_params);
 427 +       if (err < 0)
 428 +               goto clear_flow;
 429 +
 430 +       return 0;
 431 +clear_flow:
 432 +       mtk_foe_entry_clear(&eth->ppe, hash);
 433 +free:
 434 +       kfree(entry);
 435 +       return err;
 436 +}
 437 +
 438 +static int
 439 +mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f)
 440 +{
 441 +       struct mtk_flow_entry *entry;
 442 +
 443 +       entry = rhashtable_lookup(&eth->flow_table, &f->cookie,
 444 +                                 mtk_flow_ht_params);
 445 +       if (!entry)
 446 +               return -ENOENT;
 447 +
 448 +       mtk_foe_entry_clear(&eth->ppe, entry->hash);
 449 +       rhashtable_remove_fast(&eth->flow_table, &entry->node,
 450 +                              mtk_flow_ht_params);
 451 +       kfree(entry);
 452 +
 453 +       return 0;
 454 +}
 455 +
 456 +static int
 457 +mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f)
 458 +{
 459 +       struct mtk_flow_entry *entry;
 460 +       int timestamp;
 461 +       u32 idle;
 462 +
 463 +       entry = rhashtable_lookup(&eth->flow_table, &f->cookie,
 464 +                                 mtk_flow_ht_params);
 465 +       if (!entry)
 466 +               return -ENOENT;
 467 +
 468 +       timestamp = mtk_foe_entry_timestamp(&eth->ppe, entry->hash);
 469 +       if (timestamp < 0)
 470 +               return -ETIMEDOUT;
 471 +
 472 +       idle = mtk_eth_timestamp(eth) - timestamp;
 473 +       f->stats.lastused = jiffies - idle * HZ;
 474 +
 475 +       return 0;
 476 +}
 477 +
 478 +static int
 479 +mtk_eth_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
 480 +{
 481 +       struct flow_cls_offload *cls = type_data;
 482 +       struct net_device *dev = cb_priv;
 483 +       struct mtk_mac *mac = netdev_priv(dev);
 484 +       struct mtk_eth *eth = mac->hw;
 485 +
 486 +       if (!tc_can_offload(dev))
 487 +               return -EOPNOTSUPP;
 488 +
 489 +       if (type != TC_SETUP_CLSFLOWER)
 490 +               return -EOPNOTSUPP;
 491 +
 492 +       switch (cls->command) {
 493 +       case FLOW_CLS_REPLACE:
 494 +               return mtk_flow_offload_replace(eth, cls);
 495 +       case FLOW_CLS_DESTROY:
 496 +               return mtk_flow_offload_destroy(eth, cls);
 497 +       case FLOW_CLS_STATS:
 498 +               return mtk_flow_offload_stats(eth, cls);
 499 +       default:
 500 +               return -EOPNOTSUPP;
 501 +       }
 502 +
 503 +       return 0;
 504 +}
 505 +
 506 +static int
 507 +mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f)
 508 +{
 509 +       struct mtk_mac *mac = netdev_priv(dev);
 510 +       struct mtk_eth *eth = mac->hw;
 511 +       static LIST_HEAD(block_cb_list);
 512 +       struct flow_block_cb *block_cb;
 513 +       flow_setup_cb_t *cb;
 514 +
 515 +       if (!eth->ppe.foe_table)
 516 +               return -EOPNOTSUPP;
 517 +
 518 +       if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS)
 519 +               return -EOPNOTSUPP;
 520 +
 521 +       cb = mtk_eth_setup_tc_block_cb;
 522 +       f->driver_block_list = &block_cb_list;
 523 +
 524 +       switch (f->command) {
 525 +       case FLOW_BLOCK_BIND:
 526 +               block_cb = flow_block_cb_lookup(f->block, cb, dev);
 527 +               if (block_cb) {
 528 +                       flow_block_cb_incref(block_cb);
 529 +                       return 0;
 530 +               }
 531 +               block_cb = flow_block_cb_alloc(cb, dev, dev, NULL);
 532 +               if (IS_ERR(block_cb))
 533 +                       return PTR_ERR(block_cb);
 534 +
 535 +               flow_block_cb_add(block_cb, f);
 536 +               list_add_tail(&block_cb->driver_list, &block_cb_list);
 537 +               return 0;
 538 +       case FLOW_BLOCK_UNBIND:
 539 +               block_cb = flow_block_cb_lookup(f->block, cb, dev);
 540 +               if (!block_cb)
 541 +                       return -ENOENT;
 542 +
 543 +               if (flow_block_cb_decref(block_cb)) {
 544 +                       flow_block_cb_remove(block_cb, f);
 545 +                       list_del(&block_cb->driver_list);
 546 +               }
 547 +               return 0;
 548 +       default:
 549 +               return -EOPNOTSUPP;
 550 +       }
 551 +}
 552 +
 553 +int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
 554 +                    void *type_data)
 555 +{
 556 +       if (type == TC_SETUP_FT)
 557 +               return mtk_eth_setup_tc_block(dev, type_data);
 558 +
 559 +       return -EOPNOTSUPP;
 560 +}
 561 +
 562 +int mtk_eth_offload_init(struct mtk_eth *eth)
 563 +{
 564 +       if (!eth->ppe.foe_table)
 565 +               return 0;
 566 +
 567 +       return rhashtable_init(&eth->flow_table, &mtk_flow_ht_params);
 568 +}