[PATCH] net: stmmac: replace FPE workqueue with timer

[Vladimir Oltean]

What remains in the fpe_task after decoupling RX from TX appears
overengineered to use a workqueue. A timer which retransmits Verify
mPackets until the verify_limit expires, or enables transmission on
success, seems enough.

In the INITIAL state, the timer sends MPACKET_VERIFY. Eventually the
stmmac_fpe_event_status() IRQ fires and advances the state to VERIFYING,
then rearms the timer after verify_time ms. If a subsequent IRQ comes in
and modifies the state to SUCCEEDED after getting MPACKET_RESPONSE, the
timer sees this. It must enable the EFPE bit now. Otherwise, it
decrements the verify_limit counter and tries again. Eventually it
moves the status to FAILED, from which the IRQ cannot move it anywhere
else, except for another stmmac_fpe_apply() call.

Signed-off-by: Vladimir Oltean <vladimir.oltean@xxxxxxx>
---
 drivers/net/ethernet/stmicro/stmmac/stmmac.h  |  16 +-
 .../ethernet/stmicro/stmmac/stmmac_ethtool.c  |  35 +--
 .../net/ethernet/stmicro/stmmac/stmmac_main.c | 212 +++++++-----------
 3 files changed, 100 insertions(+), 163 deletions(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index 407b59f2783f..dd15f71e1663 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -152,21 +152,18 @@ enum stmmac_mpacket_type {
 	MPACKET_RESPONSE = 1,
 };
 
-enum stmmac_fpe_task_state_t {
-	__FPE_REMOVING,
-	__FPE_TASK_SCHED,
-};
-
 struct stmmac_fpe_cfg {
 	/* Serialize access to MAC Merge state between ethtool requests
 	 * and link state updates.
 	 */
 	spinlock_t lock;
-
+	struct timer_list verify_timer;
 	u32 fpe_csr;				/* MAC_FPE_CTRL_STS reg cache */
 	u32 verify_time;			/* see ethtool_mm_state */
 	bool pmac_enabled;			/* see ethtool_mm_state */
 	bool verify_enabled;			/* see ethtool_mm_state */
+	bool tx_enabled;
+	int verify_limit;
 	enum ethtool_mm_verify_status status;
 };
 
@@ -364,10 +361,6 @@ struct stmmac_priv {
 	struct work_struct service_task;
 
 	/* Frame Preemption feature (FPE) */
-	unsigned long fpe_task_state;
-	struct workqueue_struct *fpe_wq;
-	struct work_struct fpe_task;
-	char wq_name[IFNAMSIZ + 4];
 	struct stmmac_fpe_cfg fpe_cfg;
 
 	/* TC Handling */
@@ -422,7 +415,8 @@ bool stmmac_eee_init(struct stmmac_priv *priv);
 int stmmac_reinit_queues(struct net_device *dev, u32 rx_cnt, u32 tx_cnt);
 int stmmac_reinit_ringparam(struct net_device *dev, u32 rx_size, u32 tx_size);
 int stmmac_bus_clks_config(struct stmmac_priv *priv, bool enabled);
-void stmmac_fpe_handshake(struct stmmac_priv *priv, bool enable);
+void stmmac_fpe_apply(struct stmmac_priv *priv);
+void stmmac_fpe_verify_timer_arm(struct stmmac_fpe_cfg *fpe_cfg);
 
 static inline bool stmmac_xdp_is_enabled(struct stmmac_priv *priv)
 {
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
index a8cdcacecc26..3eb5344e2412 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
@@ -1293,14 +1293,7 @@ static int stmmac_get_mm(struct net_device *ndev,
 	 * variable has a range between 1 and 128 ms inclusive. Limit to that.
 	 */
 	state->max_verify_time = 128;
-
-	/* Cannot read MAC_FPE_CTRL_STS register here, or FPE interrupt events
-	 * can be lost.
-	 *
-	 * See commit 37e4b8df27bc ("net: stmmac: fix FPE events losing")
-	 */
-	state->tx_enabled = !!(priv->fpe_cfg.fpe_csr == EFPE);
-
+	state->tx_enabled = priv->fpe_cfg.tx_enabled;
 	/* FPE active if common tx_enabled and verification success or disabled (forced) */
 	state->tx_active = state->tx_enabled &&
 			   (state->verify_status == ETHTOOL_MM_VERIFY_STATUS_SUCCEEDED ||
@@ -1326,34 +1319,28 @@ static int stmmac_set_mm(struct net_device *ndev, struct ethtool_mm_cfg *cfg,
 	if (!priv->dma_cap.fpesel)
 		return -EOPNOTSUPP;
 
-	/* Wait for the fpe_task that's currently in progress to finish */
-	flush_workqueue(priv->fpe_wq);
-
 	err = ethtool_mm_frag_size_min_to_add(cfg->tx_min_frag_size,
 					      &add_frag_size, extack);
 	if (err)
 		return err;
 
-	spin_lock_irqsave(&priv->fpe_cfg.lock, flags);
+	/* Wait for the verification that's currently in progress to finish */
+	del_timer_sync(&fpe_cfg->verify_timer);
+
+	spin_lock_irqsave(&fpe_cfg->lock, flags);
 
 	fpe_cfg->pmac_enabled = cfg->pmac_enabled;
+	fpe_cfg->tx_enabled = cfg->tx_enabled;
 	fpe_cfg->verify_time = cfg->verify_time;
 	fpe_cfg->verify_enabled = cfg->verify_enabled;
-
-	stmmac_fpe_configure(priv, priv->ioaddr, fpe_cfg,
-			     priv->plat->tx_queues_to_use,
-			     priv->plat->rx_queues_to_use,
-			     cfg->tx_enabled, cfg->pmac_enabled);
+	fpe_cfg->verify_limit = 3; /* IEEE 802.3 constant */
+	if (!cfg->verify_enabled)
+		fpe_cfg->status = ETHTOOL_MM_VERIFY_STATUS_DISABLED;
 
 	stmmac_fpe_set_add_frag_size(priv, priv->ioaddr, add_frag_size);
+	stmmac_fpe_apply(priv);
 
-	if (cfg->verify_enabled)
-		stmmac_fpe_send_mpacket(priv, priv->ioaddr, fpe_cfg,
-					MPACKET_VERIFY);
-	else
-		fpe_cfg->status = ETHTOOL_MM_VERIFY_STATUS_DISABLED;
-
-	spin_unlock_irqrestore(&priv->fpe_cfg.lock, flags);
+	spin_unlock_irqrestore(&fpe_cfg->lock, flags);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index a5d01162fcc5..fa74504f3ad5 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -971,19 +971,22 @@ static void stmmac_fpe_link_state_handle(struct stmmac_priv *priv, bool is_up)
 	struct stmmac_fpe_cfg *fpe_cfg = &priv->fpe_cfg;
 	unsigned long flags;
 
-	spin_lock_irqsave(&priv->fpe_cfg.lock, flags);
+	del_timer_sync(&fpe_cfg->verify_timer);
 
-	if (!fpe_cfg->pmac_enabled)
-		goto __unlock_out;
+	spin_lock_irqsave(&fpe_cfg->lock, flags);
 
-	if (is_up && fpe_cfg->verify_enabled)
-		stmmac_fpe_send_mpacket(priv, priv->ioaddr, fpe_cfg,
-					MPACKET_VERIFY);
-	else
-		fpe_cfg->status = ETHTOOL_MM_VERIFY_STATUS_DISABLED;
+	if (is_up) {
+		/* New link => maybe new partner => new verification process */
+		stmmac_fpe_apply(priv);
+	} else {
+		/* No link => turn off EFPE */
+		stmmac_fpe_configure(priv, priv->ioaddr, fpe_cfg,
+				     priv->plat->tx_queues_to_use,
+				     priv->plat->rx_queues_to_use,
+				     false, false);
+	}
 
-__unlock_out:
-	spin_unlock_irqrestore(&priv->fpe_cfg.lock, flags);
+	spin_unlock_irqrestore(&fpe_cfg->lock, flags);
 }
 
 static void stmmac_mac_link_down(struct phylink_config *config,
@@ -3362,27 +3365,6 @@ static void stmmac_safety_feat_configuration(struct stmmac_priv *priv)
 	}
 }
 
-static int stmmac_fpe_start_wq(struct stmmac_priv *priv)
-{
-	char *name;
-
-	clear_bit(__FPE_TASK_SCHED, &priv->fpe_task_state);
-	clear_bit(__FPE_REMOVING,  &priv->fpe_task_state);
-
-	name = priv->wq_name;
-	sprintf(name, "%s-fpe", priv->dev->name);
-
-	priv->fpe_wq = create_singlethread_workqueue(name);
-	if (!priv->fpe_wq) {
-		netdev_err(priv->dev, "%s: Failed to create workqueue\n", name);
-
-		return -ENOMEM;
-	}
-	netdev_dbg(priv->dev, "FPE workqueue start");
-
-	return 0;
-}
-
 /**
  * stmmac_hw_setup - setup mac in a usable state.
  *  @dev : pointer to the device structure.
@@ -3537,22 +3519,6 @@ static int stmmac_hw_setup(struct net_device *dev, bool ptp_register)
 
 	stmmac_set_hw_vlan_mode(priv, priv->hw);
 
-	if (priv->dma_cap.fpesel) {
-		/* A SW reset just happened in stmmac_init_dma_engine(),
-		 * we should restore fpe_cfg to HW, or FPE will stop working
-		 * from suspend/resume.
-		 */
-		spin_lock(&priv->fpe_cfg.lock);
-		stmmac_fpe_configure(priv, priv->ioaddr,
-				     &priv->fpe_cfg,
-				     priv->plat->tx_queues_to_use,
-				     priv->plat->rx_queues_to_use,
-				     false, priv->fpe_cfg.pmac_enabled);
-		spin_unlock(&priv->fpe_cfg.lock);
-
-		stmmac_fpe_start_wq(priv);
-	}
-
 	return 0;
 }
 
@@ -4049,18 +4015,6 @@ static int stmmac_open(struct net_device *dev)
 	return ret;
 }
 
-static void stmmac_fpe_stop_wq(struct stmmac_priv *priv)
-{
-	set_bit(__FPE_REMOVING, &priv->fpe_task_state);
-
-	if (priv->fpe_wq) {
-		destroy_workqueue(priv->fpe_wq);
-		priv->fpe_wq = NULL;
-	}
-
-	netdev_dbg(priv->dev, "FPE workqueue stop");
-}
-
 /**
  *  stmmac_release - close entry point of the driver
  *  @dev : device pointer.
@@ -4108,22 +4062,8 @@ static int stmmac_release(struct net_device *dev)
 
 	stmmac_release_ptp(priv);
 
-	if (priv->dma_cap.fpesel) {
-		stmmac_fpe_stop_wq(priv);
-
-		/* stmmac_ethtool_ops.begin() guarantees that all ethtool
-		 * requests to fail with EBUSY when !netif_running()
-		 *
-		 * Prepare some params here, then fpe_cfg can keep consistent
-		 * with the register states after a SW reset by __stmmac_open().
-		 */
-		priv->fpe_cfg.pmac_enabled = false;
-		priv->fpe_cfg.verify_enabled = false;
-		priv->fpe_cfg.status = ETHTOOL_MM_VERIFY_STATUS_DISABLED;
-
-		/* Reset MAC_FPE_CTRL_STS reg cache */
-		priv->fpe_cfg.fpe_csr = 0;
-	}
+	if (priv->dma_cap.fpesel)
+		del_timer_sync(&priv->fpe_cfg.verify_timer);
 
 	pm_runtime_put(priv->device);
 
@@ -6030,11 +5970,7 @@ static void stmmac_fpe_event_status(struct stmmac_priv *priv, int status)
 	if ((status & FPE_EVENT_RRSP) == FPE_EVENT_RRSP)
 		fpe_cfg->status = ETHTOOL_MM_VERIFY_STATUS_SUCCEEDED;
 
-	if (!test_bit(__FPE_REMOVING, &priv->fpe_task_state) &&
-	    !test_and_set_bit(__FPE_TASK_SCHED, &priv->fpe_task_state) &&
-	    priv->fpe_wq) {
-		queue_work(priv->fpe_wq, &priv->fpe_task);
-	}
+	stmmac_fpe_verify_timer_arm(fpe_cfg);
 
 __unlock_out:
 	spin_unlock(&priv->fpe_cfg.lock);
@@ -7395,60 +7331,82 @@ int stmmac_reinit_ringparam(struct net_device *dev, u32 rx_size, u32 tx_size)
 	return ret;
 }
 
-static void stmmac_fpe_verify_task(struct work_struct *work)
+/**
+ * stmmac_fpe_verify_timer - Timer for MAC Merge verification
+ * @t:  timer_list struct containing private info
+ *
+ * Verify the MAC Merge capability in the local TX direction, by
+ * transmitting Verify mPackets up to 3 times. Wait until link
+ * partner responds with a Response mPacket, otherwise fail.
+ */
+static void stmmac_fpe_verify_timer(struct timer_list *t)
 {
-	struct stmmac_priv *priv = container_of(work, struct stmmac_priv,
-						fpe_task);
-	struct stmmac_fpe_cfg *fpe_cfg = &priv->fpe_cfg;
-	int verify_limit = 3; /* defined by 802.3 */
-	unsigned long flags;
-	u32 sleep_ms;
+	struct stmmac_fpe_cfg *fpe_cfg = from_timer(fpe_cfg, t, verify_timer);
+	struct stmmac_priv *priv = container_of(fpe_cfg, struct stmmac_priv,
+						fpe_cfg);
+	bool rearm = false;
 
-	spin_lock(&priv->fpe_cfg.lock);
-	sleep_ms = fpe_cfg->verify_time;
-	spin_unlock(&priv->fpe_cfg.lock);
+	spin_lock(&fpe_cfg->lock);
 
-	while (1) {
-		/* The initial VERIFY was triggered by linkup event or
-		 * stmmac_set_mm(), sleep then check MM_VERIFY_STATUS.
-		 */
-		msleep(sleep_ms);
-
-		if (!netif_running(priv->dev))
-			break;
-
-		spin_lock_irqsave(&priv->fpe_cfg.lock, flags);
-
-		if (fpe_cfg->status == ETHTOOL_MM_VERIFY_STATUS_DISABLED ||
-		    fpe_cfg->status == ETHTOOL_MM_VERIFY_STATUS_SUCCEEDED ||
-		    !fpe_cfg->pmac_enabled || !fpe_cfg->verify_enabled) {
-			spin_unlock_irqrestore(&priv->fpe_cfg.lock, flags);
-			break;
-		}
-
-		if (verify_limit == 0) {
-			fpe_cfg->verify_enabled = false;
+	switch (fpe_cfg->status) {
+	case ETHTOOL_MM_VERIFY_STATUS_INITIAL:
+	case ETHTOOL_MM_VERIFY_STATUS_VERIFYING:
+		stmmac_fpe_send_mpacket(priv, priv->ioaddr, fpe_cfg,
+					MPACKET_VERIFY);
+		if (fpe_cfg->verify_limit != 0) {
+			fpe_cfg->status = ETHTOOL_MM_VERIFY_STATUS_VERIFYING;
+			rearm = true;
+		} else {
 			fpe_cfg->status = ETHTOOL_MM_VERIFY_STATUS_FAILED;
-			stmmac_fpe_configure(priv, priv->ioaddr, fpe_cfg,
-					     priv->plat->tx_queues_to_use,
-					     priv->plat->rx_queues_to_use,
-					     false, fpe_cfg->pmac_enabled);
-			spin_unlock_irqrestore(&priv->fpe_cfg.lock, flags);
-			break;
 		}
+		fpe_cfg->verify_limit--;
+		break;
+	case ETHTOOL_MM_VERIFY_STATUS_SUCCEEDED:
+		stmmac_fpe_configure(priv, priv->ioaddr, fpe_cfg,
+				     priv->plat->tx_queues_to_use,
+				     priv->plat->rx_queues_to_use,
+				     true, true);
+		break;
+	default:
+		break;
+	}
 
-		if (fpe_cfg->status == ETHTOOL_MM_VERIFY_STATUS_VERIFYING)
-			stmmac_fpe_send_mpacket(priv, priv->ioaddr, fpe_cfg,
-						MPACKET_VERIFY);
-
-		sleep_ms = fpe_cfg->verify_time;
+	if (rearm) {
+		mod_timer(&fpe_cfg->verify_timer,
+			  msecs_to_jiffies(fpe_cfg->verify_time));
+	}
 
-		spin_unlock_irqrestore(&priv->fpe_cfg.lock, flags);
+	spin_unlock(&fpe_cfg->lock);
+}
 
-		verify_limit--;
+void stmmac_fpe_verify_timer_arm(struct stmmac_fpe_cfg *fpe_cfg)
+{
+	if (fpe_cfg->pmac_enabled && fpe_cfg->tx_enabled &&
+	    fpe_cfg->verify_enabled &&
+	    fpe_cfg->status != ETHTOOL_MM_VERIFY_STATUS_FAILED &&
+	    fpe_cfg->status != ETHTOOL_MM_VERIFY_STATUS_SUCCEEDED) {
+		mod_timer(&fpe_cfg->verify_timer,
+			  msecs_to_jiffies(fpe_cfg->verify_time));
 	}
+}
 
-	clear_bit(__FPE_TASK_SCHED, &priv->fpe_task_state);
+void stmmac_fpe_apply(struct stmmac_priv *priv)
+{
+	struct stmmac_fpe_cfg *fpe_cfg = &priv->fpe_cfg;
+
+	/* If verification is disabled, configure FPE right away.
+	 * Otherwise let the timer code do it.
+	 */
+	if (!fpe_cfg->verify_enabled) {
+		stmmac_fpe_configure(priv, priv->ioaddr, fpe_cfg,
+				     priv->plat->tx_queues_to_use,
+				     priv->plat->rx_queues_to_use,
+				     fpe_cfg->tx_enabled,
+				     fpe_cfg->pmac_enabled);
+	} else {
+		fpe_cfg->status = ETHTOOL_MM_VERIFY_STATUS_INITIAL;
+		stmmac_fpe_verify_timer_arm(fpe_cfg);
+	}
 }
 
 static int stmmac_xdp_rx_timestamp(const struct xdp_md *_ctx, u64 *timestamp)
@@ -7565,9 +7523,6 @@ int stmmac_dvr_probe(struct device *device,
 
 	INIT_WORK(&priv->service_task, stmmac_service_task);
 
-	/* Initialize FPE verify workqueue */
-	INIT_WORK(&priv->fpe_task, stmmac_fpe_verify_task);
-
 	/* Override with kernel parameters if supplied XXX CRS XXX
 	 * this needs to have multiple instances
 	 */
@@ -7733,6 +7688,7 @@ int stmmac_dvr_probe(struct device *device,
 	mutex_init(&priv->lock);
 
 	spin_lock_init(&priv->fpe_cfg.lock);
+	timer_setup(&priv->fpe_cfg.verify_timer, stmmac_fpe_verify_timer, 0);
 	priv->fpe_cfg.pmac_enabled = false;
 	priv->fpe_cfg.verify_time = 128; /* ethtool_mm_state.max_verify_time */
 	priv->fpe_cfg.verify_enabled = false;
@@ -7912,7 +7868,7 @@ int stmmac_suspend(struct device *dev)
 	rtnl_unlock();
 
 	if (priv->dma_cap.fpesel)
-		stmmac_fpe_stop_wq(priv);
+		del_timer_sync(&priv->fpe_cfg.verify_timer);
 
 	priv->speed = SPEED_UNKNOWN;
 	return 0;
-- 
2.34.1


--n5bc2hehdh25qze3--