net: dsa: microchip: ptp: manipulating absolute time using ptp hw clock

This patch is used for reconstructing the absolute time from the 32bit
hardware time stamping value. The do_aux ioctl is used for reading the
ptp hardware clock and store it to global variable.
The timestamped value in tail tag during rx and register during tx are
32 bit value (2 bit seconds and 30 bit nanoseconds). The time taken to
read entire ptp clock will be time consuming. In order to speed up, the
software clock is maintained. This clock time will be added to 32 bit
timestamp to get the absolute time stamp.

Signed-off-by: Christian Eggers <ceggers@arri.de>
Co-developed-by: Arun Ramadoss <arun.ramadoss@microchip.com>
Signed-off-by: Arun Ramadoss <arun.ramadoss@microchip.com>
Reviewed-by: Vladimir Oltean <olteanv@gmail.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/dsa/microchip/ksz_ptp.c b/drivers/net/dsa/microchip/ksz_ptp.c
index 5281aeb84db6e0ed6bf38fc397d3c5a9246271a7..3e124816697d9f0fb6ed8c3041c3e055a760423f 100644 (file)
--- a/drivers/net/dsa/microchip/ksz_ptp.c
+++ b/drivers/net/dsa/microchip/ksz_ptp.c
@@ -28,9 +28,11 @@
 static int ksz_ptp_enable_mode(struct ksz_device *dev)
 {
        struct ksz_tagger_data *tagger_data = ksz_tagger_data(dev->ds);
+       struct ksz_ptp_data *ptp_data = &dev->ptp_data;
        struct ksz_port *prt;
        struct dsa_port *dp;
        bool tag_en = false;
+       int ret;
 
        dsa_switch_for_each_user_port(dp, dev->ds) {
                prt = &dev->ports[dp->index];
@@ -40,6 +42,14 @@ static int ksz_ptp_enable_mode(struct ksz_device *dev)
                }
        }
 
+       if (tag_en) {
+               ret = ptp_schedule_worker(ptp_data->clock, 0);
+               if (ret)
+                       return ret;
+       } else {
+               ptp_cancel_worker_sync(ptp_data->clock);
+       }
+
        tagger_data->hwtstamp_set_state(dev->ds, tag_en);
 
        return ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_ENABLE,
@@ -221,6 +231,12 @@ static int ksz_ptp_settime(struct ptp_clock_info *ptp,
                goto unlock;
 
        ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_LOAD_TIME, PTP_LOAD_TIME);
+       if (ret)
+               goto unlock;
+
+       spin_lock_bh(&ptp_data->clock_lock);
+       ptp_data->clock_time = *ts;
+       spin_unlock_bh(&ptp_data->clock_lock);
 
 unlock:
        mutex_unlock(&ptp_data->lock);
@@ -271,6 +287,7 @@ static int ksz_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 {
        struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp);
        struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data);
+       struct timespec64 delta64 = ns_to_timespec64(delta);
        s32 sec, nsec;
        u16 data16;
        int ret;
@@ -303,15 +320,54 @@ static int ksz_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
                data16 |= PTP_STEP_DIR;
 
        ret = ksz_write16(dev, REG_PTP_CLK_CTRL, data16);
+       if (ret)
+               goto unlock;
+
+       spin_lock_bh(&ptp_data->clock_lock);
+       ptp_data->clock_time = timespec64_add(ptp_data->clock_time, delta64);
+       spin_unlock_bh(&ptp_data->clock_lock);
 
 unlock:
        mutex_unlock(&ptp_data->lock);
        return ret;
 }
 
+/*  Function is pointer to the do_aux_work in the ptp_clock capability */
+static long ksz_ptp_do_aux_work(struct ptp_clock_info *ptp)
+{
+       struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp);
+       struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data);
+       struct timespec64 ts;
+       int ret;
+
+       mutex_lock(&ptp_data->lock);
+       ret = _ksz_ptp_gettime(dev, &ts);
+       if (ret)
+               goto out;
+
+       spin_lock_bh(&ptp_data->clock_lock);
+       ptp_data->clock_time = ts;
+       spin_unlock_bh(&ptp_data->clock_lock);
+
+out:
+       mutex_unlock(&ptp_data->lock);
+
+       return HZ;  /* reschedule in 1 second */
+}
+
 static int ksz_ptp_start_clock(struct ksz_device *dev)
 {
-       return ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ENABLE, PTP_CLK_ENABLE);
+       struct ksz_ptp_data *ptp_data = &dev->ptp_data;
+       int ret;
+
+       ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ENABLE, PTP_CLK_ENABLE);
+       if (ret)
+               return ret;
+
+       ptp_data->clock_time.tv_sec = 0;
+       ptp_data->clock_time.tv_nsec = 0;
+
+       return 0;
 }
 
 int ksz_ptp_clock_register(struct dsa_switch *ds)
@@ -322,6 +378,7 @@ int ksz_ptp_clock_register(struct dsa_switch *ds)
 
        ptp_data = &dev->ptp_data;
        mutex_init(&ptp_data->lock);
+       spin_lock_init(&ptp_data->clock_lock);
 
        ptp_data->caps.owner            = THIS_MODULE;
        snprintf(ptp_data->caps.name, 16, "Microchip Clock");
@@ -330,6 +387,7 @@ int ksz_ptp_clock_register(struct dsa_switch *ds)
        ptp_data->caps.settime64        = ksz_ptp_settime;
        ptp_data->caps.adjfine          = ksz_ptp_adjfine;
        ptp_data->caps.adjtime          = ksz_ptp_adjtime;
+       ptp_data->caps.do_aux_work      = ksz_ptp_do_aux_work;
 
        ret = ksz_ptp_start_clock(dev);
        if (ret)

diff --git a/drivers/net/dsa/microchip/ksz_ptp.h b/drivers/net/dsa/microchip/ksz_ptp.h
index 7bb3fde2dd14fddd78d5946a471a7a848a029491..2c29a0b604bb3194f2c12293e5df0eebb5448e6e 100644 (file)
--- a/drivers/net/dsa/microchip/ksz_ptp.h
+++ b/drivers/net/dsa/microchip/ksz_ptp.h
@@ -17,6 +17,9 @@ struct ksz_ptp_data {
        struct ptp_clock *clock;
        /* Serializes all operations on the PTP hardware clock */
        struct mutex lock;
+       /* lock for accessing the clock_time */
+       spinlock_t clock_lock;
+       struct timespec64 clock_time;
 };
 
 int ksz_ptp_clock_register(struct dsa_switch *ds);