0001
0002
0003
0004
0005
0006
0007 #include <linux/slab.h>
0008 #include <linux/hashtable.h>
0009 #include "ptp_private.h"
0010
0011 #define PTP_VCLOCK_CC_SHIFT 31
0012 #define PTP_VCLOCK_CC_MULT (1 << PTP_VCLOCK_CC_SHIFT)
0013 #define PTP_VCLOCK_FADJ_SHIFT 9
0014 #define PTP_VCLOCK_FADJ_DENOMINATOR 15625ULL
0015 #define PTP_VCLOCK_REFRESH_INTERVAL (HZ * 2)
0016
0017
0018 static DEFINE_SPINLOCK(vclock_hash_lock);
0019
0020 static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8);
0021
0022 static void ptp_vclock_hash_add(struct ptp_vclock *vclock)
0023 {
0024 spin_lock(&vclock_hash_lock);
0025
0026 hlist_add_head_rcu(&vclock->vclock_hash_node,
0027 &vclock_hash[vclock->clock->index % HASH_SIZE(vclock_hash)]);
0028
0029 spin_unlock(&vclock_hash_lock);
0030 }
0031
0032 static void ptp_vclock_hash_del(struct ptp_vclock *vclock)
0033 {
0034 spin_lock(&vclock_hash_lock);
0035
0036 hlist_del_init_rcu(&vclock->vclock_hash_node);
0037
0038 spin_unlock(&vclock_hash_lock);
0039
0040 synchronize_rcu();
0041 }
0042
0043 static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
0044 {
0045 struct ptp_vclock *vclock = info_to_vclock(ptp);
0046 unsigned long flags;
0047 s64 adj;
0048
0049 adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT;
0050 adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR);
0051
0052 spin_lock_irqsave(&vclock->lock, flags);
0053 timecounter_read(&vclock->tc);
0054 vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj;
0055 spin_unlock_irqrestore(&vclock->lock, flags);
0056
0057 return 0;
0058 }
0059
0060 static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta)
0061 {
0062 struct ptp_vclock *vclock = info_to_vclock(ptp);
0063 unsigned long flags;
0064
0065 spin_lock_irqsave(&vclock->lock, flags);
0066 timecounter_adjtime(&vclock->tc, delta);
0067 spin_unlock_irqrestore(&vclock->lock, flags);
0068
0069 return 0;
0070 }
0071
0072 static int ptp_vclock_gettime(struct ptp_clock_info *ptp,
0073 struct timespec64 *ts)
0074 {
0075 struct ptp_vclock *vclock = info_to_vclock(ptp);
0076 unsigned long flags;
0077 u64 ns;
0078
0079 spin_lock_irqsave(&vclock->lock, flags);
0080 ns = timecounter_read(&vclock->tc);
0081 spin_unlock_irqrestore(&vclock->lock, flags);
0082 *ts = ns_to_timespec64(ns);
0083
0084 return 0;
0085 }
0086
0087 static int ptp_vclock_gettimex(struct ptp_clock_info *ptp,
0088 struct timespec64 *ts,
0089 struct ptp_system_timestamp *sts)
0090 {
0091 struct ptp_vclock *vclock = info_to_vclock(ptp);
0092 struct ptp_clock *pptp = vclock->pclock;
0093 struct timespec64 pts;
0094 unsigned long flags;
0095 int err;
0096 u64 ns;
0097
0098 err = pptp->info->getcyclesx64(pptp->info, &pts, sts);
0099 if (err)
0100 return err;
0101
0102 spin_lock_irqsave(&vclock->lock, flags);
0103 ns = timecounter_cyc2time(&vclock->tc, timespec64_to_ns(&pts));
0104 spin_unlock_irqrestore(&vclock->lock, flags);
0105
0106 *ts = ns_to_timespec64(ns);
0107
0108 return 0;
0109 }
0110
0111 static int ptp_vclock_settime(struct ptp_clock_info *ptp,
0112 const struct timespec64 *ts)
0113 {
0114 struct ptp_vclock *vclock = info_to_vclock(ptp);
0115 u64 ns = timespec64_to_ns(ts);
0116 unsigned long flags;
0117
0118 spin_lock_irqsave(&vclock->lock, flags);
0119 timecounter_init(&vclock->tc, &vclock->cc, ns);
0120 spin_unlock_irqrestore(&vclock->lock, flags);
0121
0122 return 0;
0123 }
0124
0125 static int ptp_vclock_getcrosststamp(struct ptp_clock_info *ptp,
0126 struct system_device_crosststamp *xtstamp)
0127 {
0128 struct ptp_vclock *vclock = info_to_vclock(ptp);
0129 struct ptp_clock *pptp = vclock->pclock;
0130 unsigned long flags;
0131 int err;
0132 u64 ns;
0133
0134 err = pptp->info->getcrosscycles(pptp->info, xtstamp);
0135 if (err)
0136 return err;
0137
0138 spin_lock_irqsave(&vclock->lock, flags);
0139 ns = timecounter_cyc2time(&vclock->tc, ktime_to_ns(xtstamp->device));
0140 spin_unlock_irqrestore(&vclock->lock, flags);
0141
0142 xtstamp->device = ns_to_ktime(ns);
0143
0144 return 0;
0145 }
0146
0147 static long ptp_vclock_refresh(struct ptp_clock_info *ptp)
0148 {
0149 struct ptp_vclock *vclock = info_to_vclock(ptp);
0150 struct timespec64 ts;
0151
0152 ptp_vclock_gettime(&vclock->info, &ts);
0153
0154 return PTP_VCLOCK_REFRESH_INTERVAL;
0155 }
0156
0157 static const struct ptp_clock_info ptp_vclock_info = {
0158 .owner = THIS_MODULE,
0159 .name = "ptp virtual clock",
0160 .max_adj = 500000000,
0161 .adjfine = ptp_vclock_adjfine,
0162 .adjtime = ptp_vclock_adjtime,
0163 .settime64 = ptp_vclock_settime,
0164 .do_aux_work = ptp_vclock_refresh,
0165 };
0166
0167 static u64 ptp_vclock_read(const struct cyclecounter *cc)
0168 {
0169 struct ptp_vclock *vclock = cc_to_vclock(cc);
0170 struct ptp_clock *ptp = vclock->pclock;
0171 struct timespec64 ts = {};
0172
0173 ptp->info->getcycles64(ptp->info, &ts);
0174
0175 return timespec64_to_ns(&ts);
0176 }
0177
0178 static const struct cyclecounter ptp_vclock_cc = {
0179 .read = ptp_vclock_read,
0180 .mask = CYCLECOUNTER_MASK(32),
0181 .mult = PTP_VCLOCK_CC_MULT,
0182 .shift = PTP_VCLOCK_CC_SHIFT,
0183 };
0184
0185 struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock)
0186 {
0187 struct ptp_vclock *vclock;
0188
0189 vclock = kzalloc(sizeof(*vclock), GFP_KERNEL);
0190 if (!vclock)
0191 return NULL;
0192
0193 vclock->pclock = pclock;
0194 vclock->info = ptp_vclock_info;
0195 if (pclock->info->getcyclesx64)
0196 vclock->info.gettimex64 = ptp_vclock_gettimex;
0197 else
0198 vclock->info.gettime64 = ptp_vclock_gettime;
0199 if (pclock->info->getcrosscycles)
0200 vclock->info.getcrosststamp = ptp_vclock_getcrosststamp;
0201 vclock->cc = ptp_vclock_cc;
0202
0203 snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt",
0204 pclock->index);
0205
0206 INIT_HLIST_NODE(&vclock->vclock_hash_node);
0207
0208 spin_lock_init(&vclock->lock);
0209
0210 vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev);
0211 if (IS_ERR_OR_NULL(vclock->clock)) {
0212 kfree(vclock);
0213 return NULL;
0214 }
0215
0216 timecounter_init(&vclock->tc, &vclock->cc, 0);
0217 ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL);
0218
0219 ptp_vclock_hash_add(vclock);
0220
0221 return vclock;
0222 }
0223
0224 void ptp_vclock_unregister(struct ptp_vclock *vclock)
0225 {
0226 ptp_vclock_hash_del(vclock);
0227
0228 ptp_clock_unregister(vclock->clock);
0229 kfree(vclock);
0230 }
0231
0232 #if IS_BUILTIN(CONFIG_PTP_1588_CLOCK)
0233 int ptp_get_vclocks_index(int pclock_index, int **vclock_index)
0234 {
0235 char name[PTP_CLOCK_NAME_LEN] = "";
0236 struct ptp_clock *ptp;
0237 struct device *dev;
0238 int num = 0;
0239
0240 if (pclock_index < 0)
0241 return num;
0242
0243 snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index);
0244 dev = class_find_device_by_name(ptp_class, name);
0245 if (!dev)
0246 return num;
0247
0248 ptp = dev_get_drvdata(dev);
0249
0250 if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) {
0251 put_device(dev);
0252 return num;
0253 }
0254
0255 *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL);
0256 if (!(*vclock_index))
0257 goto out;
0258
0259 memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks);
0260 num = ptp->n_vclocks;
0261 out:
0262 mutex_unlock(&ptp->n_vclocks_mux);
0263 put_device(dev);
0264 return num;
0265 }
0266 EXPORT_SYMBOL(ptp_get_vclocks_index);
0267
0268 ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index)
0269 {
0270 unsigned int hash = vclock_index % HASH_SIZE(vclock_hash);
0271 struct ptp_vclock *vclock;
0272 unsigned long flags;
0273 u64 ns;
0274 u64 vclock_ns = 0;
0275
0276 ns = ktime_to_ns(*hwtstamp);
0277
0278 rcu_read_lock();
0279
0280 hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) {
0281 if (vclock->clock->index != vclock_index)
0282 continue;
0283
0284 spin_lock_irqsave(&vclock->lock, flags);
0285 vclock_ns = timecounter_cyc2time(&vclock->tc, ns);
0286 spin_unlock_irqrestore(&vclock->lock, flags);
0287 break;
0288 }
0289
0290 rcu_read_unlock();
0291
0292 return ns_to_ktime(vclock_ns);
0293 }
0294 EXPORT_SYMBOL(ptp_convert_timestamp);
0295 #endif
