drivers/ptp/ptp_vclock.c

   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
  16 static 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
  33 static 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
  45 static 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
  60 static 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
  74 static 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
  84 static 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
  96 static 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
 110 static 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
 117 struct 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
 146 void ptp_vclock_unregister(struct ptp_vclock *vclock)
 147 {
 148         ptp_clock_unregister(vclock->clock);
 149         kfree(vclock);
 150 }
 151
 152 #if IS_BUILTIN(CONFIG_PTP_1588_CLOCK)
 153 int ptp_get_vclocks_index(int pclock_index, int **vclock_index)
 154 {
 155         char name[PTP_CLOCK_NAME_LEN] = "";
 156         struct ptp_clock *ptp;
 157         struct device *dev;
 158         int num = 0;
 159
 160         if (pclock_index < 0)
 161                 return num;
 162
 163         snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index);
 164         dev = class_find_device_by_name(ptp_class, name);
 165         if (!dev)
 166                 return num;
 167
 168         ptp = dev_get_drvdata(dev);
 169
 170         if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) {
 171                 put_device(dev);
 172                 return num;
 173         }
 174
 175         *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL);
 176         if (!(*vclock_index))
 177                 goto out;
 178
 179         memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks);
 180         num = ptp->n_vclocks;
 181 out:
 182         mutex_unlock(&ptp->n_vclocks_mux);
 183         put_device(dev);
 184         return num;
 185 }
 186 EXPORT_SYMBOL(ptp_get_vclocks_index);
 187
 188 void ptp_convert_timestamp(struct skb_shared_hwtstamps *hwtstamps,
 189                            int vclock_index)
 190 {
 191         char name[PTP_CLOCK_NAME_LEN] = "";
 192         struct ptp_vclock *vclock;
 193         struct ptp_clock *ptp;
 194         unsigned long flags;
 195         struct device *dev;
 196         u64 ns;
 197
 198         snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", vclock_index);
 199         dev = class_find_device_by_name(ptp_class, name);
 200         if (!dev)
 201                 return;
 202
 203         ptp = dev_get_drvdata(dev);
 204         if (!ptp->is_virtual_clock) {
 205                 put_device(dev);
 206                 return;
 207         }
 208
 209         vclock = info_to_vclock(ptp->info);
 210
 211         ns = ktime_to_ns(hwtstamps->hwtstamp);
 212
 213         spin_lock_irqsave(&vclock->lock, flags);
 214         ns = timecounter_cyc2time(&vclock->tc, ns);
 215         spin_unlock_irqrestore(&vclock->lock, flags);
 216
 217         put_device(dev);
 218         hwtstamps->hwtstamp = ns_to_ktime(ns);
 219 }
 220 EXPORT_SYMBOL(ptp_convert_timestamp);
 221 #endif