diff options
Diffstat (limited to 'drivers/net/ethernet/freescale/fec_ptp.c')
-rw-r--r-- | drivers/net/ethernet/freescale/fec_ptp.c | 35 |
1 files changed, 30 insertions, 5 deletions
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c index 7d49c28215f3..c74d04f4b2fd 100644 --- a/drivers/net/ethernet/freescale/fec_ptp.c +++ b/drivers/net/ethernet/freescale/fec_ptp.c @@ -135,11 +135,7 @@ static int fec_ptp_enable_pps(struct fec_enet_private *fep, uint enable) * NSEC_PER_SEC - ts.tv_nsec. Add the remaining nanoseconds * to current timer would be next second. */ - tempval = readl(fep->hwp + FEC_ATIME_CTRL); - tempval |= FEC_T_CTRL_CAPTURE; - writel(tempval, fep->hwp + FEC_ATIME_CTRL); - - tempval = readl(fep->hwp + FEC_ATIME); + tempval = fep->cc.read(&fep->cc); /* Convert the ptp local counter to 1588 timestamp */ ns = timecounter_cyc2time(&fep->tc, tempval); ts = ns_to_timespec64(ns); @@ -637,7 +633,36 @@ void fec_ptp_stop(struct platform_device *pdev) struct net_device *ndev = platform_get_drvdata(pdev); struct fec_enet_private *fep = netdev_priv(ndev); + if (fep->pps_enable) + fec_ptp_enable_pps(fep, 0); + cancel_delayed_work_sync(&fep->time_keep); if (fep->ptp_clock) ptp_clock_unregister(fep->ptp_clock); } + +void fec_ptp_save_state(struct fec_enet_private *fep) +{ + u32 atime_inc_corr; + + fec_ptp_gettime(&fep->ptp_caps, &fep->ptp_saved_state.ts_phc); + fep->ptp_saved_state.ns_sys = ktime_get_ns(); + + fep->ptp_saved_state.at_corr = readl(fep->hwp + FEC_ATIME_CORR); + atime_inc_corr = readl(fep->hwp + FEC_ATIME_INC) & FEC_T_INC_CORR_MASK; + fep->ptp_saved_state.at_inc_corr = (u8)(atime_inc_corr >> FEC_T_INC_CORR_OFFSET); +} + +int fec_ptp_restore_state(struct fec_enet_private *fep) +{ + u32 atime_inc = readl(fep->hwp + FEC_ATIME_INC) & FEC_T_INC_MASK; + u64 ns_sys; + + writel(fep->ptp_saved_state.at_corr, fep->hwp + FEC_ATIME_CORR); + atime_inc |= ((u32)fep->ptp_saved_state.at_inc_corr) << FEC_T_INC_CORR_OFFSET; + writel(atime_inc, fep->hwp + FEC_ATIME_INC); + + ns_sys = ktime_get_ns() - fep->ptp_saved_state.ns_sys; + timespec64_add_ns(&fep->ptp_saved_state.ts_phc, ns_sys); + return fec_ptp_settime(&fep->ptp_caps, &fep->ptp_saved_state.ts_phc); +} |