[PATCH net-next v1 6/7] net: stmmac: xgmac: complete FPE support

[Furong Xu <0x1207@gmail.com>]

FPE implementation for DWMAC4 and DWXGMAC differs only for:
1) Offset address of MAC_FPE_CTRL_STS
2) FPRQ(Frame Preemption Residue Queue) field in MAC_RxQ_Ctrl1

Introduce dwxgmac_fpe_ops to complete the FPE implementation for DWXGMAC.

Signed-off-by: Furong Xu <0x1207@gmail.com>
---
 drivers/net/ethernet/stmicro/stmmac/hwif.c    |  2 +
 drivers/net/ethernet/stmicro/stmmac/hwif.h    |  1 +
 .../net/ethernet/stmicro/stmmac/stmmac_fpe.c  | 37 +++++++++++++++++++
 .../net/ethernet/stmicro/stmmac/stmmac_fpe.h  |  1 +
 4 files changed, 41 insertions(+)

diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.c b/drivers/net/ethernet/stmicro/stmmac/hwif.c
index 655012ffbc0a..f13ed91b498f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.c
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.c
@@ -259,6 +259,7 @@ static const struct stmmac_hwif_entry {
 		.tc = &dwmac510_tc_ops,
 		.mmc = &dwxgmac_mmc_ops,
 		.est = &dwmac510_est_ops,
+		.fpe = &dwxgmac_fpe_ops,
 		.setup = dwxgmac2_setup,
 		.quirks = NULL,
 	}, {
@@ -280,6 +281,7 @@ static const struct stmmac_hwif_entry {
 		.tc = &dwmac510_tc_ops,
 		.mmc = &dwxgmac_mmc_ops,
 		.est = &dwmac510_est_ops,
+		.fpe = &dwxgmac_fpe_ops,
 		.setup = dwxlgmac2_setup,
 		.quirks = stmmac_dwxlgmac_quirks,
 	},
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index 7b19614c611d..81ce8ede2641 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -686,6 +686,7 @@ extern const struct stmmac_mmc_ops dwmac_mmc_ops;
 extern const struct stmmac_mmc_ops dwxgmac_mmc_ops;
 extern const struct stmmac_est_ops dwmac510_est_ops;
 extern const struct stmmac_fpe_ops dwmac4_fpe_ops;
+extern const struct stmmac_fpe_ops dwxgmac_fpe_ops;
 
 #define GMAC_VERSION		0x00000020	/* GMAC CORE Version */
 #define GMAC4_VERSION		0x00000110	/* GMAC4+ CORE Version */
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_fpe.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_fpe.c
index 97e404fac56a..c6894d5263c9 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_fpe.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_fpe.c
@@ -7,6 +7,7 @@
 #include "stmmac.h"
 #include "stmmac_fpe.h"
 #include "dwmac4.h"
+#include "dwxgmac2.h"
 
 static int __fpe_irq_status(void __iomem *ioaddr, struct net_device *dev)
 {
@@ -92,3 +93,39 @@ const struct stmmac_fpe_ops dwmac4_fpe_ops = {
 	.irq_status = dwmac4_fpe_irq_status,
 	.send_mpacket = dwmac4_fpe_send_mpacket,
 };
+
+static void dwxgmac_fpe_configure(void __iomem *ioaddr,
+				  struct stmmac_fpe_cfg *cfg,
+				  u32 num_txq, u32 num_rxq, bool enable)
+{
+	u32 value;
+
+	if (enable) {
+		cfg->fpe_csr = FPE_CTRL_STS_EFPE;
+		value = readl(ioaddr + XGMAC_RXQ_CTRL1);
+		value &= ~XGMAC_FPRQ;
+		value |= (num_rxq - 1) << XGMAC_FPRQ_SHIFT;
+		writel(value, ioaddr + XGMAC_RXQ_CTRL1);
+	} else {
+		cfg->fpe_csr = 0;
+	}
+
+	writel(cfg->fpe_csr, ioaddr + FPE_CTRL_STS_XGMAC_OFFSET);
+}
+
+static int dwxgmac_fpe_irq_status(void __iomem *ioaddr, struct net_device *dev)
+{
+	return __fpe_irq_status(ioaddr + FPE_CTRL_STS_XGMAC_OFFSET, dev);
+}
+
+static void dwxgmac_fpe_send_mpacket(void __iomem *ioaddr, struct stmmac_fpe_cfg *cfg,
+				     enum stmmac_mpacket_type type)
+{
+	__fpe_send_mpacket(ioaddr + FPE_CTRL_STS_XGMAC_OFFSET, cfg, type);
+}
+
+const struct stmmac_fpe_ops dwxgmac_fpe_ops = {
+	.configure = dwxgmac_fpe_configure,
+	.irq_status = dwxgmac_fpe_irq_status,
+	.send_mpacket = dwxgmac_fpe_send_mpacket,
+};
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_fpe.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_fpe.h
index efdb5536e856..b74cf8f2c2f2 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_fpe.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_fpe.h
@@ -5,6 +5,7 @@
  */
 
 #define FPE_CTRL_STS_GMAC4_OFFSET	0x00000234
+#define FPE_CTRL_STS_XGMAC_OFFSET	0x00000280
 
 #define FPE_CTRL_STS_TRSP		BIT(19)
 #define FPE_CTRL_STS_TVER		BIT(18)
-- 
2.34.1