Hi,
On 24.03.2021 10:07, mohammad.athari.ism...@intel.com wrote:
> From: Ong Boon Leong <boon.leong....@intel.com>
>
> 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....@intel.com>
> Co-developed-by: Voon Weifeng <weifeng.v...@intel.com>
> Signed-off-by: Voon Weifeng <weifeng.v...@intel.com>
> Co-developed-by: Tan Tee Min <tee.min....@intel.com>
> Signed-off-by: Tan Tee Min <tee.min....@intel.com>
> Co-developed-by: Mohammad Athari Bin Ismail <mohammad.athari.ism...@intel.com>
> Signed-off-by: Mohammad Athari Bin Ismail <mohammad.athari.ism...@intel.com>
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
