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