linux/drivers/ptp/ptp_vclock.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0-or-later
   2/*
   3 * PTP virtual clock driver
   4 *
   5 * Copyright 2021 NXP
   6 */
   7#include <linux/slab.h>
   8#include "ptp_private.h"
   9
  10#define PTP_VCLOCK_CC_SHIFT             31
  11#define PTP_VCLOCK_CC_MULT              (1 << PTP_VCLOCK_CC_SHIFT)
  12#define PTP_VCLOCK_FADJ_SHIFT           9
  13#define PTP_VCLOCK_FADJ_DENOMINATOR     15625ULL
  14#define PTP_VCLOCK_REFRESH_INTERVAL     (HZ * 2)
  15
  16static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
  17{
  18        struct ptp_vclock *vclock = info_to_vclock(ptp);
  19        unsigned long flags;
  20        s64 adj;
  21
  22        adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT;
  23        adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR);
  24
  25        spin_lock_irqsave(&vclock->lock, flags);
  26        timecounter_read(&vclock->tc);
  27        vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj;
  28        spin_unlock_irqrestore(&vclock->lock, flags);
  29
  30        return 0;
  31}
  32
  33static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta)
  34{
  35        struct ptp_vclock *vclock = info_to_vclock(ptp);
  36        unsigned long flags;
  37
  38        spin_lock_irqsave(&vclock->lock, flags);
  39        timecounter_adjtime(&vclock->tc, delta);
  40        spin_unlock_irqrestore(&vclock->lock, flags);
  41
  42        return 0;
  43}
  44
  45static int ptp_vclock_gettime(struct ptp_clock_info *ptp,
  46                              struct timespec64 *ts)
  47{
  48        struct ptp_vclock *vclock = info_to_vclock(ptp);
  49        unsigned long flags;
  50        u64 ns;
  51
  52        spin_lock_irqsave(&vclock->lock, flags);
  53        ns = timecounter_read(&vclock->tc);
  54        spin_unlock_irqrestore(&vclock->lock, flags);
  55        *ts = ns_to_timespec64(ns);
  56
  57        return 0;
  58}
  59
  60static int ptp_vclock_settime(struct ptp_clock_info *ptp,
  61                              const struct timespec64 *ts)
  62{
  63        struct ptp_vclock *vclock = info_to_vclock(ptp);
  64        u64 ns = timespec64_to_ns(ts);
  65        unsigned long flags;
  66
  67        spin_lock_irqsave(&vclock->lock, flags);
  68        timecounter_init(&vclock->tc, &vclock->cc, ns);
  69        spin_unlock_irqrestore(&vclock->lock, flags);
  70
  71        return 0;
  72}
  73
  74static long ptp_vclock_refresh(struct ptp_clock_info *ptp)
  75{
  76        struct ptp_vclock *vclock = info_to_vclock(ptp);
  77        struct timespec64 ts;
  78
  79        ptp_vclock_gettime(&vclock->info, &ts);
  80
  81        return PTP_VCLOCK_REFRESH_INTERVAL;
  82}
  83
  84static const struct ptp_clock_info ptp_vclock_info = {
  85        .owner          = THIS_MODULE,
  86        .name           = "ptp virtual clock",
  87        /* The maximum ppb value that long scaled_ppm can support */
  88        .max_adj        = 32767999,
  89        .adjfine        = ptp_vclock_adjfine,
  90        .adjtime        = ptp_vclock_adjtime,
  91        .gettime64      = ptp_vclock_gettime,
  92        .settime64      = ptp_vclock_settime,
  93        .do_aux_work    = ptp_vclock_refresh,
  94};
  95
  96static u64 ptp_vclock_read(const struct cyclecounter *cc)
  97{
  98        struct ptp_vclock *vclock = cc_to_vclock(cc);
  99        struct ptp_clock *ptp = vclock->pclock;
 100        struct timespec64 ts = {};
 101
 102        if (ptp->info->gettimex64)
 103                ptp->info->gettimex64(ptp->info, &ts, NULL);
 104        else
 105                ptp->info->gettime64(ptp->info, &ts);
 106
 107        return timespec64_to_ns(&ts);
 108}
 109
 110static const struct cyclecounter ptp_vclock_cc = {
 111        .read   = ptp_vclock_read,
 112        .mask   = CYCLECOUNTER_MASK(32),
 113        .mult   = PTP_VCLOCK_CC_MULT,
 114        .shift  = PTP_VCLOCK_CC_SHIFT,
 115};
 116
 117struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock)
 118{
 119        struct ptp_vclock *vclock;
 120
 121        vclock = kzalloc(sizeof(*vclock), GFP_KERNEL);
 122        if (!vclock)
 123                return NULL;
 124
 125        vclock->pclock = pclock;
 126        vclock->info = ptp_vclock_info;
 127        vclock->cc = ptp_vclock_cc;
 128
 129        snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt",
 130                 pclock->index);
 131
 132        spin_lock_init(&vclock->lock);
 133
 134        vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev);
 135        if (IS_ERR_OR_NULL(vclock->clock)) {
 136                kfree(vclock);
 137                return NULL;
 138        }
 139
 140        timecounter_init(&vclock->tc, &vclock->cc, 0);
 141        ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL);
 142
 143        return vclock;
 144}
 145
 146void ptp_vclock_unregister(struct ptp_vclock *vclock)
 147{
 148        ptp_clock_unregister(vclock->clock);
 149        kfree(vclock);
 150}
 151
 152int ptp_get_vclocks_index(int pclock_index, int **vclock_index)
 153{
 154        char name[PTP_CLOCK_NAME_LEN] = "";
 155        struct ptp_clock *ptp;
 156        struct device *dev;
 157        int num = 0;
 158
 159        if (pclock_index < 0)
 160                return num;
 161
 162        snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index);
 163        dev = class_find_device_by_name(ptp_class, name);
 164        if (!dev)
 165                return num;
 166
 167        ptp = dev_get_drvdata(dev);
 168
 169        if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) {
 170                put_device(dev);
 171                return num;
 172        }
 173
 174        *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL);
 175        if (!(*vclock_index))
 176                goto out;
 177
 178        memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks);
 179        num = ptp->n_vclocks;
 180out:
 181        mutex_unlock(&ptp->n_vclocks_mux);
 182        put_device(dev);
 183        return num;
 184}
 185EXPORT_SYMBOL(ptp_get_vclocks_index);
 186
 187void ptp_convert_timestamp(struct skb_shared_hwtstamps *hwtstamps,
 188                           int vclock_index)
 189{
 190        char name[PTP_CLOCK_NAME_LEN] = "";
 191        struct ptp_vclock *vclock;
 192        struct ptp_clock *ptp;
 193        unsigned long flags;
 194        struct device *dev;
 195        u64 ns;
 196
 197        snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", vclock_index);
 198        dev = class_find_device_by_name(ptp_class, name);
 199        if (!dev)
 200                return;
 201
 202        ptp = dev_get_drvdata(dev);
 203        if (!ptp->is_virtual_clock) {
 204                put_device(dev);
 205                return;
 206        }
 207
 208        vclock = info_to_vclock(ptp->info);
 209
 210        ns = ktime_to_ns(hwtstamps->hwtstamp);
 211
 212        spin_lock_irqsave(&vclock->lock, flags);
 213        ns = timecounter_cyc2time(&vclock->tc, ns);
 214        spin_unlock_irqrestore(&vclock->lock, flags);
 215
 216        put_device(dev);
 217        hwtstamps->hwtstamp = ns_to_ktime(ns);
 218}
 219EXPORT_SYMBOL(ptp_convert_timestamp);
 220