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 <linux/hashtable.h> 9 #include "ptp_private.h" 10 11 #define PTP_VCLOCK_CC_SHIFT 31 12 #define PTP_VCLOCK_CC_MULT (1 << PTP_VCLOCK_CC_SHIFT) 13 #define PTP_VCLOCK_FADJ_SHIFT 9 14 #define PTP_VCLOCK_FADJ_DENOMINATOR 15625ULL 15 #define PTP_VCLOCK_REFRESH_INTERVAL (HZ * 2) 16 17 /* protects vclock_hash addition/deletion */ 18 static DEFINE_SPINLOCK(vclock_hash_lock); 19 20 static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8); 21 22 static 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 32 static 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 43 static 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 60 static 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 72 static 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 87 static 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 111 static 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 125 static 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 147 static 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 157 static void ptp_vclock_set_subclass(struct ptp_clock *ptp) 158 { 159 lockdep_set_subclass(&ptp->clock.rwsem, PTP_LOCK_VIRTUAL); 160 } 161 162 static const struct ptp_clock_info ptp_vclock_info = { 163 .owner = THIS_MODULE, 164 .name = "ptp virtual clock", 165 .max_adj = 500000000, 166 .adjfine = ptp_vclock_adjfine, 167 .adjtime = ptp_vclock_adjtime, 168 .settime64 = ptp_vclock_settime, 169 .do_aux_work = ptp_vclock_refresh, 170 }; 171 172 static u64 ptp_vclock_read(struct cyclecounter *cc) 173 { 174 struct ptp_vclock *vclock = cc_to_vclock(cc); 175 struct ptp_clock *ptp = vclock->pclock; 176 struct timespec64 ts = {}; 177 178 ptp->info->getcycles64(ptp->info, &ts); 179 180 return timespec64_to_ns(&ts); 181 } 182 183 static const struct cyclecounter ptp_vclock_cc = { 184 .read = ptp_vclock_read, 185 .mask = CYCLECOUNTER_MASK(32), 186 .mult = PTP_VCLOCK_CC_MULT, 187 .shift = PTP_VCLOCK_CC_SHIFT, 188 }; 189 190 struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock) 191 { 192 struct ptp_vclock *vclock; 193 194 vclock = kzalloc(sizeof(*vclock), GFP_KERNEL); 195 if (!vclock) 196 return NULL; 197 198 vclock->pclock = pclock; 199 vclock->info = ptp_vclock_info; 200 if (pclock->info->getcyclesx64) 201 vclock->info.gettimex64 = ptp_vclock_gettimex; 202 else 203 vclock->info.gettime64 = ptp_vclock_gettime; 204 if (pclock->info->getcrosscycles) 205 vclock->info.getcrosststamp = ptp_vclock_getcrosststamp; 206 vclock->cc = ptp_vclock_cc; 207 208 snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt", 209 pclock->index); 210 211 INIT_HLIST_NODE(&vclock->vclock_hash_node); 212 213 mutex_init(&vclock->lock); 214 215 vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev); 216 if (IS_ERR_OR_NULL(vclock->clock)) { 217 kfree(vclock); 218 return NULL; 219 } 220 221 ptp_vclock_set_subclass(vclock->clock); 222 223 timecounter_init(&vclock->tc, &vclock->cc, 0); 224 ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL); 225 226 ptp_vclock_hash_add(vclock); 227 228 return vclock; 229 } 230 231 void ptp_vclock_unregister(struct ptp_vclock *vclock) 232 { 233 ptp_vclock_hash_del(vclock); 234 235 ptp_clock_unregister(vclock->clock); 236 kfree(vclock); 237 } 238 239 #if IS_BUILTIN(CONFIG_PTP_1588_CLOCK) 240 int ptp_get_vclocks_index(int pclock_index, int **vclock_index) 241 { 242 char name[PTP_CLOCK_NAME_LEN] = ""; 243 struct ptp_clock *ptp; 244 struct device *dev; 245 int num = 0; 246 247 if (pclock_index < 0) 248 return num; 249 250 snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index); 251 dev = class_find_device_by_name(&ptp_class, name); 252 if (!dev) 253 return num; 254 255 ptp = dev_get_drvdata(dev); 256 257 if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) { 258 put_device(dev); 259 return num; 260 } 261 262 *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL); 263 if (!(*vclock_index)) 264 goto out; 265 266 memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks); 267 num = ptp->n_vclocks; 268 out: 269 mutex_unlock(&ptp->n_vclocks_mux); 270 put_device(dev); 271 return num; 272 } 273 EXPORT_SYMBOL(ptp_get_vclocks_index); 274 275 ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index) 276 { 277 unsigned int hash = vclock_index % HASH_SIZE(vclock_hash); 278 struct ptp_vclock *vclock; 279 u64 ns; 280 u64 vclock_ns = 0; 281 282 ns = ktime_to_ns(*hwtstamp); 283 284 rcu_read_lock(); 285 286 hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) { 287 if (vclock->clock->index != vclock_index) 288 continue; 289 290 if (mutex_lock_interruptible(&vclock->lock)) 291 break; 292 vclock_ns = timecounter_cyc2time(&vclock->tc, ns); 293 mutex_unlock(&vclock->lock); 294 break; 295 } 296 297 rcu_read_unlock(); 298 299 return ns_to_ktime(vclock_ns); 300 } 301 EXPORT_SYMBOL(ptp_convert_timestamp); 302 #endif 303