diff options
| -rw-r--r-- | drivers/net/ethernet/stmicro/stmmac/stmmac_main.c | 48 |
1 files changed, 28 insertions, 20 deletions
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index fd5106880192..6706ae7dd89f 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -464,6 +464,33 @@ static void stmmac_get_rx_hwtstamp(struct stmmac_priv *priv, struct dma_desc *p, } } +static void stmmac_update_subsecond_increment(struct stmmac_priv *priv) +{ + bool xmac = dwmac_is_xmac(priv->plat->core_type); + u32 sec_inc = 0; + u64 temp = 0; + + stmmac_config_hw_tstamping(priv, priv->ptpaddr, priv->systime_flags); + + /* program Sub Second Increment reg */ + stmmac_config_sub_second_increment(priv, priv->ptpaddr, + priv->plat->clk_ptp_rate, + xmac, &sec_inc); + temp = div_u64(1000000000ULL, sec_inc); + + /* Store sub second increment for later use */ + priv->sub_second_inc = sec_inc; + + /* calculate default added value: + * formula is : + * addend = (2^32)/freq_div_ratio; + * where, freq_div_ratio = 1e9ns/sec_inc + */ + temp = (u64)(temp << 32); + priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate); + stmmac_config_addend(priv, priv->ptpaddr, priv->default_addend); +} + /** * stmmac_hwtstamp_set - control hardware timestamping. * @dev: device pointer. @@ -697,10 +724,7 @@ static int stmmac_hwtstamp_get(struct net_device *dev, static int stmmac_init_tstamp_counter(struct stmmac_priv *priv, u32 systime_flags) { - bool xmac = dwmac_is_xmac(priv->plat->core_type); struct timespec64 now; - u32 sec_inc = 0; - u64 temp = 0; if (!priv->plat->clk_ptp_rate) { netdev_err(priv->dev, "Invalid PTP clock rate"); @@ -710,23 +734,7 @@ static int stmmac_init_tstamp_counter(struct stmmac_priv *priv, stmmac_config_hw_tstamping(priv, priv->ptpaddr, systime_flags); priv->systime_flags = systime_flags; - /* program Sub Second Increment reg */ - stmmac_config_sub_second_increment(priv, priv->ptpaddr, - priv->plat->clk_ptp_rate, - xmac, &sec_inc); - temp = div_u64(1000000000ULL, sec_inc); - - /* Store sub second increment for later use */ - priv->sub_second_inc = sec_inc; - - /* calculate default added value: - * formula is : - * addend = (2^32)/freq_div_ratio; - * where, freq_div_ratio = 1e9ns/sec_inc - */ - temp = (u64)(temp << 32); - priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate); - stmmac_config_addend(priv, priv->ptpaddr, priv->default_addend); + stmmac_update_subsecond_increment(priv); /* initialize system time */ ktime_get_real_ts64(&now); |