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 <net/flow_offload.h> 10 #include <net/pkt_cls.h> 11 #include <net/dsa.h> 12 #include "mtk_eth_soc.h" 13 14 struct mtk_flow_data { 15 struct ethhdr eth; 16 17 union { 18 struct { 19 __be32 src_addr; 20 __be32 dst_addr; 21 } v4; 22 }; 23 24 __be16 src_port; 25 __be16 dst_port; 26 27 struct { 28 u16 id; 29 __be16 proto; 30 u8 num; 31 } vlan; 32 struct { 33 u16 sid; 34 u8 num; 35 } pppoe; 36 }; 37 38 struct mtk_flow_entry { 39 struct rhash_head node; 40 unsigned long cookie; 41 u16 hash; 42 }; 43 44 static const struct rhashtable_params mtk_flow_ht_params = { 45 .head_offset = offsetof(struct mtk_flow_entry, node), 46 .key_offset = offsetof(struct mtk_flow_entry, cookie), 47 .key_len = sizeof(unsigned long), 48 .automatic_shrinking = true, 49 }; 50 51 static u32 52 mtk_eth_timestamp(struct mtk_eth *eth) 53 { 54 return mtk_r32(eth, 0x0010) & MTK_FOE_IB1_BIND_TIMESTAMP; 55 } 56 57 static int 58 mtk_flow_set_ipv4_addr(struct mtk_foe_entry *foe, struct mtk_flow_data *data, 59 bool egress) 60 { 61 return mtk_foe_entry_set_ipv4_tuple(foe, egress, 62 data->v4.src_addr, data->src_port, 63 data->v4.dst_addr, data->dst_port); 64 } 65 66 static void 67 mtk_flow_offload_mangle_eth(const struct flow_action_entry *act, void *eth) 68 { 69 void *dest = eth + act->mangle.offset; 70 const void *src = &act->mangle.val; 71 72 if (act->mangle.offset > 8) 73 return; 74 75 if (act->mangle.mask == 0xffff) { 76 src += 2; 77 dest += 2; 78 } 79 80 memcpy(dest, src, act->mangle.mask ? 2 : 4); 81 } 82 83 84 static int 85 mtk_flow_mangle_ports(const struct flow_action_entry *act, 86 struct mtk_flow_data *data) 87 { 88 u32 val = ntohl(act->mangle.val); 89 90 switch (act->mangle.offset) { 91 case 0: 92 if (act->mangle.mask == ~htonl(0xffff)) 93 data->dst_port = cpu_to_be16(val); 94 else 95 data->src_port = cpu_to_be16(val >> 16); 96 break; 97 case 2: 98 data->dst_port = cpu_to_be16(val); 99 break; 100 default: 101 return -EINVAL; 102 } 103 104 return 0; 105 } 106 107 static int 108 mtk_flow_mangle_ipv4(const struct flow_action_entry *act, 109 struct mtk_flow_data *data) 110 { 111 __be32 *dest; 112 113 switch (act->mangle.offset) { 114 case offsetof(struct iphdr, saddr): 115 dest = &data->v4.src_addr; 116 break; 117 case offsetof(struct iphdr, daddr): 118 dest = &data->v4.dst_addr; 119 break; 120 default: 121 return -EINVAL; 122 } 123 124 memcpy(dest, &act->mangle.val, sizeof(u32)); 125 126 return 0; 127 } 128 129 static int 130 mtk_flow_get_dsa_port(struct net_device **dev) 131 { 132 #if IS_ENABLED(CONFIG_NET_DSA) 133 struct dsa_port *dp; 134 135 dp = dsa_port_from_netdev(*dev); 136 if (IS_ERR(dp)) 137 return -ENODEV; 138 139 if (dp->cpu_dp->tag_ops->proto != DSA_TAG_PROTO_MTK) 140 return -ENODEV; 141 142 *dev = dp->cpu_dp->master; 143 144 return dp->index; 145 #else 146 return -ENODEV; 147 #endif 148 } 149 150 static int 151 mtk_flow_set_output_device(struct mtk_eth *eth, struct mtk_foe_entry *foe, 152 struct net_device *dev) 153 { 154 int pse_port, dsa_port; 155 156 dsa_port = mtk_flow_get_dsa_port(&dev); 157 if (dsa_port >= 0) 158 mtk_foe_entry_set_dsa(foe, dsa_port); 159 160 if (dev == eth->netdev[0]) 161 pse_port = 1; 162 else if (dev == eth->netdev[1]) 163 pse_port = 2; 164 else 165 return -EOPNOTSUPP; 166 167 mtk_foe_entry_set_pse_port(foe, pse_port); 168 169 return 0; 170 } 171 172 static int 173 mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f) 174 { 175 struct flow_rule *rule = flow_cls_offload_flow_rule(f); 176 struct flow_action_entry *act; 177 struct mtk_flow_data data = {}; 178 struct mtk_foe_entry foe; 179 struct net_device *odev = NULL; 180 struct mtk_flow_entry *entry; 181 int offload_type = 0; 182 u16 addr_type = 0; 183 u32 timestamp; 184 u8 l4proto = 0; 185 int err = 0; 186 int hash; 187 int i; 188 189 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_META)) { 190 struct flow_match_meta match; 191 192 flow_rule_match_meta(rule, &match); 193 } else { 194 return -EOPNOTSUPP; 195 } 196 197 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_CONTROL)) { 198 struct flow_match_control match; 199 200 flow_rule_match_control(rule, &match); 201 addr_type = match.key->addr_type; 202 } else { 203 return -EOPNOTSUPP; 204 } 205 206 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) { 207 struct flow_match_basic match; 208 209 flow_rule_match_basic(rule, &match); 210 l4proto = match.key->ip_proto; 211 } else { 212 return -EOPNOTSUPP; 213 } 214 215 flow_action_for_each(i, act, &rule->action) { 216 switch (act->id) { 217 case FLOW_ACTION_MANGLE: 218 if (act->mangle.htype == FLOW_ACT_MANGLE_HDR_TYPE_ETH) 219 mtk_flow_offload_mangle_eth(act, &data.eth); 220 break; 221 case FLOW_ACTION_REDIRECT: 222 odev = act->dev; 223 break; 224 case FLOW_ACTION_CSUM: 225 break; 226 case FLOW_ACTION_VLAN_PUSH: 227 if (data.vlan.num == 1 || 228 act->vlan.proto != htons(ETH_P_8021Q)) 229 return -EOPNOTSUPP; 230 231 data.vlan.id = act->vlan.vid; 232 data.vlan.proto = act->vlan.proto; 233 data.vlan.num++; 234 break; 235 case FLOW_ACTION_VLAN_POP: 236 break; 237 case FLOW_ACTION_PPPOE_PUSH: 238 if (data.pppoe.num == 1) 239 return -EOPNOTSUPP; 240 241 data.pppoe.sid = act->pppoe.sid; 242 data.pppoe.num++; 243 break; 244 default: 245 return -EOPNOTSUPP; 246 } 247 } 248 249 switch (addr_type) { 250 case FLOW_DISSECTOR_KEY_IPV4_ADDRS: 251 offload_type = MTK_PPE_PKT_TYPE_IPV4_HNAPT; 252 break; 253 default: 254 return -EOPNOTSUPP; 255 } 256 257 if (!is_valid_ether_addr(data.eth.h_source) || 258 !is_valid_ether_addr(data.eth.h_dest)) 259 return -EINVAL; 260 261 err = mtk_foe_entry_prepare(&foe, offload_type, l4proto, 0, 262 data.eth.h_source, 263 data.eth.h_dest); 264 if (err) 265 return err; 266 267 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) { 268 struct flow_match_ports ports; 269 270 flow_rule_match_ports(rule, &ports); 271 data.src_port = ports.key->src; 272 data.dst_port = ports.key->dst; 273 } else { 274 return -EOPNOTSUPP; 275 } 276 277 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) { 278 struct flow_match_ipv4_addrs addrs; 279 280 flow_rule_match_ipv4_addrs(rule, &addrs); 281 282 data.v4.src_addr = addrs.key->src; 283 data.v4.dst_addr = addrs.key->dst; 284 285 mtk_flow_set_ipv4_addr(&foe, &data, false); 286 } 287 288 flow_action_for_each(i, act, &rule->action) { 289 if (act->id != FLOW_ACTION_MANGLE) 290 continue; 291 292 switch (act->mangle.htype) { 293 case FLOW_ACT_MANGLE_HDR_TYPE_TCP: 294 case FLOW_ACT_MANGLE_HDR_TYPE_UDP: 295 err = mtk_flow_mangle_ports(act, &data); 296 break; 297 case FLOW_ACT_MANGLE_HDR_TYPE_IP4: 298 err = mtk_flow_mangle_ipv4(act, &data); 299 break; 300 case FLOW_ACT_MANGLE_HDR_TYPE_ETH: 301 /* handled earlier */ 302 break; 303 default: 304 return -EOPNOTSUPP; 305 } 306 307 if (err) 308 return err; 309 } 310 311 if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) { 312 err = mtk_flow_set_ipv4_addr(&foe, &data, true); 313 if (err) 314 return err; 315 } 316 317 if (data.vlan.num == 1) { 318 if (data.vlan.proto != htons(ETH_P_8021Q)) 319 return -EOPNOTSUPP; 320 321 mtk_foe_entry_set_vlan(&foe, data.vlan.id); 322 } 323 if (data.pppoe.num == 1) 324 mtk_foe_entry_set_pppoe(&foe, data.pppoe.sid); 325 326 err = mtk_flow_set_output_device(eth, &foe, odev); 327 if (err) 328 return err; 329 330 entry = kzalloc(sizeof(*entry), GFP_KERNEL); 331 if (!entry) 332 return -ENOMEM; 333 334 entry->cookie = f->cookie; 335 timestamp = mtk_eth_timestamp(eth); 336 hash = mtk_foe_entry_commit(ð->ppe, &foe, timestamp); 337 if (hash < 0) { 338 err = hash; 339 goto free; 340 } 341 342 entry->hash = hash; 343 err = rhashtable_insert_fast(ð->flow_table, &entry->node, 344 mtk_flow_ht_params); 345 if (err < 0) 346 goto clear_flow; 347 348 return 0; 349 clear_flow: 350 mtk_foe_entry_clear(ð->ppe, hash); 351 free: 352 kfree(entry); 353 return err; 354 } 355 356 static int 357 mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f) 358 { 359 struct mtk_flow_entry *entry; 360 361 entry = rhashtable_lookup(ð->flow_table, &f->cookie, 362 mtk_flow_ht_params); 363 if (!entry) 364 return -ENOENT; 365 366 mtk_foe_entry_clear(ð->ppe, entry->hash); 367 rhashtable_remove_fast(ð->flow_table, &entry->node, 368 mtk_flow_ht_params); 369 kfree(entry); 370 371 return 0; 372 } 373 374 static int 375 mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f) 376 { 377 struct mtk_flow_entry *entry; 378 int timestamp; 379 u32 idle; 380 381 entry = rhashtable_lookup(ð->flow_table, &f->cookie, 382 mtk_flow_ht_params); 383 if (!entry) 384 return -ENOENT; 385 386 timestamp = mtk_foe_entry_timestamp(ð->ppe, entry->hash); 387 if (timestamp < 0) 388 return -ETIMEDOUT; 389 390 idle = mtk_eth_timestamp(eth) - timestamp; 391 f->stats.lastused = jiffies - idle * HZ; 392 393 return 0; 394 } 395 396 static DEFINE_MUTEX(mtk_flow_offload_mutex); 397 398 static int 399 mtk_eth_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv) 400 { 401 struct flow_cls_offload *cls = type_data; 402 struct net_device *dev = cb_priv; 403 struct mtk_mac *mac = netdev_priv(dev); 404 struct mtk_eth *eth = mac->hw; 405 int err; 406 407 if (!tc_can_offload(dev)) 408 return -EOPNOTSUPP; 409 410 if (type != TC_SETUP_CLSFLOWER) 411 return -EOPNOTSUPP; 412 413 mutex_lock(&mtk_flow_offload_mutex); 414 switch (cls->command) { 415 case FLOW_CLS_REPLACE: 416 err = mtk_flow_offload_replace(eth, cls); 417 break; 418 case FLOW_CLS_DESTROY: 419 err = mtk_flow_offload_destroy(eth, cls); 420 break; 421 case FLOW_CLS_STATS: 422 err = mtk_flow_offload_stats(eth, cls); 423 break; 424 default: 425 err = -EOPNOTSUPP; 426 break; 427 } 428 mutex_unlock(&mtk_flow_offload_mutex); 429 430 return err; 431 } 432 433 static int 434 mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f) 435 { 436 struct mtk_mac *mac = netdev_priv(dev); 437 struct mtk_eth *eth = mac->hw; 438 static LIST_HEAD(block_cb_list); 439 struct flow_block_cb *block_cb; 440 flow_setup_cb_t *cb; 441 442 if (!eth->ppe.foe_table) 443 return -EOPNOTSUPP; 444 445 if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS) 446 return -EOPNOTSUPP; 447 448 cb = mtk_eth_setup_tc_block_cb; 449 f->driver_block_list = &block_cb_list; 450 451 switch (f->command) { 452 case FLOW_BLOCK_BIND: 453 block_cb = flow_block_cb_lookup(f->block, cb, dev); 454 if (block_cb) { 455 flow_block_cb_incref(block_cb); 456 return 0; 457 } 458 block_cb = flow_block_cb_alloc(cb, dev, dev, NULL); 459 if (IS_ERR(block_cb)) 460 return PTR_ERR(block_cb); 461 462 flow_block_cb_add(block_cb, f); 463 list_add_tail(&block_cb->driver_list, &block_cb_list); 464 return 0; 465 case FLOW_BLOCK_UNBIND: 466 block_cb = flow_block_cb_lookup(f->block, cb, dev); 467 if (!block_cb) 468 return -ENOENT; 469 470 if (flow_block_cb_decref(block_cb)) { 471 flow_block_cb_remove(block_cb, f); 472 list_del(&block_cb->driver_list); 473 } 474 return 0; 475 default: 476 return -EOPNOTSUPP; 477 } 478 } 479 480 int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type, 481 void *type_data) 482 { 483 if (type == TC_SETUP_FT) 484 return mtk_eth_setup_tc_block(dev, type_data); 485 486 return -EOPNOTSUPP; 487 } 488 489 int mtk_eth_offload_init(struct mtk_eth *eth) 490 { 491 if (!eth->ppe.foe_table) 492 return 0; 493 494 return rhashtable_init(ð->flow_table, &mtk_flow_ht_params); 495 } 496