diff options
Diffstat (limited to 'drivers/ptp/ptp_vclock.c')
| -rw-r--r-- | drivers/ptp/ptp_vclock.c | 295 |
1 files changed, 295 insertions, 0 deletions
diff --git a/drivers/ptp/ptp_vclock.c b/drivers/ptp/ptp_vclock.c new file mode 100644 index 0000000000..dcf752c9e0 --- /dev/null +++ b/drivers/ptp/ptp_vclock.c @@ -0,0 +1,295 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * PTP virtual clock driver + * + * Copyright 2021 NXP + */ +#include <linux/slab.h> +#include <linux/hashtable.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) + +/* protects vclock_hash addition/deletion */ +static DEFINE_SPINLOCK(vclock_hash_lock); + +static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8); + +static void ptp_vclock_hash_add(struct ptp_vclock *vclock) +{ + spin_lock(&vclock_hash_lock); + + hlist_add_head_rcu(&vclock->vclock_hash_node, + &vclock_hash[vclock->clock->index % HASH_SIZE(vclock_hash)]); + + spin_unlock(&vclock_hash_lock); +} + +static void ptp_vclock_hash_del(struct ptp_vclock *vclock) +{ + spin_lock(&vclock_hash_lock); + + hlist_del_init_rcu(&vclock->vclock_hash_node); + + spin_unlock(&vclock_hash_lock); + + synchronize_rcu(); +} + +static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) +{ + struct ptp_vclock *vclock = info_to_vclock(ptp); + s64 adj; + + adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT; + adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR); + + if (mutex_lock_interruptible(&vclock->lock)) + return -EINTR; + timecounter_read(&vclock->tc); + vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj; + mutex_unlock(&vclock->lock); + + return 0; +} + +static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta) +{ + struct ptp_vclock *vclock = info_to_vclock(ptp); + + if (mutex_lock_interruptible(&vclock->lock)) + return -EINTR; + timecounter_adjtime(&vclock->tc, delta); + mutex_unlock(&vclock->lock); + + return 0; +} + +static int ptp_vclock_gettime(struct ptp_clock_info *ptp, + struct timespec64 *ts) +{ + struct ptp_vclock *vclock = info_to_vclock(ptp); + u64 ns; + + if (mutex_lock_interruptible(&vclock->lock)) + return -EINTR; + ns = timecounter_read(&vclock->tc); + mutex_unlock(&vclock->lock); + *ts = ns_to_timespec64(ns); + + return 0; +} + +static int ptp_vclock_gettimex(struct ptp_clock_info *ptp, + struct timespec64 *ts, + struct ptp_system_timestamp *sts) +{ + struct ptp_vclock *vclock = info_to_vclock(ptp); + struct ptp_clock *pptp = vclock->pclock; + struct timespec64 pts; + int err; + u64 ns; + + err = pptp->info->getcyclesx64(pptp->info, &pts, sts); + if (err) + return err; + + if (mutex_lock_interruptible(&vclock->lock)) + return -EINTR; + ns = timecounter_cyc2time(&vclock->tc, timespec64_to_ns(&pts)); + mutex_unlock(&vclock->lock); + + *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); + + if (mutex_lock_interruptible(&vclock->lock)) + return -EINTR; + timecounter_init(&vclock->tc, &vclock->cc, ns); + mutex_unlock(&vclock->lock); + + return 0; +} + +static int ptp_vclock_getcrosststamp(struct ptp_clock_info *ptp, + struct system_device_crosststamp *xtstamp) +{ + struct ptp_vclock *vclock = info_to_vclock(ptp); + struct ptp_clock *pptp = vclock->pclock; + int err; + u64 ns; + + err = pptp->info->getcrosscycles(pptp->info, xtstamp); + if (err) + return err; + + if (mutex_lock_interruptible(&vclock->lock)) + return -EINTR; + ns = timecounter_cyc2time(&vclock->tc, ktime_to_ns(xtstamp->device)); + mutex_unlock(&vclock->lock); + + xtstamp->device = ns_to_ktime(ns); + + 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", + .max_adj = 500000000, + .adjfine = ptp_vclock_adjfine, + .adjtime = ptp_vclock_adjtime, + .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 = {}; + + ptp->info->getcycles64(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; + if (pclock->info->getcyclesx64) + vclock->info.gettimex64 = ptp_vclock_gettimex; + else + vclock->info.gettime64 = ptp_vclock_gettime; + if (pclock->info->getcrosscycles) + vclock->info.getcrosststamp = ptp_vclock_getcrosststamp; + vclock->cc = ptp_vclock_cc; + + snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt", + pclock->index); + + INIT_HLIST_NODE(&vclock->vclock_hash_node); + + mutex_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); + + ptp_vclock_hash_add(vclock); + + return vclock; +} + +void ptp_vclock_unregister(struct ptp_vclock *vclock) +{ + ptp_vclock_hash_del(vclock); + + ptp_clock_unregister(vclock->clock); + kfree(vclock); +} + +#if IS_BUILTIN(CONFIG_PTP_1588_CLOCK) +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); + +ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index) +{ + unsigned int hash = vclock_index % HASH_SIZE(vclock_hash); + struct ptp_vclock *vclock; + u64 ns; + u64 vclock_ns = 0; + + ns = ktime_to_ns(*hwtstamp); + + rcu_read_lock(); + + hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) { + if (vclock->clock->index != vclock_index) + continue; + + if (mutex_lock_interruptible(&vclock->lock)) + break; + vclock_ns = timecounter_cyc2time(&vclock->tc, ns); + mutex_unlock(&vclock->lock); + break; + } + + rcu_read_unlock(); + + return ns_to_ktime(vclock_ns); +} +EXPORT_SYMBOL(ptp_convert_timestamp); +#endif |