From 8341e1b35ca14f303f828f96f5e63719f80be88e Mon Sep 17 00:00:00 2001
From: Bo-Cun Chen <bc-bocun.chen@mediatek.com>
Date: Wed, 23 Aug 2023 11:36:05 +0800
Subject: [PATCH] 999-3007-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   | 14 +++-
 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   | 48 ++++++++++---
 include/linux/netdevice.h                     |  4 ++
 8 files changed, 131 insertions(+), 42 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 e5d4e12..0c54e12 100644
--- a/arch/arm64/boot/dts/mediatek/mt7986a.dtsi
+++ b/arch/arm64/boot/dts/mediatek/mt7986a.dtsi
@@ -496,6 +496,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 f019c56..81b68d6 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -2097,6 +2097,7 @@ static int mtk_poll_rx(struct napi_struct *napi, int budget,
 	u8 *data, *new_data;
 	struct mtk_rx_dma_v2 *rxd, trxd;
 	int done = 0;
+	int i;
 
 	if (unlikely(!ring))
 		goto rx_done;
@@ -2209,14 +2210,20 @@ static int mtk_poll_rx(struct napi_struct *napi, int budget,
 
 #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) {
@@ -3588,8 +3595,12 @@ static int mtk_open(struct net_device *dev)
 		regmap_write(eth->sgmii->pcs[id].regmap,
 			     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);
 
@@ -3671,8 +3682,10 @@ static int mtk_stop(struct net_device *dev)
 
 	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;
 }
@@ -4958,15 +4971,35 @@ static int mtk_probe(struct platform_device *pdev)
 	}
 
 	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 8675c86..fad3e73 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
@@ -130,7 +130,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 */
@@ -1856,7 +1861,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;
 };
 
@@ -1918,9 +1924,11 @@ int mtk_usxgmii_init(struct mtk_eth *eth, struct device_node *r);
 int mtk_toprgu_init(struct mtk_eth *eth, struct device_node *r);
 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);
 u32 mtk_rss_indr_table(struct mtk_rss_params *rss_params, int index);
+
+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 7506670..0ff1fef 100755
--- a/drivers/net/ethernet/mediatek/mtk_ppe.c
+++ b/drivers/net/ethernet/mediatek/mtk_ppe.c
@@ -696,7 +696,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;
@@ -715,6 +715,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);
@@ -723,8 +724,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 7012351..86bbac8 100644
--- a/drivers/net/ethernet/mediatek/mtk_ppe.h
+++ b/drivers/net/ethernet/mediatek/mtk_ppe.h
@@ -9,8 +9,10 @@
 #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
 
@@ -299,6 +301,7 @@ struct mtk_flow_entry {
 		};
 	};
 	u8 type;
+	s8 ppe_index;
 	s8 wed_index;
 	u16 hash;
 	union {
@@ -318,6 +321,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;
@@ -330,7 +334,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);
 
@@ -381,6 +385,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 a591ab1..f4ebe59 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 77594f3..18f6333
--- a/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
+++ b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
@@ -229,9 +229,12 @@ 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 ppe_index = 0;
 	int wed_index = -1;
 	u16 addr_type = 0;
 	u8 l4proto = 0;
@@ -245,6 +248,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;
 	}
@@ -347,6 +354,20 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
 	if (err)
 		return err;
 
+#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);
+		ppe_index = path.mtk_wdma.wdma_idx;
+		if (ppe_index >= eth->ppe_num) {
+			if (printk_ratelimit())
+				pr_info("[%s] PPE%d doesn't exist, please check mtketh-ppe-num in dts !\n", __func__, ppe_index);
+
+			return -EINVAL;
+		}
+	}
+#endif
+
 	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS)) {
 		struct flow_match_ports ports;
 
@@ -440,9 +461,10 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
 
 	entry->cookie = f->cookie;
 	memcpy(&entry->data, &foe, sizeof(entry->data));
+	entry->ppe_index = ppe_index;
 	entry->wed_index = wed_index;
 
-	if (mtk_foe_entry_commit(eth->ppe, entry) < 0)
+	if (mtk_foe_entry_commit(eth->ppe[ppe_index], entry) < 0)
 		goto free;
 
 	err = rhashtable_insert_fast(&eth->flow_table, &entry->node,
@@ -453,7 +475,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[ppe_index], entry);
 free:
 	kfree(entry);
 	if (wed_index >= 0)
@@ -465,13 +487,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)
@@ -486,13 +510,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;
@@ -543,10 +569,12 @@ 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;
 
-	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;
@@ -603,9 +631,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 59a3e96..0967dc2 100644
--- a/include/linux/netdevice.h
+++ b/include/linux/netdevice.h
@@ -1311,6 +1311,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);
@@ -1510,6 +1512,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