From ae932a1455c8bd60e70d2c9c8fa2d35640c0bb12 Mon Sep 17 00:00:00 2001 From: Suraj Jaiswal Date: Wed, 17 Mar 2021 22:10:15 +0530 Subject: [PATCH] net: stmmac: Add suspend resume support Add support for low power features like suspend, resume and wake on lan. Change-Id: I161b1d6a00e2254cf6b855e81fab7ebd9be244d7 Acked-by: Nagarjuna Chaganti Signed-off-by: Suraj Jaiswal --- .../stmicro/stmmac/dwmac-qcom-ethqos.c | 204 +++++++++++++++++- .../stmicro/stmmac/dwmac-qcom-ethqos.h | 30 +++ drivers/net/ethernet/stmicro/stmmac/stmmac.h | 2 + .../net/ethernet/stmicro/stmmac/stmmac_main.c | 17 +- 4 files changed, 231 insertions(+), 22 deletions(-) diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c index d8ba39eb6755..7f4fafbfe883 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c @@ -320,7 +320,8 @@ static void rgmii_dump(struct qcom_ethqos *ethqos) #define RGMII_ID_MODE_10_LOW_SVS_CLK_FREQ (5 * 1000 * 1000UL) static void -ethqos_update_rgmii_clk(struct qcom_ethqos *ethqos, unsigned int speed) +ethqos_update_rgmii_clk_and_bus_cfg(struct qcom_ethqos *ethqos, + unsigned int speed) { switch (speed) { case SPEED_1000: @@ -336,6 +337,21 @@ ethqos_update_rgmii_clk(struct qcom_ethqos *ethqos, unsigned int speed) break; } + switch (speed) { + case SPEED_1000: + ethqos->vote_idx = VOTE_IDX_1000MBPS; + break; + case SPEED_100: + ethqos->vote_idx = VOTE_IDX_100MBPS; + break; + case SPEED_10: + ethqos->vote_idx = VOTE_IDX_10MBPS; + break; + case 0: + ethqos->vote_idx = VOTE_IDX_0MBPS; + ethqos->rgmii_clk_rate = 0; + break; + } clk_set_rate(ethqos->rgmii_clk, ethqos->rgmii_clk_rate); } @@ -665,7 +681,7 @@ static void ethqos_fix_mac_speed(void *priv, unsigned int speed) struct qcom_ethqos *ethqos = priv; ethqos->speed = speed; - ethqos_update_rgmii_clk(ethqos, speed); + ethqos_update_rgmii_clk_and_bus_cfg(ethqos, speed); ethqos_configure(ethqos); } @@ -769,6 +785,9 @@ static void ethqos_defer_phy_isr_work(struct work_struct *work) struct qcom_ethqos *ethqos = container_of(work, struct qcom_ethqos, emac_phy_work); + if (ethqos->clks_suspended) + wait_for_completion(ðqos->clk_enable_done); + ethqos_handle_phy_interrupt(ethqos); } @@ -776,6 +795,8 @@ static irqreturn_t ETHQOS_PHY_ISR(int irq, void *dev_data) { struct qcom_ethqos *ethqos = (struct qcom_ethqos *)dev_data; + pm_wakeup_event(ðqos->pdev->dev, 5000); + queue_work(system_wq, ðqos->emac_phy_work); return IRQ_HANDLED; } @@ -785,6 +806,8 @@ static int ethqos_phy_intr_enable(struct qcom_ethqos *ethqos) int ret = 0; INIT_WORK(ðqos->emac_phy_work, ethqos_defer_phy_isr_work); + init_completion(ðqos->clk_enable_done); + ret = request_irq(ethqos->phy_intr, ETHQOS_PHY_ISR, IRQF_SHARED, "stmmac", ethqos); if (ret) { @@ -867,6 +890,108 @@ static void ethqos_pps_irq_config(struct qcom_ethqos *ethqos) } } +static void qcom_ethqos_phy_suspend_clks(struct qcom_ethqos *ethqos) +{ + ETHQOSINFO("Enter\n"); + + if (phy_intr_en) + reinit_completion(ðqos->clk_enable_done); + + ethqos->clks_suspended = 1; + + ethqos_update_rgmii_clk_and_bus_cfg(ethqos, 0); + + ETHQOSINFO("Exit\n"); +} + +static bool qcom_ethqos_is_phy_link_up(struct qcom_ethqos *ethqos, + struct net_device *ndev) +{ + if (!ethqos) { + ETHQOSINFO("ethqos addr is NULL"); + return false; + } + + if (!ndev) { + ETHQOSINFO("dev addr is NULL"); + return false; + } + + /* PHY driver initializes phydev->link=1. + * So, phydev->link is 1 even on booup with no PHY connected. + * phydev->link is valid only after adjust_link is called once. + * Use (pdata->oldlink != -1) to indicate phy link is not up + */ + return (ethqos->oldlink != -1 && ndev->phydev && ndev->phydev->link); +} + +static void qcom_ethqos_phy_resume_clks(struct qcom_ethqos *ethqos, + struct net_device *ndev) +{ + ETHQOSINFO("Enter\n"); + + if (qcom_ethqos_is_phy_link_up(ethqos, ndev)) + ethqos_update_rgmii_clk_and_bus_cfg(ethqos, ethqos->speed); + else + ethqos_update_rgmii_clk_and_bus_cfg(ethqos, SPEED_10); + + ethqos->clks_suspended = 0; + + if (phy_intr_en) + complete_all(ðqos->clk_enable_done); + + ETHQOSINFO("Exit\n"); +} + +void qcom_ethqos_request_phy_wol(struct plat_stmmacenet_data *plat) +{ + struct qcom_ethqos *ethqos = plat->bsp_priv; + struct platform_device *pdev = ethqos->pdev; + struct net_device *ndev = platform_get_drvdata(pdev); + + ethqos->phy_wol_supported = 0; + ethqos->phy_wol_wolopts = 0; + /* Check if phydev is valid*/ + /* Check and enable Wake-on-LAN functionality in PHY*/ + + if (ndev->phydev) { + struct ethtool_wolinfo wol = {.cmd = ETHTOOL_GWOL}; + + wol.supported = 0; + wol.wolopts = 0; + ETHQOSINFO("phydev addr: 0x%pK\n", ndev->phydev); + phy_ethtool_get_wol(ndev->phydev, &wol); + ethqos->phy_wol_supported = wol.supported; + ETHQOSINFO("Get WoL[0x%x] in %s\n", wol.supported, + ndev->phydev->drv->name); + + /* Try to enable supported Wake-on-LAN features in PHY*/ + if (wol.supported) { + device_set_wakeup_capable(ðqos->pdev->dev, 1); + + wol.cmd = ETHTOOL_SWOL; + wol.wolopts = wol.supported; + + if (!phy_ethtool_set_wol(ndev->phydev, &wol)) { + ethqos->phy_wol_wolopts = wol.wolopts; + + enable_irq_wake(ethqos->phy_intr); + device_set_wakeup_enable(ðqos->pdev->dev, 1); + + ETHQOSINFO("Enabled WoL[0x%x] in %s\n", + wol.wolopts, + ndev->phydev->drv->name); + } else { + ETHQOSINFO("Disabled WoL[0x%x] in %s\n", + wol.wolopts, + ndev->phydev->drv->name); + } + } else { + ETHQOSINFO("WoL Not Supported\n"); + } + } +} + static int qcom_ethqos_probe(struct platform_device *pdev) { struct device_node *np = pdev->dev.of_node; @@ -888,8 +1013,9 @@ static int qcom_ethqos_probe(struct platform_device *pdev) ret = -ENOMEM; goto err_mem; } - ethqos->pdev = pdev; + ethqos->pdev = pdev; + ethqos->oldlink = -1; ethqos_init_reqgulators(ethqos); ethqos_init_gpio(ethqos); @@ -920,8 +1046,8 @@ static int qcom_ethqos_probe(struct platform_device *pdev) if (ret) goto err_mem; - ethqos->speed = SPEED_1000; - ethqos_update_rgmii_clk(ethqos, SPEED_1000); + ethqos->speed = SPEED_10; + ethqos_update_rgmii_clk_and_bus_cfg(ethqos, SPEED_10); ethqos_set_func_clk_en(ethqos); plat_dat->bsp_priv = ethqos; @@ -1010,20 +1136,86 @@ static int qcom_ethqos_remove(struct platform_device *pdev) if (phy_intr_en) free_irq(ethqos->phy_intr, ethqos); + + if (phy_intr_en) + cancel_work_sync(ðqos->emac_phy_work); + emac_emb_smmu_exit(); ethqos_disable_regulators(ethqos); return ret; } +static int qcom_ethqos_suspend(struct device *dev) +{ + struct qcom_ethqos *ethqos; + struct net_device *ndev = NULL; + int ret; + + if (of_device_is_compatible(dev->of_node, "qcom,emac-smmu-embedded")) { + ETHQOSDBG("smmu return\n"); + return 0; + } + + ethqos = get_stmmac_bsp_priv(dev); + if (!ethqos) + return -ENODEV; + + ndev = dev_get_drvdata(dev); + + if (!ndev || !netif_running(ndev)) + return -EINVAL; + + ret = stmmac_suspend(dev); + qcom_ethqos_phy_suspend_clks(ethqos); + + ETHQOSDBG(" ret = %d\n", ret); + return ret; +} + +static int qcom_ethqos_resume(struct device *dev) +{ + struct net_device *ndev = NULL; + struct qcom_ethqos *ethqos; + int ret; + + ETHQOSDBG("Resume Enter\n"); + if (of_device_is_compatible(dev->of_node, "qcom,emac-smmu-embedded")) + return 0; + + ethqos = get_stmmac_bsp_priv(dev); + + if (!ethqos) + return -ENODEV; + + ndev = dev_get_drvdata(dev); + + if (!ndev || !netif_running(ndev)) { + ETHQOSERR(" Resume not possible\n"); + return -EINVAL; + } + + qcom_ethqos_phy_resume_clks(ethqos, ndev); + + ret = stmmac_resume(dev); + + ETHQOSDBG("<--Resume Exit\n"); + return ret; +} + MODULE_DEVICE_TABLE(of, qcom_ethqos_match); +static const struct dev_pm_ops qcom_ethqos_pm_ops = { + .suspend = qcom_ethqos_suspend, + .resume = qcom_ethqos_resume, +}; + static struct platform_driver qcom_ethqos_driver = { .probe = qcom_ethqos_probe, .remove = qcom_ethqos_remove, .driver = { .name = DRV_NAME, - .pm = &stmmac_pltfr_pm_ops, + .pm = &qcom_ethqos_pm_ops, .of_match_table = of_match_ptr(qcom_ethqos_match), }, }; diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h index 4caf1f507b86..87f1834a0c99 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h @@ -4,6 +4,8 @@ #ifndef _DWMAC_QCOM_ETHQOS_H #define _DWMAC_QCOM_ETHQOS_H +//#include + #define DRV_NAME "qcom-ethqos" #define ETHQOSDBG(fmt, args...) \ pr_debug(DRV_NAME " %s:%d " fmt, __func__, __LINE__, ## args) @@ -50,6 +52,11 @@ #define AVB_CLASS_A_CHANNEL_NUM 2 #define AVB_CLASS_B_CHANNEL_NUM 3 +#define VOTE_IDX_0MBPS 0 +#define VOTE_IDX_10MBPS 1 +#define VOTE_IDX_100MBPS 2 +#define VOTE_IDX_1000MBPS 3 + static inline u32 PPSCMDX(u32 x, u32 val) { return (GENMASK(PPS_MINIDX(x) + 3, PPS_MINIDX(x)) & @@ -67,6 +74,12 @@ static inline u32 PPSX_MASK(u32 x) return GENMASK(PPS_MAXIDX(x), PPS_MINIDX(x)); } +enum IO_MACRO_PHY_MODE { + RGMII_MODE, + RMII_MODE, + MII_MODE +}; + struct ethqos_emac_por { unsigned int offset; unsigned int value; @@ -94,9 +107,12 @@ struct qcom_ethqos { struct platform_device *pdev; void __iomem *rgmii_base; + struct msm_bus_scale_pdata *bus_scale_vec; + u32 bus_hdl; unsigned int rgmii_clk_rate; struct clk *rgmii_clk; unsigned int speed; + unsigned int vote_idx; int gpio_phy_intr_redirect; u32 phy_intr; @@ -129,6 +145,19 @@ struct qcom_ethqos { unsigned long avb_class_a_intr_cnt; unsigned long avb_class_b_intr_cnt; + + int oldlink; + /* saving state for Wake-on-LAN */ + int wolopts; + /* state of enabled wol options in PHY*/ + u32 phy_wol_wolopts; + /* state of supported wol options in PHY*/ + u32 phy_wol_supported; + /* Boolean to check if clock is suspended*/ + int clks_suspended; + /* Structure which holds done and wait members */ + struct completion clk_enable_done; + }; struct pps_cfg { @@ -221,4 +250,5 @@ struct dwmac_qcom_avb_algorithm { void dwmac_qcom_program_avb_algorithm(struct stmmac_priv *priv, struct ifr_data_struct *req); unsigned int dwmac_qcom_get_plat_tx_coal_frames(struct sk_buff *skb); +void qcom_ethqos_request_phy_wol(struct plat_stmmacenet_data *plat); #endif diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h index 6142ee2d3a11..982c178fabfd 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h @@ -264,6 +264,8 @@ int ethqos_handle_prv_ioctl(struct net_device *dev, struct ifreq *rq, int cmd); int ethqos_init_pps(struct stmmac_priv *priv); extern bool phy_intr_en; +void qcom_ethqos_request_phy_wol(struct plat_stmmacenet_data *plat_dat); + int stmmac_mdio_unregister(struct net_device *ndev); int stmmac_mdio_register(struct net_device *ndev); int stmmac_mdio_reset(struct mii_bus *mii); diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index 12ed4ebcdf93..2329f39cc3da 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -1054,6 +1054,7 @@ static int stmmac_init_phy(struct net_device *dev) !priv->phydev->drv->config_intr(priv->phydev)) { pr_err(" qcom-ethqos: %s config_phy_intr successful aftre connect\n", __func__); + qcom_ethqos_request_phy_wol(priv->plat); } } else { pr_info("stmmac phy polling mode\n"); @@ -4042,22 +4043,6 @@ static int stmmac_setup_tc(struct net_device *ndev, enum tc_setup_type type, } } -static u16 stmmac_select_queue(struct net_device *dev, struct sk_buff *skb, - struct net_device *sb_dev) -{ - if (skb_shinfo(skb)->gso_type & (SKB_GSO_TCPV4 | SKB_GSO_TCPV6)) { - /* - * There is no way to determine the number of TSO - * capable Queues. Let's use always the Queue 0 - * because if TSO is supported then at least this - * one will be capable. - */ - return 0; - } - - return netdev_pick_tx(dev, skb, NULL) % dev->real_num_tx_queues; -} - static int stmmac_set_mac_address(struct net_device *ndev, void *addr) { struct stmmac_priv *priv = netdev_priv(ndev);