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 struct { 38 u16 id; 39 __be16 proto; 40 } vlans[2]; 41 u8 num; 42 } vlan; 43 struct { 44 u16 sid; 45 u8 num; 46 } pppoe; 47 }; 48 49 static const struct rhashtable_params mtk_flow_ht_params = { 50 .head_offset = offsetof(struct mtk_flow_entry, node), 51 .key_offset = offsetof(struct mtk_flow_entry, cookie), 52 .key_len = sizeof(unsigned long), 53 .automatic_shrinking = true, 54 }; 55 56 static int 57 mtk_flow_set_ipv4_addr(struct mtk_eth *eth, struct mtk_foe_entry *foe, 58 struct mtk_flow_data *data, bool egress) 59 { 60 return mtk_foe_entry_set_ipv4_tuple(eth, foe, egress, 61 data->v4.src_addr, data->src_port, 62 data->v4.dst_addr, data->dst_port); 63 } 64 65 static int 66 mtk_flow_set_ipv6_addr(struct mtk_eth *eth, struct mtk_foe_entry *foe, 67 struct mtk_flow_data *data) 68 { 69 return mtk_foe_entry_set_ipv6_tuple(eth, foe, 70 data->v6.src_addr.s6_addr32, data->src_port, 71 data->v6.dst_addr.s6_addr32, data->dst_port); 72 } 73 74 static void 75 mtk_flow_offload_mangle_eth(const struct flow_action_entry *act, void *eth) 76 { 77 void *dest = eth + act->mangle.offset; 78 const void *src = &act->mangle.val; 79 80 if (act->mangle.offset > 8) 81 return; 82 83 if (act->mangle.mask == 0xffff) { 84 src += 2; 85 dest += 2; 86 } 87 88 memcpy(dest, src, act->mangle.mask ? 2 : 4); 89 } 90 91 static int 92 mtk_flow_get_wdma_info(struct net_device *dev, const u8 *addr, struct mtk_wdma_info *info) 93 { 94 struct net_device_path_stack stack; 95 struct net_device_path_ctx ctx = { 96 .dev = dev, 97 }; 98 struct net_device_path *path; 99 int err; 100 101 if (!dev) 102 return -ENODEV; 103 104 if (!IS_ENABLED(CONFIG_NET_MEDIATEK_SOC_WED)) 105 return -1; 106 107 ether_addr_copy(ctx.daddr, addr); 108 109 rcu_read_lock(); 110 err = dev_fill_forward_path(&ctx, &stack); 111 rcu_read_unlock(); 112 if (err) 113 return err; 114 115 path = &stack.path[stack.num_paths - 1]; 116 if (path->type != DEV_PATH_MTK_WDMA) { 117 err = -EINVAL; 118 goto err_out; 119 } 120 121 info->wdma_idx = path->mtk_wdma.wdma_idx; 122 info->queue = path->mtk_wdma.queue; 123 info->bss = path->mtk_wdma.bss; 124 info->wcid = path->mtk_wdma.wcid; 125 info->amsdu = path->mtk_wdma.amsdu; 126 err_out: 127 dev_fill_forward_path_release(&stack); 128 129 return err; 130 } 131 132 133 static int 134 mtk_flow_mangle_ports(const struct flow_action_entry *act, 135 struct mtk_flow_data *data) 136 { 137 u32 val = ntohl(act->mangle.val); 138 139 switch (act->mangle.offset) { 140 case 0: 141 if (act->mangle.mask == ~htonl(0xffff)) 142 data->dst_port = cpu_to_be16(val); 143 else 144 data->src_port = cpu_to_be16(val >> 16); 145 break; 146 case 2: 147 data->dst_port = cpu_to_be16(val); 148 break; 149 default: 150 return -EINVAL; 151 } 152 153 return 0; 154 } 155 156 static int 157 mtk_flow_mangle_ipv4(const struct flow_action_entry *act, 158 struct mtk_flow_data *data) 159 { 160 __be32 *dest; 161 162 switch (act->mangle.offset) { 163 case offsetof(struct iphdr, saddr): 164 dest = &data->v4.src_addr; 165 break; 166 case offsetof(struct iphdr, daddr): 167 dest = &data->v4.dst_addr; 168 break; 169 default: 170 return -EINVAL; 171 } 172 173 memcpy(dest, &act->mangle.val, sizeof(u32)); 174 175 return 0; 176 } 177 178 static int 179 mtk_flow_get_dsa_port(struct net_device **dev) 180 { 181 #if IS_ENABLED(CONFIG_NET_DSA) 182 struct dsa_port *dp; 183 184 dp = dsa_port_from_netdev(*dev); 185 if (IS_ERR(dp)) 186 return -ENODEV; 187 188 if (dp->cpu_dp->tag_ops->proto != DSA_TAG_PROTO_MTK) 189 return -ENODEV; 190 191 *dev = dsa_port_to_conduit(dp); 192 193 return dp->index; 194 #else 195 return -ENODEV; 196 #endif 197 } 198 199 static int 200 mtk_flow_set_output_device(struct mtk_eth *eth, struct mtk_foe_entry *foe, 201 struct net_device *dev, const u8 *dest_mac, 202 int *wed_index) 203 { 204 struct mtk_wdma_info info = {}; 205 int pse_port, dsa_port, queue; 206 207 if (mtk_flow_get_wdma_info(dev, dest_mac, &info) == 0) { 208 mtk_foe_entry_set_wdma(eth, foe, info.wdma_idx, info.queue, 209 info.bss, info.wcid, info.amsdu); 210 if (mtk_is_netsys_v2_or_greater(eth)) { 211 switch (info.wdma_idx) { 212 case 0: 213 pse_port = PSE_WDMA0_PORT; 214 break; 215 case 1: 216 pse_port = PSE_WDMA1_PORT; 217 break; 218 case 2: 219 pse_port = PSE_WDMA2_PORT; 220 break; 221 default: 222 return -EINVAL; 223 } 224 } else { 225 pse_port = 3; 226 } 227 *wed_index = info.wdma_idx; 228 goto out; 229 } 230 231 dsa_port = mtk_flow_get_dsa_port(&dev); 232 233 if (dev == eth->netdev[0]) 234 pse_port = PSE_GDM1_PORT; 235 else if (dev == eth->netdev[1]) 236 pse_port = PSE_GDM2_PORT; 237 else if (dev == eth->netdev[2]) 238 pse_port = PSE_GDM3_PORT; 239 else 240 return -EOPNOTSUPP; 241 242 if (dsa_port >= 0) { 243 mtk_foe_entry_set_dsa(eth, foe, dsa_port); 244 queue = 3 + dsa_port; 245 } else { 246 queue = pse_port - 1; 247 } 248 mtk_foe_entry_set_queue(eth, foe, queue); 249 250 out: 251 mtk_foe_entry_set_pse_port(eth, foe, pse_port); 252 253 return 0; 254 } 255 256 static bool 257 mtk_flow_is_valid_idev(const struct mtk_eth *eth, const struct net_device *idev) 258 { 259 size_t i; 260 261 if (!idev) 262 return false; 263 264 for (i = 0; i < ARRAY_SIZE(eth->netdev); i++) { 265 if (!eth->netdev[i]) 266 continue; 267 268 if (idev->netdev_ops == eth->netdev[i]->netdev_ops) 269 return true; 270 } 271 272 return false; 273 } 274 275 static int 276 mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f, 277 int ppe_index) 278 { 279 struct flow_rule *rule = flow_cls_offload_flow_rule(f); 280 struct net_device *idev = NULL, *odev = NULL; 281 struct flow_action_entry *act; 282 struct mtk_flow_data data = {}; 283 struct mtk_foe_entry foe; 284 struct mtk_flow_entry *entry; 285 int offload_type = 0; 286 int wed_index = -1; 287 u16 addr_type = 0; 288 u8 l4proto = 0; 289 int err = 0; 290 int i; 291 292 if (rhashtable_lookup(ð->flow_table, &f->cookie, mtk_flow_ht_params)) 293 return -EEXIST; 294 295 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_META)) { 296 struct flow_match_meta match; 297 298 flow_rule_match_meta(rule, &match); 299 if (mtk_is_netsys_v2_or_greater(eth)) { 300 idev = __dev_get_by_index(&init_net, match.key->ingress_ifindex); 301 if (mtk_flow_is_valid_idev(eth, idev)) { 302 struct mtk_mac *mac = netdev_priv(idev); 303 304 if (WARN_ON(mac->ppe_idx >= eth->soc->ppe_num)) 305 return -EINVAL; 306 307 ppe_index = mac->ppe_idx; 308 } 309 } 310 } else { 311 return -EOPNOTSUPP; 312 } 313 314 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_CONTROL)) { 315 struct flow_match_control match; 316 317 flow_rule_match_control(rule, &match); 318 addr_type = match.key->addr_type; 319 320 if (flow_rule_has_control_flags(match.mask->flags, 321 f->common.extack)) 322 return -EOPNOTSUPP; 323 } else { 324 return -EOPNOTSUPP; 325 } 326 327 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) { 328 struct flow_match_basic match; 329 330 flow_rule_match_basic(rule, &match); 331 l4proto = match.key->ip_proto; 332 } else { 333 return -EOPNOTSUPP; 334 } 335 336 switch (addr_type) { 337 case 0: 338 offload_type = MTK_PPE_PKT_TYPE_BRIDGE; 339 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) { 340 struct flow_match_eth_addrs match; 341 342 flow_rule_match_eth_addrs(rule, &match); 343 memcpy(data.eth.h_dest, match.key->dst, ETH_ALEN); 344 memcpy(data.eth.h_source, match.key->src, ETH_ALEN); 345 } else { 346 return -EOPNOTSUPP; 347 } 348 349 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) { 350 struct flow_match_vlan match; 351 352 flow_rule_match_vlan(rule, &match); 353 354 if (match.key->vlan_tpid != cpu_to_be16(ETH_P_8021Q)) 355 return -EOPNOTSUPP; 356 357 data.vlan_in = match.key->vlan_id; 358 } 359 break; 360 case FLOW_DISSECTOR_KEY_IPV4_ADDRS: 361 offload_type = MTK_PPE_PKT_TYPE_IPV4_HNAPT; 362 break; 363 case FLOW_DISSECTOR_KEY_IPV6_ADDRS: 364 offload_type = MTK_PPE_PKT_TYPE_IPV6_ROUTE_5T; 365 break; 366 default: 367 return -EOPNOTSUPP; 368 } 369 370 flow_action_for_each(i, act, &rule->action) { 371 switch (act->id) { 372 case FLOW_ACTION_MANGLE: 373 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE) 374 return -EOPNOTSUPP; 375 if (act->mangle.htype == FLOW_ACT_MANGLE_HDR_TYPE_ETH) 376 mtk_flow_offload_mangle_eth(act, &data.eth); 377 break; 378 case FLOW_ACTION_REDIRECT: 379 odev = act->dev; 380 break; 381 case FLOW_ACTION_CSUM: 382 break; 383 case FLOW_ACTION_VLAN_PUSH: 384 if (data.vlan.num + data.pppoe.num == 2 || 385 act->vlan.proto != htons(ETH_P_8021Q)) 386 return -EOPNOTSUPP; 387 388 data.vlan.vlans[data.vlan.num].id = act->vlan.vid; 389 data.vlan.vlans[data.vlan.num].proto = act->vlan.proto; 390 data.vlan.num++; 391 break; 392 case FLOW_ACTION_VLAN_POP: 393 break; 394 case FLOW_ACTION_PPPOE_PUSH: 395 if (data.pppoe.num == 1 || 396 data.vlan.num == 2) 397 return -EOPNOTSUPP; 398 399 data.pppoe.sid = act->pppoe.sid; 400 data.pppoe.num++; 401 break; 402 default: 403 return -EOPNOTSUPP; 404 } 405 } 406 407 if (!is_valid_ether_addr(data.eth.h_source) || 408 !is_valid_ether_addr(data.eth.h_dest)) 409 return -EINVAL; 410 411 err = mtk_foe_entry_prepare(eth, &foe, offload_type, l4proto, 0, 412 data.eth.h_source, data.eth.h_dest); 413 if (err) 414 return err; 415 416 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) { 417 struct flow_match_ports ports; 418 419 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE) 420 return -EOPNOTSUPP; 421 422 flow_rule_match_ports(rule, &ports); 423 data.src_port = ports.key->src; 424 data.dst_port = ports.key->dst; 425 } else if (offload_type != MTK_PPE_PKT_TYPE_BRIDGE) { 426 return -EOPNOTSUPP; 427 } 428 429 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) { 430 struct flow_match_ipv4_addrs addrs; 431 432 flow_rule_match_ipv4_addrs(rule, &addrs); 433 434 data.v4.src_addr = addrs.key->src; 435 data.v4.dst_addr = addrs.key->dst; 436 437 mtk_flow_set_ipv4_addr(eth, &foe, &data, false); 438 } 439 440 if (addr_type == FLOW_DISSECTOR_KEY_IPV6_ADDRS) { 441 struct flow_match_ipv6_addrs addrs; 442 443 flow_rule_match_ipv6_addrs(rule, &addrs); 444 445 data.v6.src_addr = addrs.key->src; 446 data.v6.dst_addr = addrs.key->dst; 447 448 mtk_flow_set_ipv6_addr(eth, &foe, &data); 449 } 450 451 flow_action_for_each(i, act, &rule->action) { 452 if (act->id != FLOW_ACTION_MANGLE) 453 continue; 454 455 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE) 456 return -EOPNOTSUPP; 457 458 switch (act->mangle.htype) { 459 case FLOW_ACT_MANGLE_HDR_TYPE_TCP: 460 case FLOW_ACT_MANGLE_HDR_TYPE_UDP: 461 err = mtk_flow_mangle_ports(act, &data); 462 break; 463 case FLOW_ACT_MANGLE_HDR_TYPE_IP4: 464 err = mtk_flow_mangle_ipv4(act, &data); 465 break; 466 case FLOW_ACT_MANGLE_HDR_TYPE_ETH: 467 /* handled earlier */ 468 break; 469 default: 470 return -EOPNOTSUPP; 471 } 472 473 if (err) 474 return err; 475 } 476 477 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) { 478 err = mtk_flow_set_ipv4_addr(eth, &foe, &data, true); 479 if (err) 480 return err; 481 } 482 483 if (offload_type == MTK_PPE_PKT_TYPE_BRIDGE) 484 foe.bridge.vlan = data.vlan_in; 485 486 for (i = 0; i < data.vlan.num; i++) 487 mtk_foe_entry_set_vlan(eth, &foe, data.vlan.vlans[i].id); 488 489 if (data.pppoe.num == 1) 490 mtk_foe_entry_set_pppoe(eth, &foe, data.pppoe.sid); 491 492 err = mtk_flow_set_output_device(eth, &foe, odev, data.eth.h_dest, 493 &wed_index); 494 if (err) 495 return err; 496 497 if (wed_index >= 0 && (err = mtk_wed_flow_add(wed_index)) < 0) 498 return err; 499 500 entry = kzalloc_obj(*entry); 501 if (!entry) 502 return -ENOMEM; 503 504 entry->cookie = f->cookie; 505 memcpy(&entry->data, &foe, sizeof(entry->data)); 506 entry->wed_index = wed_index; 507 entry->ppe_index = ppe_index; 508 509 err = mtk_foe_entry_commit(eth->ppe[entry->ppe_index], entry); 510 if (err < 0) 511 goto free; 512 513 err = rhashtable_insert_fast(ð->flow_table, &entry->node, 514 mtk_flow_ht_params); 515 if (err < 0) 516 goto clear; 517 518 return 0; 519 520 clear: 521 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry); 522 free: 523 kfree(entry); 524 if (wed_index >= 0) 525 mtk_wed_flow_remove(wed_index); 526 return err; 527 } 528 529 static int 530 mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f) 531 { 532 struct mtk_flow_entry *entry; 533 534 entry = rhashtable_lookup(ð->flow_table, &f->cookie, 535 mtk_flow_ht_params); 536 if (!entry) 537 return -ENOENT; 538 539 mtk_foe_entry_clear(eth->ppe[entry->ppe_index], entry); 540 rhashtable_remove_fast(ð->flow_table, &entry->node, 541 mtk_flow_ht_params); 542 if (entry->wed_index >= 0) 543 mtk_wed_flow_remove(entry->wed_index); 544 kfree(entry); 545 546 return 0; 547 } 548 549 static int 550 mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f) 551 { 552 struct mtk_flow_entry *entry; 553 struct mtk_foe_accounting diff; 554 u32 idle; 555 556 entry = rhashtable_lookup(ð->flow_table, &f->cookie, 557 mtk_flow_ht_params); 558 if (!entry) 559 return -ENOENT; 560 561 idle = mtk_foe_entry_idle_time(eth->ppe[entry->ppe_index], entry); 562 f->stats.lastused = jiffies - idle * HZ; 563 564 if (entry->hash != 0xFFFF && 565 mtk_foe_entry_get_mib(eth->ppe[entry->ppe_index], entry->hash, 566 &diff)) { 567 f->stats.pkts += diff.packets; 568 f->stats.bytes += diff.bytes; 569 } 570 571 return 0; 572 } 573 574 static DEFINE_MUTEX(mtk_flow_offload_mutex); 575 576 int mtk_flow_offload_cmd(struct mtk_eth *eth, struct flow_cls_offload *cls, 577 int ppe_index) 578 { 579 int err; 580 581 mutex_lock(&mtk_flow_offload_mutex); 582 switch (cls->command) { 583 case FLOW_CLS_REPLACE: 584 err = mtk_flow_offload_replace(eth, cls, ppe_index); 585 break; 586 case FLOW_CLS_DESTROY: 587 err = mtk_flow_offload_destroy(eth, cls); 588 break; 589 case FLOW_CLS_STATS: 590 err = mtk_flow_offload_stats(eth, cls); 591 break; 592 default: 593 err = -EOPNOTSUPP; 594 break; 595 } 596 mutex_unlock(&mtk_flow_offload_mutex); 597 598 return err; 599 } 600 601 static int 602 mtk_eth_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv) 603 { 604 struct flow_cls_offload *cls = type_data; 605 struct net_device *dev = cb_priv; 606 struct mtk_mac *mac; 607 struct mtk_eth *eth; 608 609 mac = netdev_priv(dev); 610 eth = mac->hw; 611 612 if (!tc_can_offload(dev)) 613 return -EOPNOTSUPP; 614 615 if (type != TC_SETUP_CLSFLOWER) 616 return -EOPNOTSUPP; 617 618 return mtk_flow_offload_cmd(eth, cls, 0); 619 } 620 621 static int 622 mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f) 623 { 624 struct mtk_mac *mac = netdev_priv(dev); 625 struct mtk_eth *eth = mac->hw; 626 static LIST_HEAD(block_cb_list); 627 struct flow_block_cb *block_cb; 628 flow_setup_cb_t *cb; 629 630 if (!eth->soc->offload_version) 631 return -EOPNOTSUPP; 632 633 if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS) 634 return -EOPNOTSUPP; 635 636 cb = mtk_eth_setup_tc_block_cb; 637 f->driver_block_list = &block_cb_list; 638 639 switch (f->command) { 640 case FLOW_BLOCK_BIND: 641 block_cb = flow_block_cb_lookup(f->block, cb, dev); 642 if (block_cb) { 643 flow_block_cb_incref(block_cb); 644 return 0; 645 } 646 block_cb = flow_block_cb_alloc(cb, dev, dev, NULL); 647 if (IS_ERR(block_cb)) 648 return PTR_ERR(block_cb); 649 650 flow_block_cb_incref(block_cb); 651 flow_block_cb_add(block_cb, f); 652 list_add_tail(&block_cb->driver_list, &block_cb_list); 653 return 0; 654 case FLOW_BLOCK_UNBIND: 655 block_cb = flow_block_cb_lookup(f->block, cb, dev); 656 if (!block_cb) 657 return -ENOENT; 658 659 if (!flow_block_cb_decref(block_cb)) { 660 flow_block_cb_remove(block_cb, f); 661 list_del(&block_cb->driver_list); 662 } 663 return 0; 664 default: 665 return -EOPNOTSUPP; 666 } 667 } 668 669 int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type, 670 void *type_data) 671 { 672 switch (type) { 673 case TC_SETUP_BLOCK: 674 case TC_SETUP_FT: 675 return mtk_eth_setup_tc_block(dev, type_data); 676 default: 677 return -EOPNOTSUPP; 678 } 679 } 680 681 int mtk_eth_offload_init(struct mtk_eth *eth, u8 id) 682 { 683 if (!eth->ppe[id] || !eth->ppe[id]->foe_table) 684 return 0; 685 return rhashtable_init(ð->flow_table, &mtk_flow_ht_params); 686 } 687