summaryrefslogtreecommitdiff
path: root/drivers/ptp/ptp_vclock.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/ptp/ptp_vclock.c')
-rw-r--r--drivers/ptp/ptp_vclock.c219
1 files changed, 219 insertions, 0 deletions
diff --git a/drivers/ptp/ptp_vclock.c b/drivers/ptp/ptp_vclock.c
new file mode 100644
index 000000000000..e0f87c57749a
--- /dev/null
+++ b/drivers/ptp/ptp_vclock.c
@@ -0,0 +1,219 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * PTP virtual clock driver
+ *
+ * Copyright 2021 NXP
+ */
+#include <linux/slab.h>
+#include "ptp_private.h"
+
+#define PTP_VCLOCK_CC_SHIFT 31
+#define PTP_VCLOCK_CC_MULT (1 << PTP_VCLOCK_CC_SHIFT)
+#define PTP_VCLOCK_FADJ_SHIFT 9
+#define PTP_VCLOCK_FADJ_DENOMINATOR 15625ULL
+#define PTP_VCLOCK_REFRESH_INTERVAL (HZ * 2)
+
+static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+ struct ptp_vclock *vclock = info_to_vclock(ptp);
+ unsigned long flags;
+ s64 adj;
+
+ adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT;
+ adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR);
+
+ spin_lock_irqsave(&vclock->lock, flags);
+ timecounter_read(&vclock->tc);
+ vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj;
+ spin_unlock_irqrestore(&vclock->lock, flags);
+
+ return 0;
+}
+
+static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+ struct ptp_vclock *vclock = info_to_vclock(ptp);
+ unsigned long flags;
+
+ spin_lock_irqsave(&vclock->lock, flags);
+ timecounter_adjtime(&vclock->tc, delta);
+ spin_unlock_irqrestore(&vclock->lock, flags);
+
+ return 0;
+}
+
+static int ptp_vclock_gettime(struct ptp_clock_info *ptp,
+ struct timespec64 *ts)
+{
+ struct ptp_vclock *vclock = info_to_vclock(ptp);
+ unsigned long flags;
+ u64 ns;
+
+ spin_lock_irqsave(&vclock->lock, flags);
+ ns = timecounter_read(&vclock->tc);
+ spin_unlock_irqrestore(&vclock->lock, flags);
+ *ts = ns_to_timespec64(ns);
+
+ return 0;
+}
+
+static int ptp_vclock_settime(struct ptp_clock_info *ptp,
+ const struct timespec64 *ts)
+{
+ struct ptp_vclock *vclock = info_to_vclock(ptp);
+ u64 ns = timespec64_to_ns(ts);
+ unsigned long flags;
+
+ spin_lock_irqsave(&vclock->lock, flags);
+ timecounter_init(&vclock->tc, &vclock->cc, ns);
+ spin_unlock_irqrestore(&vclock->lock, flags);
+
+ return 0;
+}
+
+static long ptp_vclock_refresh(struct ptp_clock_info *ptp)
+{
+ struct ptp_vclock *vclock = info_to_vclock(ptp);
+ struct timespec64 ts;
+
+ ptp_vclock_gettime(&vclock->info, &ts);
+
+ return PTP_VCLOCK_REFRESH_INTERVAL;
+}
+
+static const struct ptp_clock_info ptp_vclock_info = {
+ .owner = THIS_MODULE,
+ .name = "ptp virtual clock",
+ /* The maximum ppb value that long scaled_ppm can support */
+ .max_adj = 32767999,
+ .adjfine = ptp_vclock_adjfine,
+ .adjtime = ptp_vclock_adjtime,
+ .gettime64 = ptp_vclock_gettime,
+ .settime64 = ptp_vclock_settime,
+ .do_aux_work = ptp_vclock_refresh,
+};
+
+static u64 ptp_vclock_read(const struct cyclecounter *cc)
+{
+ struct ptp_vclock *vclock = cc_to_vclock(cc);
+ struct ptp_clock *ptp = vclock->pclock;
+ struct timespec64 ts = {};
+
+ if (ptp->info->gettimex64)
+ ptp->info->gettimex64(ptp->info, &ts, NULL);
+ else
+ ptp->info->gettime64(ptp->info, &ts);
+
+ return timespec64_to_ns(&ts);
+}
+
+static const struct cyclecounter ptp_vclock_cc = {
+ .read = ptp_vclock_read,
+ .mask = CYCLECOUNTER_MASK(32),
+ .mult = PTP_VCLOCK_CC_MULT,
+ .shift = PTP_VCLOCK_CC_SHIFT,
+};
+
+struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock)
+{
+ struct ptp_vclock *vclock;
+
+ vclock = kzalloc(sizeof(*vclock), GFP_KERNEL);
+ if (!vclock)
+ return NULL;
+
+ vclock->pclock = pclock;
+ vclock->info = ptp_vclock_info;
+ vclock->cc = ptp_vclock_cc;
+
+ snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt",
+ pclock->index);
+
+ spin_lock_init(&vclock->lock);
+
+ vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev);
+ if (IS_ERR_OR_NULL(vclock->clock)) {
+ kfree(vclock);
+ return NULL;
+ }
+
+ timecounter_init(&vclock->tc, &vclock->cc, 0);
+ ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL);
+
+ return vclock;
+}
+
+void ptp_vclock_unregister(struct ptp_vclock *vclock)
+{
+ ptp_clock_unregister(vclock->clock);
+ kfree(vclock);
+}
+
+int ptp_get_vclocks_index(int pclock_index, int **vclock_index)
+{
+ char name[PTP_CLOCK_NAME_LEN] = "";
+ struct ptp_clock *ptp;
+ struct device *dev;
+ int num = 0;
+
+ if (pclock_index < 0)
+ return num;
+
+ snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index);
+ dev = class_find_device_by_name(ptp_class, name);
+ if (!dev)
+ return num;
+
+ ptp = dev_get_drvdata(dev);
+
+ if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) {
+ put_device(dev);
+ return num;
+ }
+
+ *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL);
+ if (!(*vclock_index))
+ goto out;
+
+ memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks);
+ num = ptp->n_vclocks;
+out:
+ mutex_unlock(&ptp->n_vclocks_mux);
+ put_device(dev);
+ return num;
+}
+EXPORT_SYMBOL(ptp_get_vclocks_index);
+
+void ptp_convert_timestamp(struct skb_shared_hwtstamps *hwtstamps,
+ int vclock_index)
+{
+ char name[PTP_CLOCK_NAME_LEN] = "";
+ struct ptp_vclock *vclock;
+ struct ptp_clock *ptp;
+ unsigned long flags;
+ struct device *dev;
+ u64 ns;
+
+ snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", vclock_index);
+ dev = class_find_device_by_name(ptp_class, name);
+ if (!dev)
+ return;
+
+ ptp = dev_get_drvdata(dev);
+ if (!ptp->is_virtual_clock) {
+ put_device(dev);
+ return;
+ }
+
+ vclock = info_to_vclock(ptp->info);
+
+ ns = ktime_to_ns(hwtstamps->hwtstamp);
+
+ spin_lock_irqsave(&vclock->lock, flags);
+ ns = timecounter_cyc2time(&vclock->tc, ns);
+ spin_unlock_irqrestore(&vclock->lock, flags);
+
+ put_device(dev);
+ hwtstamps->hwtstamp = ns_to_ktime(ns);
+}
+EXPORT_SYMBOL(ptp_convert_timestamp);