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_gettimex(struct ptp_clock_info *ptp, 61 struct timespec64 *ts, 62 struct ptp_system_timestamp *sts) 63 { 64 struct ptp_vclock *vclock = info_to_vclock(ptp); 65 struct ptp_clock *pptp = vclock->pclock; 66 struct timespec64 pts; 67 unsigned long flags; 68 int err; 69 u64 ns; 70 71 err = pptp->info->gettimex64(pptp->info, &pts, sts); 72 if (err) 73 return err; 74 75 spin_lock_irqsave(&vclock->lock, flags); 76 ns = timecounter_cyc2time(&vclock->tc, timespec64_to_ns(&pts)); 77 spin_unlock_irqrestore(&vclock->lock, flags); 78 79 *ts = ns_to_timespec64(ns); 80 81 return 0; 82 } 83 84 static int ptp_vclock_settime(struct ptp_clock_info *ptp, 85 const struct timespec64 *ts) 86 { 87 struct ptp_vclock *vclock = info_to_vclock(ptp); 88 u64 ns = timespec64_to_ns(ts); 89 unsigned long flags; 90 91 spin_lock_irqsave(&vclock->lock, flags); 92 timecounter_init(&vclock->tc, &vclock->cc, ns); 93 spin_unlock_irqrestore(&vclock->lock, flags); 94 95 return 0; 96 } 97 98 static int ptp_vclock_getcrosststamp(struct ptp_clock_info *ptp, 99 struct system_device_crosststamp *xtstamp) 100 { 101 struct ptp_vclock *vclock = info_to_vclock(ptp); 102 struct ptp_clock *pptp = vclock->pclock; 103 unsigned long flags; 104 int err; 105 u64 ns; 106 107 err = pptp->info->getcrosststamp(pptp->info, xtstamp); 108 if (err) 109 return err; 110 111 spin_lock_irqsave(&vclock->lock, flags); 112 ns = timecounter_cyc2time(&vclock->tc, ktime_to_ns(xtstamp->device)); 113 spin_unlock_irqrestore(&vclock->lock, flags); 114 115 xtstamp->device = ns_to_ktime(ns); 116 117 return 0; 118 } 119 120 static long ptp_vclock_refresh(struct ptp_clock_info *ptp) 121 { 122 struct ptp_vclock *vclock = info_to_vclock(ptp); 123 struct timespec64 ts; 124 125 ptp_vclock_gettime(&vclock->info, &ts); 126 127 return PTP_VCLOCK_REFRESH_INTERVAL; 128 } 129 130 static const struct ptp_clock_info ptp_vclock_info = { 131 .owner = THIS_MODULE, 132 .name = "ptp virtual clock", 133 .max_adj = 500000000, 134 .adjfine = ptp_vclock_adjfine, 135 .adjtime = ptp_vclock_adjtime, 136 .settime64 = ptp_vclock_settime, 137 .do_aux_work = ptp_vclock_refresh, 138 }; 139 140 static u64 ptp_vclock_read(const struct cyclecounter *cc) 141 { 142 struct ptp_vclock *vclock = cc_to_vclock(cc); 143 struct ptp_clock *ptp = vclock->pclock; 144 struct timespec64 ts = {}; 145 146 if (ptp->info->gettimex64) 147 ptp->info->gettimex64(ptp->info, &ts, NULL); 148 else 149 ptp->info->gettime64(ptp->info, &ts); 150 151 return timespec64_to_ns(&ts); 152 } 153 154 static const struct cyclecounter ptp_vclock_cc = { 155 .read = ptp_vclock_read, 156 .mask = CYCLECOUNTER_MASK(32), 157 .mult = PTP_VCLOCK_CC_MULT, 158 .shift = PTP_VCLOCK_CC_SHIFT, 159 }; 160 161 struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock) 162 { 163 struct ptp_vclock *vclock; 164 165 vclock = kzalloc(sizeof(*vclock), GFP_KERNEL); 166 if (!vclock) 167 return NULL; 168 169 vclock->pclock = pclock; 170 vclock->info = ptp_vclock_info; 171 if (pclock->info->gettimex64) 172 vclock->info.gettimex64 = ptp_vclock_gettimex; 173 else 174 vclock->info.gettime64 = ptp_vclock_gettime; 175 if (pclock->info->getcrosststamp) 176 vclock->info.getcrosststamp = ptp_vclock_getcrosststamp; 177 vclock->cc = ptp_vclock_cc; 178 179 snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt", 180 pclock->index); 181 182 spin_lock_init(&vclock->lock); 183 184 vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev); 185 if (IS_ERR_OR_NULL(vclock->clock)) { 186 kfree(vclock); 187 return NULL; 188 } 189 190 timecounter_init(&vclock->tc, &vclock->cc, 0); 191 ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL); 192 193 return vclock; 194 } 195 196 void ptp_vclock_unregister(struct ptp_vclock *vclock) 197 { 198 ptp_clock_unregister(vclock->clock); 199 kfree(vclock); 200 } 201 202 #if IS_BUILTIN(CONFIG_PTP_1588_CLOCK) 203 int ptp_get_vclocks_index(int pclock_index, int **vclock_index) 204 { 205 char name[PTP_CLOCK_NAME_LEN] = ""; 206 struct ptp_clock *ptp; 207 struct device *dev; 208 int num = 0; 209 210 if (pclock_index < 0) 211 return num; 212 213 snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index); 214 dev = class_find_device_by_name(ptp_class, name); 215 if (!dev) 216 return num; 217 218 ptp = dev_get_drvdata(dev); 219 220 if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) { 221 put_device(dev); 222 return num; 223 } 224 225 *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL); 226 if (!(*vclock_index)) 227 goto out; 228 229 memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks); 230 num = ptp->n_vclocks; 231 out: 232 mutex_unlock(&ptp->n_vclocks_mux); 233 put_device(dev); 234 return num; 235 } 236 EXPORT_SYMBOL(ptp_get_vclocks_index); 237 238 ktime_t ptp_convert_timestamp(const struct skb_shared_hwtstamps *hwtstamps, 239 int vclock_index) 240 { 241 char name[PTP_CLOCK_NAME_LEN] = ""; 242 struct ptp_vclock *vclock; 243 struct ptp_clock *ptp; 244 unsigned long flags; 245 struct device *dev; 246 u64 ns; 247 248 snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", vclock_index); 249 dev = class_find_device_by_name(ptp_class, name); 250 if (!dev) 251 return 0; 252 253 ptp = dev_get_drvdata(dev); 254 if (!ptp->is_virtual_clock) { 255 put_device(dev); 256 return 0; 257 } 258 259 vclock = info_to_vclock(ptp->info); 260 261 ns = ktime_to_ns(hwtstamps->hwtstamp); 262 263 spin_lock_irqsave(&vclock->lock, flags); 264 ns = timecounter_cyc2time(&vclock->tc, ns); 265 spin_unlock_irqrestore(&vclock->lock, flags); 266 267 put_device(dev); 268 return ns_to_ktime(ns); 269 } 270 EXPORT_SYMBOL(ptp_convert_timestamp); 271 #endif 272