From 1738002951f0884b2f3ad5b8691ba17c94191be7 Mon Sep 17 00:00:00 2001 From: Suraj Jaiswal Date: Tue, 24 Nov 2020 13:37:00 +0530 Subject: [PATCH] net: stmmac: Add support for pps alignment feature Align pps output frequency between different targets. Change-Id: Iabb09c63e12ae7e6dd36c41b48673aa50fd1a349 Signed-off-by: Suraj Jaiswal --- .../stmicro/stmmac/dwmac-qcom-ethqos.c | 11 +++- .../stmicro/stmmac/dwmac-qcom-ethqos.h | 8 +++ .../ethernet/stmicro/stmmac/dwmac-qcom-pps.c | 56 +++++++++++++++---- 3 files changed, 62 insertions(+), 13 deletions(-) diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c index 360ab7a0dec3..677efdbb0b6d 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c @@ -263,7 +263,16 @@ int ethqos_handle_prv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd) if (copy_from_user(ð_pps_cfg, (void __user *)req.ptr, sizeof(struct pps_cfg))) return -EFAULT; - ret = ppsout_config(pdata, ð_pps_cfg); + + if (eth_pps_cfg.ppsout_ch < 0 || + eth_pps_cfg.ppsout_ch >= pdata->dma_cap.pps_out_num) + ret = -EOPNOTSUPP; + else if ((eth_pps_cfg.ppsout_align == 1) && + ((eth_pps_cfg.ppsout_ch != DWC_ETH_QOS_PPS_CH_0) && + (eth_pps_cfg.ppsout_ch != DWC_ETH_QOS_PPS_CH_3))) + ret = -EOPNOTSUPP; + else + 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 7ae368f97676..a1e1404613f6 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h @@ -42,6 +42,12 @@ #define MAC_PPSX_INTERVAL(x) (0x00000b88 + ((x) * 0x10)) #define MAC_PPSX_WIDTH(x) (0x00000b8c + ((x) * 0x10)) +#define PPS_START_DELAY 100000000 +#define ONE_NS 1000000000 +#define PPS_ADJUST_NS 32 + +#define DWC_ETH_QOS_PPS_CH_0 0 +#define DWC_ETH_QOS_PPS_CH_1 1 #define DWC_ETH_QOS_PPS_CH_2 2 #define DWC_ETH_QOS_PPS_CH_3 3 @@ -164,6 +170,8 @@ struct pps_cfg { unsigned int ppsout_ch; unsigned int ppsout_duty; unsigned int ppsout_start; + unsigned int ppsout_align; + unsigned int ppsout_align_ns; }; struct ifr_data_struct { diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c index 9a409fe01b3c..7088f00a04dd 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c @@ -46,12 +46,38 @@ static int strlcmp(const char *s, const char *t, size_t n) return ret; } +static void align_target_time_reg(u32 ch, void __iomem *ioaddr, + struct pps_cfg *eth_pps_cfg, + unsigned int align_ns) +{ + unsigned int system_s, system_ns, temp_system_s; + + system_s = readl_relaxed(ioaddr + 0xb00 + PTP_STSR); + system_ns = readl_relaxed(ioaddr + 0xb00 + PTP_STNSR); + temp_system_s = readl_relaxed(ioaddr + 0xb00 + PTP_STSR); + + if (temp_system_s != system_s) { // second roll over + system_s = readl_relaxed(ioaddr + 0xb00 + PTP_STSR); + system_ns = readl_relaxed(ioaddr + 0xb00 + PTP_STNSR); + } + + system_ns += PPS_START_DELAY; + if (system_ns >= align_ns) + system_s += 1; + + writel_relaxed(system_s, ioaddr + + MAC_PPSX_TARGET_TIME_SEC(eth_pps_cfg->ppsout_ch)); + + writel_relaxed(align_ns, ioaddr + + MAC_PPSX_TARGET_TIME_NSEC(eth_pps_cfg->ppsout_ch)); +} + static u32 pps_config_sub_second_increment(void __iomem *ioaddr, u32 ptp_clock, int gmac4) { u32 value = readl_relaxed(ioaddr + PTP_TCR); - unsigned long data; - unsigned int sns_inc = 0; + u64 data; + u64 sns_inc = 0; u32 reg_value; u32 reg_value2; /* For GMAC3.x, 4.x versions, convert the ptp_clock to nano second @@ -59,16 +85,16 @@ static u32 pps_config_sub_second_increment(void __iomem *ioaddr, * where ptp_clock is 50MHz if fine method is used to update system */ if (value & PTP_TCR_TSCFUPDT) { - data = (1000000000ULL / ptp_clock); + data = div_u64((1000000000ULL), ptp_clock); sns_inc = 1000000000ull - (data * ptp_clock); - sns_inc = (sns_inc * 256) / ptp_clock; + sns_inc = div_u64((sns_inc * 256), ptp_clock); } else { - data = (1000000000ULL / ptp_clock); + data = div_u64((1000000000ULL), ptp_clock); } /* 0.465ns accuracy */ if (!(value & PTP_TCR_TSCTRLSSR)) - data = (data * 1000) / 465; + data = div_u64((data * 1000), 465); data &= PTP_SSIR_SSINC_MASK; @@ -163,10 +189,11 @@ static void ethqos_unregister_pps_isr(struct stmmac_priv *priv, int ch) int ppsout_config(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg) { int interval, width; - u32 sub_second_inc, value; + u32 sub_second_inc; void __iomem *ioaddr = priv->ioaddr; u32 val; u64 temp; + u32 align_ns = 0; if (!eth_pps_cfg->ppsout_start) { ppsout_stop(priv, eth_pps_cfg); @@ -176,11 +203,6 @@ int ppsout_config(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg) return 0; } - value = (PTP_TCR_TSENA | PTP_TCR_TSCFUPDT | PTP_TCR_TSUPDT); - priv->hw->ptp->config_hw_tstamping(priv->ptpaddr, value); - priv->hw->ptp->init_systime(priv->ptpaddr, 0, 0); - priv->hw->ptp->adjust_systime(priv->ptpaddr, 0, 0, 0, 1); - val = readl_relaxed(ioaddr + MAC_PPS_CONTROL); sub_second_inc = pps_config_sub_second_increment @@ -216,6 +238,16 @@ int ppsout_config(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg) if (width < 0) width = 0; + if (eth_pps_cfg->ppsout_align == 1) { + align_ns = eth_pps_cfg->ppsout_align_ns; + if (align_ns < PPS_ADJUST_NS) + align_ns += (ONE_NS - PPS_ADJUST_NS); + else + align_ns -= PPS_ADJUST_NS; + align_target_time_reg(eth_pps_cfg->ppsout_ch, + priv->ioaddr, eth_pps_cfg, align_ns); + } + writel_relaxed(interval, ioaddr + MAC_PPSX_INTERVAL(eth_pps_cfg->ppsout_ch));