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
 115         return 0;
 116 }
 117
 118
 119 static int
 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
 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
 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
 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
 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(&eth->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(&eth->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
 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(&eth->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(&eth->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
 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(&eth->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
 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
 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
 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
 603 int mtk_eth_offload_init(struct mtk_eth *eth)
 604 {
 605         return rhashtable_init(&eth->flow_table, &mtk_flow_ht_params);
 606 }