drivers/net/ethernet/mediatek/mtk_ppe_offload.c

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