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_settime(struct ptp_clock_info *ptp, 61 const struct timespec64 *ts) 62 { 63 struct ptp_vclock *vclock = info_to_vclock(ptp); 64 u64 ns = timespec64_to_ns(ts); 65 unsigned long flags; 66 67 spin_lock_irqsave(&vclock->lock, flags); 68 timecounter_init(&vclock->tc, &vclock->cc, ns); 69 spin_unlock_irqrestore(&vclock->lock, flags); 70 71 return 0; 72 } 73 74 static long ptp_vclock_refresh(struct ptp_clock_info *ptp) 75 { 76 struct ptp_vclock *vclock = info_to_vclock(ptp); 77 struct timespec64 ts; 78 79 ptp_vclock_gettime(&vclock->info, &ts); 80 81 return PTP_VCLOCK_REFRESH_INTERVAL; 82 } 83 84 static const struct ptp_clock_info ptp_vclock_info = { 85 .owner = THIS_MODULE, 86 .name = "ptp virtual clock", 87 /* The maximum ppb value that long scaled_ppm can support */ 88 .max_adj = 32767999, 89 .adjfine = ptp_vclock_adjfine, 90 .adjtime = ptp_vclock_adjtime, 91 .gettime64 = ptp_vclock_gettime, 92 .settime64 = ptp_vclock_settime, 93 .do_aux_work = ptp_vclock_refresh, 94 }; 95 96 static u64 ptp_vclock_read(const struct cyclecounter *cc) 97 { 98 struct ptp_vclock *vclock = cc_to_vclock(cc); 99 struct ptp_clock *ptp = vclock->pclock; 100 struct timespec64 ts = {}; 101 102 if (ptp->info->gettimex64) 103 ptp->info->gettimex64(ptp->info, &ts, NULL); 104 else 105 ptp->info->gettime64(ptp->info, &ts); 106 107 return timespec64_to_ns(&ts); 108 } 109 110 static const struct cyclecounter ptp_vclock_cc = { 111 .read = ptp_vclock_read, 112 .mask = CYCLECOUNTER_MASK(32), 113 .mult = PTP_VCLOCK_CC_MULT, 114 .shift = PTP_VCLOCK_CC_SHIFT, 115 }; 116 117 struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock) 118 { 119 struct ptp_vclock *vclock; 120 121 vclock = kzalloc(sizeof(*vclock), GFP_KERNEL); 122 if (!vclock) 123 return NULL; 124 125 vclock->pclock = pclock; 126 vclock->info = ptp_vclock_info; 127 vclock->cc = ptp_vclock_cc; 128 129 snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt", 130 pclock->index); 131 132 spin_lock_init(&vclock->lock); 133 134 vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev); 135 if (IS_ERR_OR_NULL(vclock->clock)) { 136 kfree(vclock); 137 return NULL; 138 } 139 140 timecounter_init(&vclock->tc, &vclock->cc, 0); 141 ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL); 142 143 return vclock; 144 } 145 146 void ptp_vclock_unregister(struct ptp_vclock *vclock) 147 { 148 ptp_clock_unregister(vclock->clock); 149 kfree(vclock); 150 } 151 152 int ptp_get_vclocks_index(int pclock_index, int **vclock_index) 153 { 154 char name[PTP_CLOCK_NAME_LEN] = ""; 155 struct ptp_clock *ptp; 156 struct device *dev; 157 int num = 0; 158 159 if (pclock_index < 0) 160 return num; 161 162 snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index); 163 dev = class_find_device_by_name(ptp_class, name); 164 if (!dev) 165 return num; 166 167 ptp = dev_get_drvdata(dev); 168 169 if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) { 170 put_device(dev); 171 return num; 172 } 173 174 *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL); 175 if (!(*vclock_index)) 176 goto out; 177 178 memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks); 179 num = ptp->n_vclocks; 180 out: 181 mutex_unlock(&ptp->n_vclocks_mux); 182 put_device(dev); 183 return num; 184 } 185 EXPORT_SYMBOL(ptp_get_vclocks_index); 186 187 void ptp_convert_timestamp(struct skb_shared_hwtstamps *hwtstamps, 188 int vclock_index) 189 { 190 char name[PTP_CLOCK_NAME_LEN] = ""; 191 struct ptp_vclock *vclock; 192 struct ptp_clock *ptp; 193 unsigned long flags; 194 struct device *dev; 195 u64 ns; 196 197 snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", vclock_index); 198 dev = class_find_device_by_name(ptp_class, name); 199 if (!dev) 200 return; 201 202 ptp = dev_get_drvdata(dev); 203 if (!ptp->is_virtual_clock) { 204 put_device(dev); 205 return; 206 } 207 208 vclock = info_to_vclock(ptp->info); 209 210 ns = ktime_to_ns(hwtstamps->hwtstamp); 211 212 spin_lock_irqsave(&vclock->lock, flags); 213 ns = timecounter_cyc2time(&vclock->tc, ns); 214 spin_unlock_irqrestore(&vclock->lock, flags); 215 216 put_device(dev); 217 hwtstamps->hwtstamp = ns_to_ktime(ns); 218 } 219 EXPORT_SYMBOL(ptp_convert_timestamp); 220