Re: [PATCH net-next] net: stmmac: support FPE link partner hand-shaking procedure

[Marek Szyprowski]

Hi,

On 24.03.2021 10:07, mohammad.athari.ismail@xxxxxxxxx wrote:
> From: Ong Boon Leong <boon.leong.ong@xxxxxxxxx>
>
> In order to discover whether remote station supports frame preemption,
> local station sends verify mPacket and expects response mPacket in
> return from the remote station.
>
> So, we add the functions to send and handle event when verify mPacket
> and response mPacket are exchanged between the networked stations.
>
> The mechanism to handle different FPE states between local and remote
> station (link partner) is implemented using workqueue which starts a
> task each time there is some sign of verify & response mPacket exchange
> as check in FPE IRQ event. The task retries couple of times to try to
> spot the states that both stations are ready to enter FPE ON. This allows
> different end points to enable FPE at different time and verify-response
> mPacket can happen asynchronously. Ultimately, the task will only turn
> FPE ON when local station have both exchange response in both directions.
>
> Thanks to Voon Weifeng for implementing the core functions for detecting
> FPE events and send mPacket and phylink related change.
>
> Signed-off-by: Ong Boon Leong <boon.leong.ong@xxxxxxxxx>
> Co-developed-by: Voon Weifeng <weifeng.voon@xxxxxxxxx>
> Signed-off-by: Voon Weifeng <weifeng.voon@xxxxxxxxx>
> Co-developed-by: Tan Tee Min <tee.min.tan@xxxxxxxxx>
> Signed-off-by: Tan Tee Min <tee.min.tan@xxxxxxxxx>
> Co-developed-by: Mohammad Athari Bin Ismail <mohammad.athari.ismail@xxxxxxxxx>
> Signed-off-by: Mohammad Athari Bin Ismail <mohammad.athari.ismail@xxxxxxxxx>

This patch landed in today's linux-next as commit 5a5586112b92 ("net: 
stmmac: support FPE link partner hand-shaking procedure"). It causes the 
following NULL pointer dereference issue on various Amlogic SoC based 
boards:

  meson8b-dwmac ff3f0000.ethernet eth0: PHY [0.0:00] driver [RTL8211F 
Gigabit Ethernet] (irq=35)
  meson8b-dwmac ff3f0000.ethernet eth0: No Safety Features support found
  meson8b-dwmac ff3f0000.ethernet eth0: PTP not supported by HW
  meson8b-dwmac ff3f0000.ethernet eth0: configuring for phy/rgmii link mode
  Unable to handle kernel NULL pointer dereference at virtual address 
0000000000000001
  Mem abort info:
...
  user pgtable: 4k pages, 48-bit VAs, pgdp=00000000044eb000
  [0000000000000001] pgd=0000000000000000, p4d=0000000000000000
  Internal error: Oops: 96000004 [#1] PREEMPT SMP
  Modules linked in: dw_hdmi_i2s_audio dw_hdmi_cec meson_gxl realtek 
meson_gxbb_wdt snd_soc_meson_axg_sound_card dwmac_generic axg_audio 
meson_dw_hdmi crct10dif_ce snd_soc_meson_card_utils 
snd_soc_meson_axg_tdmout panfrost rc_odroid gpu_sched 
reset_meson_audio_arb meson_ir snd_soc_meson_g12a_tohdmitx 
snd_soc_meson_axg_frddr sclk_div clk_phase snd_soc_meson_codec_glue 
dwmac_meson8b snd_soc_meson_axg_fifo stmmac_platform meson_rng meson_drm 
stmmac rtc_meson_vrtc rng_core meson_canvas pwm_meson dw_hdmi 
mdio_mux_meson_g12a pcs_xpcs snd_soc_meson_axg_tdm_interface 
snd_soc_meson_axg_tdm_formatter nvmem_meson_efuse display_connector
  CPU: 1 PID: 7 Comm: kworker/u8:0 Not tainted 5.12.0-rc4-next-20210325+ 
#2747
  Hardware name: Hardkernel ODROID-C4 (DT)
  Workqueue: events_power_efficient phylink_resolve
  pstate: 20400009 (nzCv daif +PAN -UAO -TCO BTYPE=--)
  pc : stmmac_mac_link_up+0x14c/0x348 [stmmac]
  lr : stmmac_mac_link_up+0x284/0x348 [stmmac]
...
  Call trace:
   stmmac_mac_link_up+0x14c/0x348 [stmmac]
   phylink_resolve+0x104/0x420
   process_one_work+0x2a8/0x718
   worker_thread+0x48/0x460
   kthread+0x134/0x160
   ret_from_fork+0x10/0x18
  Code: b971ba60 350007c0 f958c260 f9402000 (39400401)
  ---[ end trace 0c9deb6c510228aa ]---

> ---
>   drivers/net/ethernet/stmicro/stmmac/common.h  |   7 +
>   .../net/ethernet/stmicro/stmmac/dwmac4_core.c |   8 +
>   drivers/net/ethernet/stmicro/stmmac/dwmac5.c  |  49 +++++
>   drivers/net/ethernet/stmicro/stmmac/dwmac5.h  |  11 ++
>   drivers/net/ethernet/stmicro/stmmac/hwif.h    |   7 +
>   drivers/net/ethernet/stmicro/stmmac/stmmac.h  |   7 +
>   .../net/ethernet/stmicro/stmmac/stmmac_main.c | 183 ++++++++++++++++++
>   .../net/ethernet/stmicro/stmmac/stmmac_tc.c   |  39 +++-
>   include/linux/stmmac.h                        |  27 +++
>   9 files changed, 331 insertions(+), 7 deletions(-)
>
> diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
> index 1c0c60bdf854..4511945df802 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/common.h
> +++ b/drivers/net/ethernet/stmicro/stmmac/common.h
> @@ -315,6 +315,13 @@ enum dma_irq_status {
>   #define	CORE_IRQ_RX_PATH_IN_LPI_MODE	(1 << 2)
>   #define	CORE_IRQ_RX_PATH_EXIT_LPI_MODE	(1 << 3)
>   
> +/* FPE defines */
> +#define FPE_EVENT_UNKNOWN		0
> +#define FPE_EVENT_TRSP			BIT(0)
> +#define FPE_EVENT_TVER			BIT(1)
> +#define FPE_EVENT_RRSP			BIT(2)
> +#define FPE_EVENT_RVER			BIT(3)
> +
>   #define CORE_IRQ_MTL_RX_OVERFLOW	BIT(8)
>   
>   /* Physical Coding Sublayer */
> diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
> index 29f765a246a0..95864f014ffa 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
> +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
> @@ -53,6 +53,10 @@ static void dwmac4_core_init(struct mac_device_info *hw,
>   	if (hw->pcs)
>   		value |= GMAC_PCS_IRQ_DEFAULT;
>   
> +	/* Enable FPE interrupt */
> +	if ((GMAC_HW_FEAT_FPESEL & readl(ioaddr + GMAC_HW_FEATURE3)) >> 26)
> +		value |= GMAC_INT_FPE_EN;
> +
>   	writel(value, ioaddr + GMAC_INT_EN);
>   }
>   
> @@ -1245,6 +1249,8 @@ const struct stmmac_ops dwmac410_ops = {
>   	.config_l4_filter = dwmac4_config_l4_filter,
>   	.est_configure = dwmac5_est_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,
>   	.del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr,
>   	.restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr,
> @@ -1294,6 +1300,8 @@ const struct stmmac_ops dwmac510_ops = {
>   	.config_l4_filter = dwmac4_config_l4_filter,
>   	.est_configure = dwmac5_est_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,
>   	.del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr,
>   	.restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr,
> diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac5.c b/drivers/net/ethernet/stmicro/stmmac/dwmac5.c
> index 0ae85f8adf67..5b010ebfede9 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/dwmac5.c
> +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac5.c
> @@ -707,3 +707,52 @@ void dwmac5_fpe_configure(void __iomem *ioaddr, u32 num_txq, u32 num_rxq,
>   	value |= EFPE;
>   	writel(value, ioaddr + MAC_FPE_CTRL_STS);
>   }
> +
> +int dwmac5_fpe_irq_status(void __iomem *ioaddr, struct net_device *dev)
> +{
> +	u32 value;
> +	int status;
> +
> +	status = FPE_EVENT_UNKNOWN;
> +
> +	value = readl(ioaddr + MAC_FPE_CTRL_STS);
> +
> +	if (value & TRSP) {
> +		status |= FPE_EVENT_TRSP;
> +		netdev_info(dev, "FPE: Respond mPacket is transmitted\n");
> +	}
> +
> +	if (value & TVER) {
> +		status |= FPE_EVENT_TVER;
> +		netdev_info(dev, "FPE: Verify mPacket is transmitted\n");
> +	}
> +
> +	if (value & RRSP) {
> +		status |= FPE_EVENT_RRSP;
> +		netdev_info(dev, "FPE: Respond mPacket is received\n");
> +	}
> +
> +	if (value & RVER) {
> +		status |= FPE_EVENT_RVER;
> +		netdev_info(dev, "FPE: Verify mPacket is received\n");
> +	}
> +
> +	return status;
> +}
> +
> +void dwmac5_fpe_send_mpacket(void __iomem *ioaddr, enum stmmac_mpacket_type type)
> +{
> +	u32 value;
> +
> +	value = readl(ioaddr + MAC_FPE_CTRL_STS);
> +
> +	if (type == MPACKET_VERIFY) {
> +		value &= ~SRSP;
> +		value |= SVER;
> +	} else {
> +		value &= ~SVER;
> +		value |= SRSP;
> +	}
> +
> +	writel(value, ioaddr + MAC_FPE_CTRL_STS);
> +}
> diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac5.h b/drivers/net/ethernet/stmicro/stmmac/dwmac5.h
> index 709bbfc9ae61..ff555d8b0cdf 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/dwmac5.h
> +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac5.h
> @@ -12,6 +12,12 @@
>   #define TMOUTEN				BIT(0)
>   
>   #define MAC_FPE_CTRL_STS		0x00000234
> +#define TRSP				BIT(19)
> +#define TVER				BIT(18)
> +#define RRSP				BIT(17)
> +#define RVER				BIT(16)
> +#define SRSP				BIT(2)
> +#define SVER				BIT(1)
>   #define EFPE				BIT(0)
>   
>   #define MAC_PPS_CONTROL			0x00000b70
> @@ -128,6 +134,8 @@
>   #define GMAC_RXQCTRL_VFFQ_SHIFT		17
>   #define GMAC_RXQCTRL_VFFQE		BIT(16)
>   
> +#define GMAC_INT_FPE_EN			BIT(17)
> +
>   int dwmac5_safety_feat_config(void __iomem *ioaddr, unsigned int asp);
>   int dwmac5_safety_feat_irq_status(struct net_device *ndev,
>   		void __iomem *ioaddr, unsigned int asp,
> @@ -145,5 +153,8 @@ void dwmac5_est_irq_status(void __iomem *ioaddr, struct net_device *dev,
>   			   struct stmmac_extra_stats *x, u32 txqcnt);
>   void dwmac5_fpe_configure(void __iomem *ioaddr, u32 num_txq, u32 num_rxq,
>   			  bool enable);
> +void dwmac5_fpe_send_mpacket(void __iomem *ioaddr,
> +			     enum stmmac_mpacket_type type);
> +int dwmac5_fpe_irq_status(void __iomem *ioaddr, struct net_device *dev);
>   
>   #endif /* __DWMAC5_H__ */
> diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
> index 692541c7b419..38cfc2cae129 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
> +++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
> @@ -397,6 +397,9 @@ struct stmmac_ops {
>   			       struct stmmac_extra_stats *x, u32 txqcnt);
>   	void (*fpe_configure)(void __iomem *ioaddr, u32 num_txq, u32 num_rxq,
>   			      bool enable);
> +	void (*fpe_send_mpacket)(void __iomem *ioaddr,
> +				 enum stmmac_mpacket_type type);
> +	int (*fpe_irq_status)(void __iomem *ioaddr, struct net_device *dev);
>   };
>   
>   #define stmmac_core_init(__priv, __args...) \
> @@ -497,6 +500,10 @@ struct stmmac_ops {
>   	stmmac_do_void_callback(__priv, mac, est_irq_status, __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...) \
> +	stmmac_do_callback(__priv, mac, fpe_irq_status, __args)
>   
>   /* PTP and HW Timer helpers */
>   struct stmmac_hwtimestamp {
> diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
> index 375c503d2df8..4faad331a4ca 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
> +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
> @@ -234,6 +234,12 @@ struct stmmac_priv {
>   	struct workqueue_struct *wq;
>   	struct work_struct service_task;
>   
> +	/* Workqueue for handling FPE hand-shaking */
> +	unsigned long fpe_task_state;
> +	struct workqueue_struct *fpe_wq;
> +	struct work_struct fpe_task;
> +	char wq_name[IFNAMSIZ + 4];
> +
>   	/* TC Handling */
>   	unsigned int tc_entries_max;
>   	unsigned int tc_off_max;
> @@ -273,6 +279,7 @@ 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);
>   
>   #if IS_ENABLED(CONFIG_STMMAC_SELFTESTS)
>   void stmmac_selftest_run(struct net_device *dev,
> diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
> index 8d7015d3a537..170296820af0 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
> +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
> @@ -971,6 +971,21 @@ static void stmmac_mac_an_restart(struct phylink_config *config)
>   	/* Not Supported */
>   }
>   
> +static void stmmac_fpe_link_state_handle(struct stmmac_priv *priv, bool is_up)
> +{
> +	struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg;
> +	enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state;
> +	enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state;
> +	bool *hs_enable = &fpe_cfg->hs_enable;
> +
> +	if (is_up && *hs_enable) {
> +		stmmac_fpe_send_mpacket(priv, priv->ioaddr, MPACKET_VERIFY);
> +	} else {
> +		*lo_state = FPE_EVENT_UNKNOWN;
> +		*lp_state = FPE_EVENT_UNKNOWN;
> +	}
> +}
> +
>   static void stmmac_mac_link_down(struct phylink_config *config,
>   				 unsigned int mode, phy_interface_t interface)
>   {
> @@ -981,6 +996,8 @@ static void stmmac_mac_link_down(struct phylink_config *config,
>   	priv->tx_lpi_enabled = false;
>   	stmmac_eee_init(priv);
>   	stmmac_set_eee_pls(priv, priv->hw, false);
> +
> +	stmmac_fpe_link_state_handle(priv, false);
>   }
>   
>   static void stmmac_mac_link_up(struct phylink_config *config,
> @@ -1079,6 +1096,8 @@ static void stmmac_mac_link_up(struct phylink_config *config,
>   		priv->tx_lpi_enabled = priv->eee_enabled;
>   		stmmac_set_eee_pls(priv, priv->hw, true);
>   	}
> +
> +	stmmac_fpe_link_state_handle(priv, true);
>   }
>   
>   static const struct phylink_mac_ops stmmac_phylink_mac_ops = {
> @@ -2793,6 +2812,26 @@ 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);
> +
> +	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_info(priv->dev, "FPE workqueue start");
> +
> +	return 0;
> +}
> +
>   /**
>    * stmmac_hw_setup - setup mac in a usable state.
>    *  @dev : pointer to the device structure.
> @@ -2929,6 +2968,13 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
>   	/* Start the ball rolling... */
>   	stmmac_start_all_dma(priv);
>   
> +	if (priv->dma_cap.fpesel) {
> +		stmmac_fpe_start_wq(priv);
> +
> +		if (priv->plat->fpe_cfg->enable)
> +			stmmac_fpe_handshake(priv, true);
> +	}
> +
>   	return 0;
>   }
>   
> @@ -3090,6 +3136,16 @@ 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);
> +
> +	netdev_info(priv->dev, "FPE workqueue stop");
> +}
> +
>   /**
>    *  stmmac_release - close entry point of the driver
>    *  @dev : device pointer.
> @@ -3139,6 +3195,9 @@ static int stmmac_release(struct net_device *dev)
>   
>   	pm_runtime_put(priv->device);
>   
> +	if (priv->dma_cap.fpesel)
> +		stmmac_fpe_stop_wq(priv);
> +
>   	return 0;
>   }
>   
> @@ -4280,6 +4339,48 @@ static int stmmac_set_features(struct net_device *netdev,
>   	return 0;
>   }
>   
> +static void stmmac_fpe_event_status(struct stmmac_priv *priv, int status)
> +{
> +	struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg;
> +	enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state;
> +	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 user has requested FPE enable, quickly response */
> +		if (*hs_enable)
> +			stmmac_fpe_send_mpacket(priv, priv->ioaddr,
> +						MPACKET_RESPONSE);
> +	}
> +
> +	/* If Local has sent verify mPacket, Local is FPE capable */
> +	if ((status & FPE_EVENT_TVER) == FPE_EVENT_TVER) {
> +		if (*lo_state < FPE_STATE_CAPABLE)
> +			*lo_state = FPE_STATE_CAPABLE;
> +	}
> +
> +	/* If LP has sent response mPacket, LP is entering FPE ON */
> +	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) {
> +		queue_work(priv->fpe_wq, &priv->fpe_task);
> +	}
> +}
> +
>   /**
>    *  stmmac_interrupt - main ISR
>    *  @irq: interrupt number.
> @@ -4318,6 +4419,13 @@ static irqreturn_t stmmac_interrupt(int irq, void *dev_id)
>   		stmmac_est_irq_status(priv, priv->ioaddr, priv->dev,
>   				      &priv->xstats, tx_cnt);
>   
> +	if (priv->dma_cap.fpesel) {
> +		int status = stmmac_fpe_irq_status(priv, priv->ioaddr,
> +						   priv->dev);
> +
> +		stmmac_fpe_event_status(priv, status);
> +	}
> +
>   	/* To handle GMAC own interrupts */
>   	if ((priv->plat->has_gmac) || xmac) {
>   		int status = stmmac_host_irq_status(priv, priv->hw, &priv->xstats);
> @@ -5065,6 +5173,68 @@ int stmmac_reinit_ringparam(struct net_device *dev, u32 rx_size, u32 tx_size)
>   	return ret;
>   }
>   
> +#define SEND_VERIFY_MPAKCET_FMT "Send Verify mPacket lo_state=%d lp_state=%d\n"
> +static void stmmac_fpe_lp_task(struct work_struct *work)
> +{
> +	struct stmmac_priv *priv = container_of(work, struct stmmac_priv,
> +						fpe_task);
> +	struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg;
> +	enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state;
> +	enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state;
> +	bool *hs_enable = &fpe_cfg->hs_enable;
> +	bool *enable = &fpe_cfg->enable;
> +	int retries = 20;
> +
> +	while (retries-- > 0) {
> +		/* Bail out immediately if FPE handshake is OFF */
> +		if (*lo_state == FPE_STATE_OFF || !*hs_enable)
> +			break;
> +
> +		if (*lo_state == FPE_STATE_ENTERING_ON &&
> +		    *lp_state == FPE_STATE_ENTERING_ON) {
> +			stmmac_fpe_configure(priv, priv->ioaddr,
> +					     priv->plat->tx_queues_to_use,
> +					     priv->plat->rx_queues_to_use,
> +					     *enable);
> +
> +			netdev_info(priv->dev, "configured FPE\n");
> +
> +			*lo_state = FPE_STATE_ON;
> +			*lp_state = FPE_STATE_ON;
> +			netdev_info(priv->dev, "!!! BOTH FPE stations ON\n");
> +			break;
> +		}
> +
> +		if ((*lo_state == FPE_STATE_CAPABLE ||
> +		     *lo_state == FPE_STATE_ENTERING_ON) &&
> +		     *lp_state != FPE_STATE_ON) {
> +			netdev_info(priv->dev, SEND_VERIFY_MPAKCET_FMT,
> +				    *lo_state, *lp_state);
> +			stmmac_fpe_send_mpacket(priv, priv->ioaddr,
> +						MPACKET_VERIFY);
> +		}
> +		/* Sleep then retry */
> +		msleep(500);
> +	}
> +
> +	clear_bit(__FPE_TASK_SCHED, &priv->fpe_task_state);
> +}
> +
> +void stmmac_fpe_handshake(struct stmmac_priv *priv, bool enable)
> +{
> +	if (priv->plat->fpe_cfg->hs_enable != enable) {
> +		if (enable) {
> +			stmmac_fpe_send_mpacket(priv, priv->ioaddr,
> +						MPACKET_VERIFY);
> +		} else {
> +			priv->plat->fpe_cfg->lo_fpe_state = FPE_STATE_OFF;
> +			priv->plat->fpe_cfg->lp_fpe_state = FPE_STATE_OFF;
> +		}
> +
> +		priv->plat->fpe_cfg->hs_enable = enable;
> +	}
> +}
> +
>   /**
>    * stmmac_dvr_probe
>    * @device: device pointer
> @@ -5122,6 +5292,9 @@ int stmmac_dvr_probe(struct device *device,
>   
>   	INIT_WORK(&priv->service_task, stmmac_service_task);
>   
> +	/* Initialize Link Partner FPE workqueue */
> +	INIT_WORK(&priv->fpe_task, stmmac_fpe_lp_task);
> +
>   	/* Override with kernel parameters if supplied XXX CRS XXX
>   	 * this needs to have multiple instances
>   	 */
> @@ -5435,8 +5608,18 @@ int stmmac_suspend(struct device *dev)
>   		if (ret)
>   			return ret;
>   	}
> +
>   	mutex_unlock(&priv->lock);
>   
> +	if (priv->dma_cap.fpesel) {
> +		/* Disable FPE */
> +		stmmac_fpe_configure(priv, priv->ioaddr,
> +				     priv->plat->tx_queues_to_use,
> +				     priv->plat->rx_queues_to_use, false);
> +
> +		stmmac_fpe_handshake(priv, false);
> +	}
> +
>   	priv->speed = SPEED_UNKNOWN;
>   	return 0;
>   }
> diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
> index b80cb2985b39..1d84ee359808 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
> +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
> @@ -297,6 +297,17 @@ static int tc_init(struct stmmac_priv *priv)
>   
>   	dev_info(priv->device, "Enabling HW TC (entries=%d, max_off=%d)\n",
>   			priv->tc_entries_max, priv->tc_off_max);
> +
> +	if (!priv->plat->fpe_cfg) {
> +		priv->plat->fpe_cfg = devm_kzalloc(priv->device,
> +						   sizeof(*priv->plat->fpe_cfg),
> +						   GFP_KERNEL);
> +		if (!priv->plat->fpe_cfg)
> +			return -ENOMEM;
> +	} else {
> +		memset(priv->plat->fpe_cfg, 0, sizeof(*priv->plat->fpe_cfg));
> +	}
> +
>   	return 0;
>   }
>   
> @@ -829,13 +840,10 @@ static int tc_setup_taprio(struct stmmac_priv *priv,
>   	if (fpe && !priv->dma_cap.fpesel)
>   		return -EOPNOTSUPP;
>   
> -	ret = stmmac_fpe_configure(priv, priv->ioaddr,
> -				   priv->plat->tx_queues_to_use,
> -				   priv->plat->rx_queues_to_use, fpe);
> -	if (ret && fpe) {
> -		netdev_err(priv->dev, "failed to enable Frame Preemption\n");
> -		return ret;
> -	}
> +	/* Actual FPE register configuration will be done after FPE handshake
> +	 * is success.
> +	 */
> +	priv->plat->fpe_cfg->enable = fpe;
>   
>   	ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
>   				   priv->plat->clk_ptp_rate);
> @@ -845,12 +853,29 @@ static int tc_setup_taprio(struct stmmac_priv *priv,
>   	}
>   
>   	netdev_info(priv->dev, "configured EST\n");
> +
> +	if (fpe) {
> +		stmmac_fpe_handshake(priv, true);
> +		netdev_info(priv->dev, "start FPE handshake\n");
> +	}
> +
>   	return 0;
>   
>   disable:
>   	priv->plat->est->enable = false;
>   	stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
>   			     priv->plat->clk_ptp_rate);
> +
> +	priv->plat->fpe_cfg->enable = false;
> +	stmmac_fpe_configure(priv, priv->ioaddr,
> +			     priv->plat->tx_queues_to_use,
> +			     priv->plat->rx_queues_to_use,
> +			     false);
> +	netdev_info(priv->dev, "disabled FPE\n");
> +
> +	stmmac_fpe_handshake(priv, false);
> +	netdev_info(priv->dev, "stop FPE handshake\n");
> +
>   	return ret;
>   }
>   
> diff --git a/include/linux/stmmac.h b/include/linux/stmmac.h
> index 10abc80b601e..072f269b1618 100644
> --- a/include/linux/stmmac.h
> +++ b/include/linux/stmmac.h
> @@ -144,6 +144,32 @@ struct stmmac_txq_cfg {
>   	int tbs_en;
>   };
>   
> +/* FPE link state */
> +enum stmmac_fpe_state {
> +	FPE_STATE_OFF = 0,
> +	FPE_STATE_CAPABLE = 1,
> +	FPE_STATE_ENTERING_ON = 2,
> +	FPE_STATE_ON = 3,
> +};
> +
> +/* FPE link-partner hand-shaking mPacket type */
> +enum stmmac_mpacket_type {
> +	MPACKET_VERIFY = 0,
> +	MPACKET_RESPONSE = 1,
> +};
> +
> +enum stmmac_fpe_task_state_t {
> +	__FPE_REMOVING,
> +	__FPE_TASK_SCHED,
> +};
> +
> +struct stmmac_fpe_cfg {
> +	bool enable;				/* FPE enable */
> +	bool hs_enable;				/* FPE handshake enable */
> +	enum stmmac_fpe_state lp_fpe_state;	/* Link Partner FPE state */
> +	enum stmmac_fpe_state lo_fpe_state;	/* Local station FPE state */
> +};
> +
>   struct plat_stmmacenet_data {
>   	int bus_id;
>   	int phy_addr;
> @@ -155,6 +181,7 @@ struct plat_stmmacenet_data {
>   	struct device_node *mdio_node;
>   	struct stmmac_dma_cfg *dma_cfg;
>   	struct stmmac_est *est;
> +	struct stmmac_fpe_cfg *fpe_cfg;
>   	int clk_csr;
>   	int has_gmac;
>   	int enh_desc;

Best regards
-- 
Marek Szyprowski, PhD
Samsung R&D Institute Poland