From d0cebea93c9bf7acbcadadf8b94a6c515a4cf313 Mon Sep 17 00:00:00 2001 From: Suraj Jaiswal Date: Mon, 23 Nov 2020 17:44:42 +0530 Subject: [PATCH] net: stmmac: Fix PPS0 not coming up on bootup Change ppsout_config definition and ioctl handling. Change-Id: I4173e50368478661122d1f88c78aa1d28d8c8f6d Signed-off-by: Suraj Jaiswal --- .../stmicro/stmmac/dwmac-qcom-ethqos.c | 6 +- .../stmicro/stmmac/dwmac-qcom-ethqos.h | 4 +- .../ethernet/stmicro/stmmac/dwmac-qcom-pps.c | 92 ++++++------------- 3 files changed, 38 insertions(+), 64 deletions(-) diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c index dac46f351ee7..cc5d0d5a689a 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c @@ -251,6 +251,7 @@ int ethqos_handle_prv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd) { struct stmmac_priv *pdata = netdev_priv(dev); struct ifr_data_struct req; + struct pps_cfg eth_pps_cfg; int ret = 0; if (copy_from_user(&req, ifr->ifr_ifru.ifru_data, @@ -259,7 +260,10 @@ int ethqos_handle_prv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd) switch (req.cmd) { case ETHQOS_CONFIG_PPSOUT_CMD: - ret = ppsout_config(pdata, &req); + if (copy_from_user(ð_pps_cfg, (void __user *)req.ptr, + sizeof(struct pps_cfg))) + return -EFAULT; + ret = ppsout_config(pdata, ð_pps_cfg); break; case ETHQOS_AVB_ALGORITHM: dwmac_qcom_program_avb_algorithm(pdata, &req); diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h index cf675a3f810a..7ae368f97676 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h @@ -194,7 +194,7 @@ int create_pps_interrupt_device_node(dev_t *pps_dev_t, bool qcom_ethqos_is_phy_link_up(struct qcom_ethqos *ethqos); void *qcom_ethqos_get_priv(struct qcom_ethqos *ethqos); -int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req); +int ppsout_config(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg); u16 dwmac_qcom_select_queue(struct net_device *dev, struct sk_buff *skb, @@ -225,6 +225,8 @@ u16 dwmac_qcom_select_queue(struct net_device *dev, #define IP_PKT_INT_MOD 32 #define PTP_INT_MOD 1 +#define PPS_19_2_FREQ 19200000 + enum dwmac_qcom_queue_operating_mode { DWMAC_QCOM_QDISABLED = 0X0, DWMAC_QCOM_QAVB, diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c index 086628b34164..9a409fe01b3c 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c @@ -84,36 +84,6 @@ static u32 pps_config_sub_second_increment(void __iomem *ioaddr, return data; } -static u32 pps_config_default_addend(void __iomem *ioaddr, - struct stmmac_priv *priv, u32 ptp_clock) -{ - u64 temp; - - /* formula is : - * addend = 2^32/freq_div_ratio; - * - * where, freq_div_ratio = DWC_ETH_QOS_SYSCLOCK/50MHz - * - * hence, addend = ((2^32) * 50MHz)/DWC_ETH_QOS_SYSCLOCK; - * - * NOTE: DWC_ETH_QOS_SYSCLOCK should be >= 50MHz to - * achive 20ns accuracy. - * - * 2^x * y == (y << x), hence - * 2^32 * 50000000 ==> (50000000 << 32) - */ - if (ptp_clock == 250000000) { - // If PTP_CLOCK == SYS_CLOCK, best we can do is 2^32 - 1 - priv->default_addend = 0xFFFFFFFF; - } else { - temp = (u64)((u64)ptp_clock << 32); - priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate); - } - priv->hw->ptp->config_addend(ioaddr, priv->default_addend); - - return 1; -} - int ppsout_stop(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg) { u32 val; @@ -190,23 +160,19 @@ static void ethqos_unregister_pps_isr(struct stmmac_priv *priv, int ch) } } -int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req) +int ppsout_config(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg) { int interval, width; u32 sub_second_inc, value; void __iomem *ioaddr = priv->ioaddr; u32 val; - struct pps_cfg eth_pps_cfg; + u64 temp; - if (copy_from_user(ð_pps_cfg, (void __user *)req->ptr, - sizeof(struct pps_cfg))) - return -EFAULT; - - if (!eth_pps_cfg.ppsout_start) { - ppsout_stop(priv, ð_pps_cfg); - if (eth_pps_cfg.ppsout_ch == DWC_ETH_QOS_PPS_CH_2 || - eth_pps_cfg.ppsout_ch == DWC_ETH_QOS_PPS_CH_3) - ethqos_unregister_pps_isr(priv, eth_pps_cfg.ppsout_ch); + if (!eth_pps_cfg->ppsout_start) { + ppsout_stop(priv, eth_pps_cfg); + if (eth_pps_cfg->ppsout_ch == DWC_ETH_QOS_PPS_CH_2 || + eth_pps_cfg->ppsout_ch == DWC_ETH_QOS_PPS_CH_3) + ethqos_unregister_pps_isr(priv, eth_pps_cfg->ppsout_ch); return 0; } @@ -218,40 +184,42 @@ int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req) val = readl_relaxed(ioaddr + MAC_PPS_CONTROL); sub_second_inc = pps_config_sub_second_increment - (priv->ptpaddr, eth_pps_cfg.ptpclk_freq, + (priv->ptpaddr, eth_pps_cfg->ptpclk_freq, priv->plat->has_gmac4); - pps_config_default_addend(priv->ptpaddr, priv, - eth_pps_cfg.ptpclk_freq); - val &= ~PPSX_MASK(eth_pps_cfg.ppsout_ch); + temp = (u64)((u64)eth_pps_cfg->ptpclk_freq << 32); + priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate); + priv->hw->ptp->config_addend(priv->ptpaddr, priv->default_addend); - val |= PPSCMDX(eth_pps_cfg.ppsout_ch, 0x2); - val |= TRGTMODSELX(eth_pps_cfg.ppsout_ch, 0x2); + val &= ~PPSX_MASK(eth_pps_cfg->ppsout_ch); + + val |= PPSCMDX(eth_pps_cfg->ppsout_ch, 0x2); + val |= TRGTMODSELX(eth_pps_cfg->ppsout_ch, 0x2); val |= PPSEN0; - if (eth_pps_cfg.ppsout_ch == DWC_ETH_QOS_PPS_CH_2 || - eth_pps_cfg.ppsout_ch == DWC_ETH_QOS_PPS_CH_3) - ethqos_register_pps_isr(priv, eth_pps_cfg.ppsout_ch); + if (eth_pps_cfg->ppsout_ch == DWC_ETH_QOS_PPS_CH_2 || + eth_pps_cfg->ppsout_ch == DWC_ETH_QOS_PPS_CH_3) + ethqos_register_pps_isr(priv, eth_pps_cfg->ppsout_ch); writel_relaxed(0, ioaddr + - MAC_PPSX_TARGET_TIME_SEC(eth_pps_cfg.ppsout_ch)); + MAC_PPSX_TARGET_TIME_SEC(eth_pps_cfg->ppsout_ch)); writel_relaxed(0, ioaddr + - MAC_PPSX_TARGET_TIME_NSEC(eth_pps_cfg.ppsout_ch)); + MAC_PPSX_TARGET_TIME_NSEC(eth_pps_cfg->ppsout_ch)); - interval = ((eth_pps_cfg.ptpclk_freq + eth_pps_cfg.ppsout_freq / 2) - / eth_pps_cfg.ppsout_freq); + interval = ((eth_pps_cfg->ptpclk_freq + eth_pps_cfg->ppsout_freq / 2) + / eth_pps_cfg->ppsout_freq); - width = ((interval * eth_pps_cfg.ppsout_duty) + 50) / 100 - 1; + width = ((interval * eth_pps_cfg->ppsout_duty) + 50) / 100 - 1; if (width >= interval) width = interval - 1; if (width < 0) width = 0; writel_relaxed(interval, ioaddr + - MAC_PPSX_INTERVAL(eth_pps_cfg.ppsout_ch)); + MAC_PPSX_INTERVAL(eth_pps_cfg->ppsout_ch)); - writel_relaxed(width, ioaddr + MAC_PPSX_WIDTH(eth_pps_cfg.ppsout_ch)); + writel_relaxed(width, ioaddr + MAC_PPSX_WIDTH(eth_pps_cfg->ppsout_ch)); writel_relaxed(val, ioaddr + MAC_PPS_CONTROL); @@ -261,7 +229,6 @@ int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req) int ethqos_init_pps(struct stmmac_priv *priv) { u32 value; - struct ifr_data_struct req = {0}; struct pps_cfg eth_pps_cfg = {0}; priv->ptpaddr = priv->ioaddr + PTP_GMAC4_OFFSET; @@ -272,13 +239,14 @@ int ethqos_init_pps(struct stmmac_priv *priv) /*Configuaring PPS0 PPS output frequency to default 19.2 Mhz*/ eth_pps_cfg.ppsout_ch = 0; - eth_pps_cfg.ptpclk_freq = 62500000; - eth_pps_cfg.ppsout_freq = 19200000; + + eth_pps_cfg.ptpclk_freq = priv->plat->clk_ptp_req_rate; + eth_pps_cfg.ppsout_freq = PPS_19_2_FREQ; + eth_pps_cfg.ppsout_start = 1; eth_pps_cfg.ppsout_duty = 50; - req.ptr = (void *)ð_pps_cfg; - ppsout_config(priv, &req); + ppsout_config(priv, ð_pps_cfg); return 0; }