From a59cb5c770a694cb34ab179ec59e91ba5c39908b Mon Sep 17 00:00:00 2001
From: Bo Jiao <Bo.Jiao@mediatek.com>
Date: Mon, 27 Jun 2022 14:48:35 +0800
Subject: [PATCH 6/8] 9995-flow-offload-add-mkhnat-dual-ppe-new-v2

---
 arch/arm64/boot/dts/mediatek/mt7986a.dtsi     |  1 +
 drivers/net/ethernet/mediatek/mtk_eth_soc.c   | 67 ++++++++++++++-----
 drivers/net/ethernet/mediatek/mtk_eth_soc.h   | 10 ++-
 drivers/net/ethernet/mediatek/mtk_ppe.c       |  5 +-
 drivers/net/ethernet/mediatek/mtk_ppe.h       |  7 +-
 .../net/ethernet/mediatek/mtk_ppe_debugfs.c   | 27 ++++++--
 .../net/ethernet/mediatek/mtk_ppe_offload.c   | 45 ++++++++++---
 include/linux/netdevice.h                     |  4 ++
 8 files changed, 125 insertions(+), 41 deletions(-)
 mode change 100644 => 100755 drivers/net/ethernet/mediatek/mtk_ppe_offload.c

diff --git a/arch/arm64/boot/dts/mediatek/mt7986a.dtsi b/arch/arm64/boot/dts/mediatek/mt7986a.dtsi
index 7f78de6b9..381136c21 100644
--- a/arch/arm64/boot/dts/mediatek/mt7986a.dtsi
+++ b/arch/arm64/boot/dts/mediatek/mt7986a.dtsi
@@ -479,6 +479,7 @@
                 mediatek,ethsys = <&ethsys>;
 		mediatek,sgmiisys = <&sgmiisys0>, <&sgmiisys1>;
 		mediatek,wed = <&wed0>, <&wed1>;
+                mtketh-ppe-num = <2>;
                 #reset-cells = <1>;
                 #address-cells = <1>;
                 #size-cells = <0>;
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.c b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
index 01fc1e5c0..3f67bebfe 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -1732,6 +1732,7 @@ static int mtk_poll_rx(struct napi_struc
 	u8 *data, *new_data;
 	struct mtk_rx_dma_v2 *rxd, trxd;
 	int done = 0;
+	int i;
 
 	if (unlikely(!ring))
 		goto rx_done;
@@ -1843,14 +1844,20 @@ static int mtk_poll_rx(struct napi_struc
 
 #if defined(CONFIG_MEDIATEK_NETSYS_RX_V2)
 		reason = FIELD_GET(MTK_RXD5_PPE_CPU_REASON_V2, trxd.rxd5);
-		if (reason == MTK_PPE_CPU_REASON_HIT_UNBIND_RATE_REACHED)
-			mtk_ppe_check_skb(eth->ppe, skb,
-					  trxd.rxd5 & MTK_RXD5_FOE_ENTRY_V2);
+		if (reason == MTK_PPE_CPU_REASON_HIT_UNBIND_RATE_REACHED) {
+			for (i = 0; i < eth->ppe_num; i++) {
+				mtk_ppe_check_skb(eth->ppe[i], skb,
+						  trxd.rxd5 & MTK_RXD5_FOE_ENTRY_V2);
+			}
+		}
 #else
 		reason = FIELD_GET(MTK_RXD4_PPE_CPU_REASON, trxd.rxd4);
-		if (reason == MTK_PPE_CPU_REASON_HIT_UNBIND_RATE_REACHED)
-			mtk_ppe_check_skb(eth->ppe, skb,
-					  trxd.rxd4 & MTK_RXD4_FOE_ENTRY);
+		if (reason == MTK_PPE_CPU_REASON_HIT_UNBIND_RATE_REACHED) {
+			for (i = 0; i < eth->ppe_num; i++) {
+				mtk_ppe_check_skb(eth->ppe[i], skb,
+						  trxd.rxd4 & MTK_RXD4_FOE_ENTRY);
+			}
+		}
 #endif
 
 		if (netdev->features & NETIF_F_HW_VLAN_CTAG_RX) {
@@ -3184,8 +3191,12 @@ static int mtk_open(struct net_device *d
 	if (!phy_node && eth->xgmii->regmap_sgmii[mac->id])
 		regmap_write(eth->xgmii->regmap_sgmii[mac->id], SGMSYS_QPHY_PWR_STATE_CTRL, 0);
 
-	if (eth->soc->offload_version && mtk_ppe_start(&eth->ppe) == 0)
-			gdm_config = MTK_GDMA_TO_PPE;
+	if (eth->soc->offload_version) {
+			gdm_config = MTK_GDMA_TO_PPE0;
+
+			for (i = 0; i < eth->ppe_num; i++)
+				mtk_ppe_start(eth->ppe[i]);
+		}
 
 	mtk_gdm_config(eth, mac->id, gdm_config);
 
@@ -3268,8 +3279,10 @@ static int mtk_stop(struct net_device *d
 
 	mtk_dma_free(eth);
 
-	if (eth->soc->offload_version)
-		mtk_ppe_stop(eth->ppe);
+	if (eth->soc->offload_version) {
+		for (i = 0; i < eth->ppe_num; i++)
+			mtk_ppe_stop(eth->ppe[i]);
+	}
 
 	return 0;
 }
@@ -4408,15 +4421,35 @@ static int mtk_probe(struct platform_dev
 	}
 
 	if (eth->soc->offload_version) {
-		eth->ppe = mtk_ppe_init(eth, eth->base + MTK_ETH_PPE_BASE, 2);
-		if (!eth->ppe) {
-			err = -ENOMEM;
-			goto err_free_dev;
+		unsigned int val;
+ 
+		err = of_property_read_u32_index(pdev->dev.of_node, "mtketh-ppe-num", 0, &val);
+		if (err < 0)
+			eth->ppe_num = 1;
+		else
+			eth->ppe_num = val;
+ 
+		if (eth->ppe_num > MTK_MAX_PPE_NUM) {
+			dev_warn(&pdev->dev, "%d is not a valid ppe num, please check mtketh-ppe-num in dts !", eth->ppe_num);
+			eth->ppe_num = MTK_MAX_PPE_NUM;
 		}
 
-		err = mtk_eth_offload_init(eth);
-		if (err)
-			goto err_free_dev;
+		dev_info(&pdev->dev, "ppe num = %d\n", eth->ppe_num);
+
+		for (i = 0; i < eth->ppe_num; i++) {
+			eth->ppe[i] = mtk_ppe_init(eth,
+					   eth->base + MTK_ETH_PPE_BASE + i * 0x400, 2, i);
+			if (!eth->ppe[i]) {
+				err = -ENOMEM;
+				goto err_free_dev;
+			}
+
+			err = mtk_eth_offload_init(eth, i);
+			if (err)
+				goto err_free_dev;
+		}
+
+		mtk_ppe_debugfs_init(eth);
 	}
 
 	for (i = 0; i < MTK_MAX_DEVS; i++) {
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.h b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
index fce1a7172..b4de7c0c6 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
@@ -118,7 +118,12 @@
 #define MTK_GDMA_UCS_EN		BIT(20)
 #define MTK_GDMA_STRP_CRC	BIT(16)
 #define MTK_GDMA_TO_PDMA	0x0
-#define MTK_GDMA_TO_PPE		0x3333
+#if defined(CONFIG_MEDIATEK_NETSYS_V2)
+#define MTK_GDMA_TO_PPE0	0x3333
+#define MTK_GDMA_TO_PPE1	0x4444
+#else
+#define MTK_GDMA_TO_PPE0	0x4444
+#endif
 #define MTK_GDMA_DROP_ALL	0x7777
 
 /* GDM Egress Control Register */
@@ -1612,7 +1617,8 @@ struct mtk_eth {
 	spinlock_t			syscfg0_lock;
 	struct timer_list		mtk_dma_monitor_timer;
 
-	struct mtk_ppe			*ppe;
+	u8				ppe_num;
+	struct mtk_ppe			*ppe[MTK_MAX_PPE_NUM];
 	struct rhashtable		flow_table;
 };
 
@@ -1668,9 +1674,11 @@ int mtk_gmac_usxgmii_path_setup(struct m
 void mtk_usxgmii_reset(struct mtk_xgmii *ss, int mac_id);
 int mtk_dump_usxgmii(struct regmap *pmap, char *name, u32 offset, u32 range);
 
-int mtk_eth_offload_init(struct mtk_eth *eth);
+int mtk_eth_offload_init(struct mtk_eth *eth, int id);
 int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
 		     void *type_data);
 void mtk_eth_set_dma_device(struct mtk_eth *eth, struct device *dma_dev);
 int mtk_rss_set_indr_tbl(struct mtk_eth *eth, int num);
+
+int mtk_ppe_debugfs_init(struct mtk_eth *eth);
 #endif /* MTK_ETH_H */
diff --git a/drivers/net/ethernet/mediatek/mtk_ppe.c b/drivers/net/ethernet/mediatek/mtk_ppe.c
index d46e91178..3d6ff30ba 100755
--- a/drivers/net/ethernet/mediatek/mtk_ppe.c
+++ b/drivers/net/ethernet/mediatek/mtk_ppe.c
@@ -677,7 +677,7 @@ int mtk_foe_entry_idle_time(struct mtk_ppe *ppe, struct mtk_flow_entry *entry)
 }
 
 struct mtk_ppe *mtk_ppe_init(struct mtk_eth *eth, void __iomem *base,
-		 int version)
+		 int version, int id)
 {
 	struct device *dev = eth->dev;
 	struct mtk_foe_entry *foe;
@@ -696,6 +696,7 @@ struct mtk_ppe *mtk_ppe_init(struct mtk_eth *eth, void __iomem *base,
 	ppe->eth = eth;
 	ppe->dev = dev;
 	ppe->version = version;
+	ppe->id = id;
 
 	foe = dmam_alloc_coherent(ppe->dev, MTK_PPE_ENTRIES * sizeof(*foe),
 				  &ppe->foe_phys, GFP_KERNEL);
@@ -704,8 +705,6 @@ struct mtk_ppe *mtk_ppe_init(struct mtk_eth *eth, void __iomem *base,
 
 	ppe->foe_table = foe;
 
-	mtk_ppe_debugfs_init(ppe);
-
 	return ppe;
 }
 
diff --git a/drivers/net/ethernet/mediatek/mtk_ppe.h b/drivers/net/ethernet/mediatek/mtk_ppe.h
index a76f4b0ac..21cc55145 100644
--- a/drivers/net/ethernet/mediatek/mtk_ppe.h
+++ b/drivers/net/ethernet/mediatek/mtk_ppe.h
@@ -8,10 +8,12 @@
 #include <linux/bitfield.h>
 #include <linux/rhashtable.h>
 
 #if defined(CONFIG_MEDIATEK_NETSYS_V2)
+#define MTK_MAX_PPE_NUM			2
 #define MTK_ETH_PPE_BASE		0x2000
 #else
+#define MTK_MAX_PPE_NUM			1
 #define MTK_ETH_PPE_BASE		0xc00
 #endif
 
 #define MTK_PPE_ENTRIES_SHIFT		3
@@ -253,6 +255,7 @@ struct mtk_flow_entry {
 		};
 	};
 	u8 type;
+	s8 ppe_index;
 	s8 wed_index;
 	u16 hash;
 	union {
@@ -272,6 +275,7 @@ struct mtk_ppe {
 	struct device *dev;
 	void __iomem *base;
 	int version;
+	int id;
 
 	struct mtk_foe_entry *foe_table;
 	dma_addr_t foe_phys;
@@ -284,7 +288,7 @@ struct mtk_ppe {
 	void *acct_table;
 };
 
-struct mtk_ppe *mtk_ppe_init(struct mtk_eth *eth, void __iomem *base, int version);
+struct mtk_ppe *mtk_ppe_init(struct mtk_eth *eth, void __iomem *base, int version, int id);
 int mtk_ppe_start(struct mtk_ppe *ppe);
 int mtk_ppe_stop(struct mtk_ppe *ppe);
 
@@ -335,6 +339,5 @@ int mtk_foe_entry_set_wdma(struct mtk_foe_entry *entry, int wdma_idx, int txq,
 int mtk_foe_entry_commit(struct mtk_ppe *ppe, struct mtk_flow_entry *entry);
 void mtk_foe_entry_clear(struct mtk_ppe *ppe, struct mtk_flow_entry *entry);
 int mtk_foe_entry_idle_time(struct mtk_ppe *ppe, struct mtk_flow_entry *entry);
-int mtk_ppe_debugfs_init(struct mtk_ppe *ppe);
 
 #endif
diff --git a/drivers/net/ethernet/mediatek/mtk_ppe_debugfs.c b/drivers/net/ethernet/mediatek/mtk_ppe_debugfs.c
index a591ab1fd..f4ebe5944 100644
--- a/drivers/net/ethernet/mediatek/mtk_ppe_debugfs.c
+++ b/drivers/net/ethernet/mediatek/mtk_ppe_debugfs.c
@@ -73,9 +73,8 @@ mtk_print_addr_info(struct seq_file *m, struct mtk_flow_addr_info *ai)
 }
 
 static int
-mtk_ppe_debugfs_foe_show(struct seq_file *m, void *private, bool bind)
+mtk_ppe_debugfs_foe_show(struct seq_file *m, struct mtk_ppe *ppe, bool bind)
 {
-	struct mtk_ppe *ppe = m->private;
 	int i;
 
 	for (i = 0; i < MTK_PPE_ENTRIES; i++) {
@@ -122,6 +121,8 @@ mtk_ppe_debugfs_foe_show(struct seq_file *m, void *private, bool bind)
 			break;
 		}
 
+		seq_printf(m, " ppe=%d", ppe->id);
+
 		seq_printf(m, " orig=");
 		mtk_print_addr_info(m, &ai);
 
@@ -164,13 +165,25 @@ mtk_ppe_debugfs_foe_show(struct seq_file *m, void *private, bool bind)
 static int
 mtk_ppe_debugfs_foe_show_all(struct seq_file *m, void *private)
 {
-	return mtk_ppe_debugfs_foe_show(m, private, false);
+	struct mtk_eth *eth = m->private;
+	int i;
+
+	for (i = 0; i < eth->ppe_num; i++)
+		mtk_ppe_debugfs_foe_show(m, eth->ppe[i], false);
+
+	return 0;
 }
 
 static int
 mtk_ppe_debugfs_foe_show_bind(struct seq_file *m, void *private)
 {
-	return mtk_ppe_debugfs_foe_show(m, private, true);
+	struct mtk_eth *eth = m->private;
+	int i;
+
+	for (i = 0; i < eth->ppe_num; i++)
+		mtk_ppe_debugfs_foe_show(m, eth->ppe[i], true);
+
+	return 0;
 }
 
 static int
@@ -187,7 +200,7 @@ mtk_ppe_debugfs_foe_open_bind(struct inode *inode, struct file *file)
 			   inode->i_private);
 }
 
-int mtk_ppe_debugfs_init(struct mtk_ppe *ppe)
+int mtk_ppe_debugfs_init(struct mtk_eth *eth)
 {
 	static const struct file_operations fops_all = {
 		.open = mtk_ppe_debugfs_foe_open_all,
@@ -209,8 +222,8 @@ int mtk_ppe_debugfs_init(struct mtk_ppe *ppe)
 	if (!root)
 		return -ENOMEM;
 
-	debugfs_create_file("entries", S_IRUGO, root, ppe, &fops_all);
-	debugfs_create_file("bind", S_IRUGO, root, ppe, &fops_bind);
+	debugfs_create_file("entries", S_IRUGO, root, eth, &fops_all);
+	debugfs_create_file("bind", S_IRUGO, root, eth, &fops_bind);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/mediatek/mtk_ppe_offload.c b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
old mode 100644
new mode 100755
index 5a4201447..2f7d76d3b
--- a/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
+++ b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
@@ -226,8 +226,10 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
 	struct flow_action_entry *act;
 	struct mtk_flow_data data = {};
 	struct mtk_foe_entry foe;
-	struct net_device *odev = NULL;
+	struct net_device *idev = NULL, *odev = NULL;
 	struct mtk_flow_entry *entry;
+	struct net_device_path_ctx ctx = {};
+	struct net_device_path path = {};
 	int offload_type = 0;
 	int wed_index = -1;
 	u16 addr_type = 0;
@@ -242,6 +244,10 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
 		struct flow_match_meta match;
 
 		flow_rule_match_meta(rule, &match);
+		idev = __dev_get_by_index(&init_net, match.key->ingress_ifindex);
+
+		if (!idev)
+			pr_info("[%s] idev doesn't exist !\n", __func__);
 	} else {
 		return -EOPNOTSUPP;
 	}
@@ -435,11 +441,27 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
 	if (!entry)
 		return -ENOMEM;
 
+	i = 0;
+#if defined(CONFIG_MEDIATEK_NETSYS_V2)
+	if (idev && idev->netdev_ops->ndo_fill_receive_path) {
+		ctx.dev = idev;
+		idev->netdev_ops->ndo_fill_receive_path(&ctx, &path);
+		i = path.mtk_wdma.wdma_idx;
+		if (i >= eth->ppe_num) {
+			if (printk_ratelimit())
+				pr_info("[%s] PPE%d doesn't exist, please check mtketh-ppe-num in dts !\n", __func__, i);
+
+			return -EINVAL;
+		}
+	}
+#endif
+
 	entry->cookie = f->cookie;
 	memcpy(&entry->data, &foe, sizeof(entry->data));
+	entry->ppe_index = i;
 	entry->wed_index = wed_index;
 
-	if (mtk_foe_entry_commit(eth->ppe, entry) < 0)
+	if (mtk_foe_entry_commit(eth->ppe[i], entry) < 0)
 		goto free;
 
 	err = rhashtable_insert_fast(&eth->flow_table, &entry->node,
@@ -450,7 +470,7 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
 	return 0;
 
 clear:
-	mtk_foe_entry_clear(eth->ppe, entry);
+	mtk_foe_entry_clear(eth->ppe[i], entry);
 free:
 	kfree(entry);
 	if (wed_index >= 0)
@@ -462,13 +482,15 @@ static int
 mtk_flow_offload_destroy(struct mtk_eth *eth, struct flow_cls_offload *f)
 {
 	struct mtk_flow_entry *entry;
+	int i;
 
 	entry = rhashtable_lookup(&eth->flow_table, &f->cookie,
 				  mtk_flow_ht_params);
 	if (!entry)
 		return -ENOENT;
 
-	mtk_foe_entry_clear(eth->ppe, entry);
+	i = entry->ppe_index;
+	mtk_foe_entry_clear(eth->ppe[i], entry);
 	rhashtable_remove_fast(&eth->flow_table, &entry->node,
 			       mtk_flow_ht_params);
 	if (entry->wed_index >= 0)
@@ -483,13 +505,15 @@ mtk_flow_offload_stats(struct mtk_eth *eth, struct flow_cls_offload *f)
 {
 	struct mtk_flow_entry *entry;
 	u32 idle;
+	int i;
 
 	entry = rhashtable_lookup(&eth->flow_table, &f->cookie,
 				  mtk_flow_ht_params);
 	if (!entry)
 		return -ENOENT;
 
-	idle = mtk_foe_entry_idle_time(eth->ppe, entry);
+	i = entry->ppe_index;
+	idle = mtk_foe_entry_idle_time(eth->ppe[i], entry);
 	f->stats.lastused = jiffies - idle * HZ;
 
 	return 0;
@@ -540,12 +564,14 @@ mtk_eth_setup_tc_block(struct net_device *dev, struct flow_block_offload *f)
 	static LIST_HEAD(block_cb_list);
 	struct flow_block_cb *block_cb;
 	flow_setup_cb_t *cb;
-	int err = 0;
+	int i, err = 0;
 
 	flowtable = container_of(f->block, struct nf_flowtable, flow_block);
 
-	if (!eth->ppe || !eth->ppe->foe_table)
-		return -EOPNOTSUPP;
+	for (i = 0; i < eth->ppe_num; i++) {
+		if (!eth->ppe[i] || !eth->ppe[i]->foe_table)
+			return -EOPNOTSUPP;
+	}
 
 	if (f->binder_type != FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS)
 		return -EOPNOTSUPP;
@@ -591,9 +618,9 @@ int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
 	}
 }
 
-int mtk_eth_offload_init(struct mtk_eth *eth)
+int mtk_eth_offload_init(struct mtk_eth *eth, int id)
 {
-	if (!eth->ppe || !eth->ppe->foe_table)
+	if (!eth->ppe[id] || !eth->ppe[id]->foe_table)
 		return 0;
 
 	return rhashtable_init(&eth->flow_table, &mtk_flow_ht_params);
diff --git a/include/linux/netdevice.h b/include/linux/netdevice.h
index 35998b1a7..0ada2461b 100644
--- a/include/linux/netdevice.h
+++ b/include/linux/netdevice.h
@@ -1302,6 +1302,8 @@ struct tlsdev_ops;
  *	rtnl_lock is not held.
  * int (*ndo_fill_forward_path)(struct net_device_path_ctx *ctx, struct net_device_path *path);
  *     Get the forwarding path to reach the real device from the HW destination address
+ * int (*ndo_fill_receive_path)(struct net_device_path_ctx *ctx, struct net_device_path *path);
+ *     Get the receiving path to reach the real device from the HW source address
  */
 struct net_device_ops {
 	int			(*ndo_init)(struct net_device *dev);
@@ -1501,6 +1503,8 @@ struct net_device_ops {
 	struct devlink_port *	(*ndo_get_devlink_port)(struct net_device *dev);
 	int                     (*ndo_fill_forward_path)(struct net_device_path_ctx *ctx,
                                                          struct net_device_path *path);
+	int                     (*ndo_fill_receive_path)(struct net_device_path_ctx *ctx,
+							 struct net_device_path *path);
 };
 
 /**
-- 
2.18.0