123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295 |
- // 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
|