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