drivers/net/ethernet/mediatek/mtk_ppe_offload.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  *  Copyright (C) 2020 Felix Fietkau <[email protected]>
   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 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(&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                 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(&eth->flow_table, &entry->node,
 484                                      mtk_flow_ht_params);
 485         if (err < 0)
 486                 goto clear;
 487
 488         return 0;
 489
 490 clear:
 491         mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry);
 492 free:
 493         kfree(entry);
 494         if (wed_index >= 0)
 495             mtk_wed_flow_remove(wed_index);
 496         return err;
 497 }
 498
 499 static int
 500 mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f)
 501 {
 502         struct mtk_flow_entry *entry;
 503
 504         entry = rhashtable_lookup(&eth->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(&eth->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
 519 static int
 520 mtk_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(&eth->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
 544 static DEFINE_MUTEX(mtk_flow_offload_mutex);
 545
 546 int 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
 571 static int
 572 mtk_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
 591 static int
 592 mtk_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
 639 int 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
 651 int 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(&eth->flow_table, &mtk_flow_ht_params);
 656 }