1 files changed, 22 insertions, 11 deletions

diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
index 82386b29914a..cca3617a2321 100644
--- a/drivers/net/ethernet/freescale/fec_ptp.c
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -245,12 +245,20 @@ static int fec_ptp_settime(struct ptp_clock_info *ptp,
         u64 ns;
         unsigned long flags;
 
+        mutex_lock(&fep->ptp_clk_mutex);
+        /* Check the ptp clock */
+        if (!fep->ptp_clk_on) {
+                mutex_unlock(&fep->ptp_clk_mutex);
+                return -EINVAL;
+        }
+
         ns = ts->tv_sec * 1000000000ULL;
         ns += ts->tv_nsec;
 
         spin_lock_irqsave(&fep->tmreg_lock, flags);
         timecounter_init(&fep->tc, &fep->cc, ns);
         spin_unlock_irqrestore(&fep->tmreg_lock, flags);
+        mutex_unlock(&fep->ptp_clk_mutex);
         return 0;
 }
 
@@ -338,17 +346,22 @@ int fec_ptp_get(struct net_device *ndev, struct ifreq *ifr)
  * fec_time_keep - call timecounter_read every second to avoid timer overrun
  *                 because ENET just support 32bit counter, will timeout in 4s
  */
-static void fec_time_keep(unsigned long _data)
+static void fec_time_keep(struct work_struct *work)
 {
-        struct fec_enet_private *fep = (struct fec_enet_private *)_data;
+        struct delayed_work *dwork = to_delayed_work(work);
+        struct fec_enet_private *fep = container_of(dwork, struct fec_enet_private, time_keep);
         u64 ns;
         unsigned long flags;
 
-        spin_lock_irqsave(&fep->tmreg_lock, flags);
-        ns = timecounter_read(&fep->tc);
-        spin_unlock_irqrestore(&fep->tmreg_lock, flags);
+        mutex_lock(&fep->ptp_clk_mutex);
+        if (fep->ptp_clk_on) {
+                spin_lock_irqsave(&fep->tmreg_lock, flags);
+                ns = timecounter_read(&fep->tc);
+                spin_unlock_irqrestore(&fep->tmreg_lock, flags);
+        }
+        mutex_unlock(&fep->ptp_clk_mutex);
 
-        mod_timer(&fep->time_keep, jiffies + HZ);
+        schedule_delayed_work(&fep->time_keep, HZ);
 }
 
 /**
@@ -386,15 +399,13 @@ void fec_ptp_init(struct platform_device *pdev)
 
         fec_ptp_start_cyclecounter(ndev);
 
-        init_timer(&fep->time_keep);
-        fep->time_keep.data = (unsigned long)fep;
-        fep->time_keep.function = fec_time_keep;
-        fep->time_keep.expires = jiffies + HZ;
-        add_timer(&fep->time_keep);
+        INIT_DELAYED_WORK(&fep->time_keep, fec_time_keep);
 
         fep->ptp_clock = ptp_clock_register(&fep->ptp_caps, &pdev->dev);
         if (IS_ERR(fep->ptp_clock)) {
                 fep->ptp_clock = NULL;
                 pr_err("ptp_clock_register failed\n");
         }
+
+        schedule_delayed_work(&fep->time_keep, HZ);
 }