296 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * PTP virtual clock driver4 *5 * Copyright 2021 NXP6 */7#include <linux/slab.h>8#include <linux/hashtable.h>9#include "ptp_private.h"10 11#define PTP_VCLOCK_CC_SHIFT 3112#define PTP_VCLOCK_CC_MULT (1 << PTP_VCLOCK_CC_SHIFT)13#define PTP_VCLOCK_FADJ_SHIFT 914#define PTP_VCLOCK_FADJ_DENOMINATOR 15625ULL15#define PTP_VCLOCK_REFRESH_INTERVAL (HZ * 2)16 17/* protects vclock_hash addition/deletion */18static DEFINE_SPINLOCK(vclock_hash_lock);19 20static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8);21 22static void ptp_vclock_hash_add(struct ptp_vclock *vclock)23{24 spin_lock(&vclock_hash_lock);25 26 hlist_add_head_rcu(&vclock->vclock_hash_node,27 &vclock_hash[vclock->clock->index % HASH_SIZE(vclock_hash)]);28 29 spin_unlock(&vclock_hash_lock);30}31 32static void ptp_vclock_hash_del(struct ptp_vclock *vclock)33{34 spin_lock(&vclock_hash_lock);35 36 hlist_del_init_rcu(&vclock->vclock_hash_node);37 38 spin_unlock(&vclock_hash_lock);39 40 synchronize_rcu();41}42 43static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)44{45 struct ptp_vclock *vclock = info_to_vclock(ptp);46 s64 adj;47 48 adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT;49 adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR);50 51 if (mutex_lock_interruptible(&vclock->lock))52 return -EINTR;53 timecounter_read(&vclock->tc);54 vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj;55 mutex_unlock(&vclock->lock);56 57 return 0;58}59 60static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta)61{62 struct ptp_vclock *vclock = info_to_vclock(ptp);63 64 if (mutex_lock_interruptible(&vclock->lock))65 return -EINTR;66 timecounter_adjtime(&vclock->tc, delta);67 mutex_unlock(&vclock->lock);68 69 return 0;70}71 72static int ptp_vclock_gettime(struct ptp_clock_info *ptp,73 struct timespec64 *ts)74{75 struct ptp_vclock *vclock = info_to_vclock(ptp);76 u64 ns;77 78 if (mutex_lock_interruptible(&vclock->lock))79 return -EINTR;80 ns = timecounter_read(&vclock->tc);81 mutex_unlock(&vclock->lock);82 *ts = ns_to_timespec64(ns);83 84 return 0;85}86 87static int ptp_vclock_gettimex(struct ptp_clock_info *ptp,88 struct timespec64 *ts,89 struct ptp_system_timestamp *sts)90{91 struct ptp_vclock *vclock = info_to_vclock(ptp);92 struct ptp_clock *pptp = vclock->pclock;93 struct timespec64 pts;94 int err;95 u64 ns;96 97 err = pptp->info->getcyclesx64(pptp->info, &pts, sts);98 if (err)99 return err;100 101 if (mutex_lock_interruptible(&vclock->lock))102 return -EINTR;103 ns = timecounter_cyc2time(&vclock->tc, timespec64_to_ns(&pts));104 mutex_unlock(&vclock->lock);105 106 *ts = ns_to_timespec64(ns);107 108 return 0;109}110 111static int ptp_vclock_settime(struct ptp_clock_info *ptp,112 const struct timespec64 *ts)113{114 struct ptp_vclock *vclock = info_to_vclock(ptp);115 u64 ns = timespec64_to_ns(ts);116 117 if (mutex_lock_interruptible(&vclock->lock))118 return -EINTR;119 timecounter_init(&vclock->tc, &vclock->cc, ns);120 mutex_unlock(&vclock->lock);121 122 return 0;123}124 125static int ptp_vclock_getcrosststamp(struct ptp_clock_info *ptp,126 struct system_device_crosststamp *xtstamp)127{128 struct ptp_vclock *vclock = info_to_vclock(ptp);129 struct ptp_clock *pptp = vclock->pclock;130 int err;131 u64 ns;132 133 err = pptp->info->getcrosscycles(pptp->info, xtstamp);134 if (err)135 return err;136 137 if (mutex_lock_interruptible(&vclock->lock))138 return -EINTR;139 ns = timecounter_cyc2time(&vclock->tc, ktime_to_ns(xtstamp->device));140 mutex_unlock(&vclock->lock);141 142 xtstamp->device = ns_to_ktime(ns);143 144 return 0;145}146 147static long ptp_vclock_refresh(struct ptp_clock_info *ptp)148{149 struct ptp_vclock *vclock = info_to_vclock(ptp);150 struct timespec64 ts;151 152 ptp_vclock_gettime(&vclock->info, &ts);153 154 return PTP_VCLOCK_REFRESH_INTERVAL;155}156 157static const struct ptp_clock_info ptp_vclock_info = {158 .owner = THIS_MODULE,159 .name = "ptp virtual clock",160 .max_adj = 500000000,161 .adjfine = ptp_vclock_adjfine,162 .adjtime = ptp_vclock_adjtime,163 .settime64 = ptp_vclock_settime,164 .do_aux_work = ptp_vclock_refresh,165};166 167static u64 ptp_vclock_read(const struct cyclecounter *cc)168{169 struct ptp_vclock *vclock = cc_to_vclock(cc);170 struct ptp_clock *ptp = vclock->pclock;171 struct timespec64 ts = {};172 173 ptp->info->getcycles64(ptp->info, &ts);174 175 return timespec64_to_ns(&ts);176}177 178static const struct cyclecounter ptp_vclock_cc = {179 .read = ptp_vclock_read,180 .mask = CYCLECOUNTER_MASK(32),181 .mult = PTP_VCLOCK_CC_MULT,182 .shift = PTP_VCLOCK_CC_SHIFT,183};184 185struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock)186{187 struct ptp_vclock *vclock;188 189 vclock = kzalloc(sizeof(*vclock), GFP_KERNEL);190 if (!vclock)191 return NULL;192 193 vclock->pclock = pclock;194 vclock->info = ptp_vclock_info;195 if (pclock->info->getcyclesx64)196 vclock->info.gettimex64 = ptp_vclock_gettimex;197 else198 vclock->info.gettime64 = ptp_vclock_gettime;199 if (pclock->info->getcrosscycles)200 vclock->info.getcrosststamp = ptp_vclock_getcrosststamp;201 vclock->cc = ptp_vclock_cc;202 203 snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt",204 pclock->index);205 206 INIT_HLIST_NODE(&vclock->vclock_hash_node);207 208 mutex_init(&vclock->lock);209 210 vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev);211 if (IS_ERR_OR_NULL(vclock->clock)) {212 kfree(vclock);213 return NULL;214 }215 216 timecounter_init(&vclock->tc, &vclock->cc, 0);217 ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL);218 219 ptp_vclock_hash_add(vclock);220 221 return vclock;222}223 224void ptp_vclock_unregister(struct ptp_vclock *vclock)225{226 ptp_vclock_hash_del(vclock);227 228 ptp_clock_unregister(vclock->clock);229 kfree(vclock);230}231 232#if IS_BUILTIN(CONFIG_PTP_1588_CLOCK)233int ptp_get_vclocks_index(int pclock_index, int **vclock_index)234{235 char name[PTP_CLOCK_NAME_LEN] = "";236 struct ptp_clock *ptp;237 struct device *dev;238 int num = 0;239 240 if (pclock_index < 0)241 return num;242 243 snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index);244 dev = class_find_device_by_name(&ptp_class, name);245 if (!dev)246 return num;247 248 ptp = dev_get_drvdata(dev);249 250 if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) {251 put_device(dev);252 return num;253 }254 255 *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL);256 if (!(*vclock_index))257 goto out;258 259 memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks);260 num = ptp->n_vclocks;261out:262 mutex_unlock(&ptp->n_vclocks_mux);263 put_device(dev);264 return num;265}266EXPORT_SYMBOL(ptp_get_vclocks_index);267 268ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index)269{270 unsigned int hash = vclock_index % HASH_SIZE(vclock_hash);271 struct ptp_vclock *vclock;272 u64 ns;273 u64 vclock_ns = 0;274 275 ns = ktime_to_ns(*hwtstamp);276 277 rcu_read_lock();278 279 hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) {280 if (vclock->clock->index != vclock_index)281 continue;282 283 if (mutex_lock_interruptible(&vclock->lock))284 break;285 vclock_ns = timecounter_cyc2time(&vclock->tc, ns);286 mutex_unlock(&vclock->lock);287 break;288 }289 290 rcu_read_unlock();291 292 return ns_to_ktime(vclock_ns);293}294EXPORT_SYMBOL(ptp_convert_timestamp);295#endif296