Revert "HSD #16020988342: net: stmmac: Fix FPE handshaking flow"

This reverts commit a555202.

drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c

@@ -1243,8 +1243,7 @@ const struct stmmac_ops dwmac410_ops = {
 	.config_l4_filter = dwmac4_config_l4_filter,
 	.est_configure = dwmac5_est_configure,
 	.est_irq_status = dwmac5_est_irq_status,
-	.fpe_tx_configure = dwmac5_fpe_tx_configure,
-	.fpe_rx_configure = dwmac5_fpe_rx_configure,
+	.fpe_configure = dwmac5_fpe_configure,
 	.fpe_send_mpacket = dwmac5_fpe_send_mpacket,
 	.fpe_irq_status = dwmac5_fpe_irq_status,
 	.add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr,
@@ -1297,8 +1296,7 @@ const struct stmmac_ops dwmac510_ops = {
 	.config_l4_filter = dwmac4_config_l4_filter,
 	.est_configure = dwmac5_est_configure,
 	.est_irq_status = dwmac5_est_irq_status,
-	.fpe_tx_configure = dwmac5_fpe_tx_configure,
-	.fpe_rx_configure = dwmac5_fpe_rx_configure,
+	.fpe_configure = dwmac5_fpe_configure,
 	.fpe_send_mpacket = dwmac5_fpe_send_mpacket,
 	.fpe_irq_status = dwmac5_fpe_irq_status,
 	.add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr,

drivers/net/ethernet/stmicro/stmmac/dwmac5.c

@@ -710,28 +710,24 @@ void dwmac5_est_irq_status(void __iomem *ioaddr, struct net_device *dev,
 	}
 }
 
-void dwmac5_fpe_tx_configure(void __iomem *ioaddr, struct stmmac_fpe_cfg *cfg,
-			  u32 num_txq, u32 txqpec, bool enable)
+void dwmac5_fpe_configure(void __iomem *ioaddr, struct stmmac_fpe_cfg *cfg,
+			  u32 num_txq, u32 num_rxq, u32 txqpec, bool enable)
 {
+	u32 value;
+
 	if (enable) {
 		cfg->fpe_csr = EFPE;
 		cfg->fpe_csr = (txqpec << PEC_SHIFT);
+		value = readl(ioaddr + GMAC_RXQ_CTRL1);
+		value &= ~GMAC_RXQCTRL_FPRQ;
+		value |= (num_rxq - 1) << GMAC_RXQCTRL_FPRQ_SHIFT;
+		writel(value, ioaddr + GMAC_RXQ_CTRL1);
 	} else {
 		cfg->fpe_csr = 0;
 	}
 	writel(cfg->fpe_csr, ioaddr + MAC_FPE_CTRL_STS);
 }
 
-void dwmac5_fpe_rx_configure(void __iomem *ioaddr, u32 num_rxq)
-{
-	u32 value;
-
-	value = readl(ioaddr + GMAC_RXQ_CTRL1);
-	value &= ~GMAC_RXQCTRL_FPRQ;
-	value |= (num_rxq - 1) << GMAC_RXQCTRL_FPRQ_SHIFT;
-	writel(value, ioaddr + GMAC_RXQ_CTRL1);
-}
-
 int dwmac5_fpe_irq_status(void __iomem *ioaddr, struct net_device *dev)
 {
 	u32 value;

drivers/net/ethernet/stmicro/stmmac/dwmac5.h

@@ -157,9 +157,8 @@ int dwmac5_est_configure(void __iomem *ioaddr, struct stmmac_est *cfg,
 			 unsigned int ptp_rate);
 void dwmac5_est_irq_status(void __iomem *ioaddr, struct net_device *dev,
 			   struct stmmac_extra_stats *x, u32 txqcnt);
-void dwmac5_fpe_tx_configure(void __iomem *ioaddr, struct stmmac_fpe_cfg *cfg,
-			  u32 num_txq, u32 txqpec, bool enable);
-void dwmac5_fpe_rx_configure(void __iomem *ioaddr, u32 num_rxq);
+void dwmac5_fpe_configure(void __iomem *ioaddr, struct stmmac_fpe_cfg *cfg,
+			  u32 num_txq, u32 num_rxq, u32 txqpec, bool enable);
 void dwmac5_fpe_send_mpacket(void __iomem *ioaddr,
 			     struct stmmac_fpe_cfg *cfg,
 			     enum stmmac_mpacket_type type);

drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c

@@ -1890,37 +1890,35 @@ static void dwxgmac3_est_irq_status(void __iomem *ioaddr,
 	}
 }
 
-static void dwxgmac3_fpe_tx_configure(void __iomem *ioaddr, struct stmmac_fpe_cfg *cfg,
-				   u32 num_txq, u32 txqpec, bool enable)
+static void dwxgmac3_fpe_configure(void __iomem *ioaddr, struct stmmac_fpe_cfg *cfg,
+				   u32 num_txq, u32 num_rxq, u32 txqpec,
+				   bool enable)
 {
 	u32 value;
 	u32 txqmask = (1 << num_txq) - 1;
 
-	if (enable) {
-		value = readl(ioaddr + XGMAC_MTL_FPE_CTRL_STS);
-		value &= ~(txqmask << XGMAC_PEC_SHIFT);
-		value |= (txqpec << XGMAC_PEC_SHIFT);
-		writel(value, ioaddr + XGMAC_MTL_FPE_CTRL_STS);
-
-		value = readl(ioaddr + XGMAC_FPE_CTRL_STS);
-		value |= XGMAC_EFPE;
-		writel(value, ioaddr + XGMAC_FPE_CTRL_STS);
-	}
-	else {
+	if (!enable) {
 		value = readl(ioaddr + XGMAC_FPE_CTRL_STS);
+
 		value &= ~XGMAC_EFPE;
+
 		writel(value, ioaddr + XGMAC_FPE_CTRL_STS);
+		return;
 	}
-}
-
-static void dwxgmac3_fpe_rx_configure(void __iomem *ioaddr, u32 num_rxq)
-{
-	u32 value;
 
 	value = readl(ioaddr + XGMAC_RXQ_CTRL1);
 	value &= ~XGMAC_RQ;
 	value |= (num_rxq - 1) << XGMAC_RQ_SHIFT;
 	writel(value, ioaddr + XGMAC_RXQ_CTRL1);
+
+	value = readl(ioaddr + XGMAC_MTL_FPE_CTRL_STS);
+	value &= ~(txqmask << XGMAC_PEC_SHIFT);
+	value |= (txqpec << XGMAC_PEC_SHIFT);
+	writel(value, ioaddr + XGMAC_MTL_FPE_CTRL_STS);
+
+	value = readl(ioaddr + XGMAC_FPE_CTRL_STS);
+	value |= XGMAC_EFPE;
+	writel(value, ioaddr + XGMAC_FPE_CTRL_STS);
 }
 
 static void dwxgmac3_fpe_send_mpacket(void __iomem *ioaddr,
@@ -2017,8 +2015,7 @@ const struct stmmac_ops dwxgmac210_ops = {
 	.set_arp_offload = dwxgmac2_set_arp_offload,
 	.est_configure = dwxgmac3_est_configure,
 	.est_irq_status = dwxgmac3_est_irq_status,
-	.fpe_tx_configure = dwxgmac3_fpe_tx_configure,
-	.fpe_rx_configure = dwxgmac3_fpe_rx_configure,
+	.fpe_configure = dwxgmac3_fpe_configure,
 	.fpe_send_mpacket = dwxgmac3_fpe_send_mpacket,
 	.fpe_irq_status = dwxgmac3_fpe_irq_status,
 	.rx_hw_vlan = dwxgmac2_rx_hw_vlan,
@@ -2083,8 +2080,7 @@ const struct stmmac_ops dwxlgmac2_ops = {
 	.config_l4_filter = dwxgmac2_config_l4_filter,
 	.set_arp_offload = dwxgmac2_set_arp_offload,
 	.est_configure = dwxgmac3_est_configure,
-	.fpe_tx_configure = dwxgmac3_fpe_tx_configure,
-	.fpe_rx_configure = dwxgmac3_fpe_rx_configure,
+	.fpe_configure = dwxgmac3_fpe_configure,
 	.rx_hw_vlan = dwxgmac2_rx_hw_vlan,
 	.set_hw_vlan_mode = dwxgmac2_set_hw_vlan_mode,
 };

drivers/net/ethernet/stmicro/stmmac/hwif.h

@@ -424,10 +424,9 @@ struct stmmac_ops {
 			     unsigned int ptp_rate);
 	void (*est_irq_status)(void __iomem *ioaddr, struct net_device *dev,
 			       struct stmmac_extra_stats *x, u32 txqcnt);
-	void (*fpe_tx_configure)(void __iomem *ioaddr, struct stmmac_fpe_cfg *cfg,
-				 u32 num_txq, u32 txqpec,
-				 bool enable);
-	void (*fpe_rx_configure)(void __iomem *ioaddr, u32 num_rxq);
+	void (*fpe_configure)(void __iomem *ioaddr, struct stmmac_fpe_cfg *cfg,
+			      u32 num_txq, u32 num_rxq, u32 txqpec,
+			      bool enable);
 	void (*fpe_send_mpacket)(void __iomem *ioaddr,
 				 struct stmmac_fpe_cfg *cfg,
 				 enum stmmac_mpacket_type type);
@@ -536,10 +535,8 @@ struct stmmac_ops {
 	stmmac_do_callback(__priv, mac, est_configure, __args)
 #define stmmac_est_irq_status(__priv, __args...) \
 	stmmac_do_void_callback(__priv, mac, est_irq_status, __args)
-#define stmmac_fpe_tx_configure(__priv, __args...) \
-	stmmac_do_void_callback(__priv, mac, fpe_tx_configure, __args)
-#define stmmac_fpe_rx_configure(__priv, __args...) \
-	stmmac_do_void_callback(__priv, mac, fpe_rx_configure, __args)
+#define stmmac_fpe_configure(__priv, __args...) \
+	stmmac_do_void_callback(__priv, mac, fpe_configure, __args)
 #define stmmac_fpe_send_mpacket(__priv, __args...) \
 	stmmac_do_void_callback(__priv, mac, fpe_send_mpacket, __args)
 #define stmmac_fpe_irq_status(__priv, __args...) \

drivers/net/ethernet/stmicro/stmmac/stmmac_main.c

@@ -3454,8 +3454,6 @@ static int stmmac_hw_setup(struct net_device *dev, bool ptp_register)
 	stmmac_start_all_dma(priv);
 
 	if (priv->dma_cap.fpesel) {
-		stmmac_fpe_rx_configure(priv, priv->ioaddr,
-				     priv->plat->rx_queues_to_use);
 		stmmac_fpe_start_wq(priv);
 
 		if (priv->plat->fpe_cfg->enable)
@@ -5873,22 +5871,21 @@ static void stmmac_fpe_event_status(struct stmmac_priv *priv, int status)
 	enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state;
 	bool *hs_enable = &fpe_cfg->hs_enable;
 
+	if (status == FPE_EVENT_UNKNOWN || !*hs_enable)
+		return;
+
 	/* If LP has sent verify mPacket, LP is FPE capable */
 	if ((status & FPE_EVENT_RVER) == FPE_EVENT_RVER) {
 		if (*lp_state < FPE_STATE_CAPABLE)
 			*lp_state = FPE_STATE_CAPABLE;
 
-		/* If FPE is supported by default the rx side FPE is enabled.
-		 * And this callback will be called only if FPE is supported. So
-		 * quickly send response mPacket.
-		 */
-		stmmac_fpe_send_mpacket(priv, priv->ioaddr, fpe_cfg,
-					MPACKET_RESPONSE);
+		/* If user has requested FPE enable, quickly response */
+		if (*hs_enable)
+			stmmac_fpe_send_mpacket(priv, priv->ioaddr,
+						fpe_cfg,
+						MPACKET_RESPONSE);
 	}
 
-	if (status == FPE_EVENT_UNKNOWN || !*hs_enable)
-		return;
-
 	/* If Local has sent verify mPacket, Local is FPE capable */
 	if ((status & FPE_EVENT_TVER) == FPE_EVENT_TVER) {
 		if (*lo_state < FPE_STATE_CAPABLE)
@@ -5899,6 +5896,10 @@ static void stmmac_fpe_event_status(struct stmmac_priv *priv, int status)
 	if ((status & FPE_EVENT_RRSP) == FPE_EVENT_RRSP)
 		*lp_state = FPE_STATE_ENTERING_ON;
 
+	/* If Local has sent response mPacket, Local is entering FPE ON */
+	if ((status & FPE_EVENT_TRSP) == FPE_EVENT_TRSP)
+		*lo_state = FPE_STATE_ENTERING_ON;
+
 	if (!test_bit(__FPE_REMOVING, &priv->fpe_task_state) &&
 	    !test_and_set_bit(__FPE_TASK_SCHED, &priv->fpe_task_state) &&
 	    priv->fpe_wq) {
@@ -5927,8 +5928,8 @@ static void stmmac_common_interrupt(struct stmmac_priv *priv)
 	if (priv->dma_cap.fpesel) {
 		int status = stmmac_fpe_irq_status(priv, priv->ioaddr,
 						   priv->dev);
-		if (status >= 0)
-			stmmac_fpe_event_status(priv, status);
+
+		stmmac_fpe_event_status(priv, status);
 	}
 
 	/* To handle GMAC own interrupts */
@@ -7297,11 +7298,13 @@ static void stmmac_fpe_lp_task(struct work_struct *work)
 		if (*lo_state == FPE_STATE_OFF || !*hs_enable)
 			break;
 
-		if (*lo_state == FPE_STATE_CAPABLE &&
+		if (*lo_state == FPE_STATE_ENTERING_ON &&
 		    *lp_state == FPE_STATE_ENTERING_ON) {
-			stmmac_fpe_tx_configure(priv, priv->ioaddr, fpe_cfg,
-						priv->plat->tx_queues_to_use,
-						*txqpec, *enable);
+			stmmac_fpe_configure(priv, priv->ioaddr,
+					     fpe_cfg,
+					     priv->plat->tx_queues_to_use,
+					     priv->plat->rx_queues_to_use,
+					     *txqpec, *enable);
 
 			netdev_info(priv->dev, "configured FPE\n");
 
@@ -7804,10 +7807,10 @@ int stmmac_suspend(struct device *dev)
 
 	if (priv->dma_cap.fpesel) {
 		/* Disable FPE */
-		stmmac_fpe_tx_configure(priv, priv->ioaddr,
-					priv->plat->fpe_cfg,
-					priv->plat->tx_queues_to_use,
-					0, false);
+		stmmac_fpe_configure(priv, priv->ioaddr,
+				     priv->plat->fpe_cfg,
+				     priv->plat->tx_queues_to_use,
+				     priv->plat->rx_queues_to_use, 0, false);
 
 		stmmac_fpe_handshake(priv, false);
 		stmmac_fpe_stop_wq(priv);

drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c

@@ -1217,10 +1217,11 @@ static int tc_taprio_configure(struct stmmac_priv *priv,
 	}
 
 	priv->plat->fpe_cfg->enable = false;
-	stmmac_fpe_tx_configure(priv, priv->ioaddr,
-				priv->plat->fpe_cfg,
-				priv->plat->tx_queues_to_use,
-				0, false);
+	stmmac_fpe_configure(priv, priv->ioaddr,
+			     priv->plat->fpe_cfg,
+			     priv->plat->tx_queues_to_use,
+			     priv->plat->rx_queues_to_use,
+			     0, false);
 	netdev_info(priv->dev, "disabled FPE\n");
 
 	stmmac_fpe_handshake(priv, false);