Quelle  ptp_vclock.c   Sprache: C

// 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 void ptp_vclock_set_subclass(struct ptp_clock *ptp)
{
 lockdep_set_subclass(&ptp->clock.rwsem, PTP_LOCK_VIRTUAL);
}

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(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;
 }

 ptp_vclock_set_subclass(vclock->clock);

 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