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
ptp_vclock_hash_add(struct ptp_vclock * vclock)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
ptp_vclock_hash_del(struct ptp_vclock * vclock)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
ptp_vclock_adjfine(struct ptp_clock_info * ptp,long scaled_ppm)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
ptp_vclock_adjtime(struct ptp_clock_info * ptp,s64 delta)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
ptp_vclock_gettime(struct ptp_clock_info * ptp,struct timespec64 * ts)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
ptp_vclock_gettimex(struct ptp_clock_info * ptp,struct timespec64 * ts,struct ptp_system_timestamp * sts)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
ptp_vclock_settime(struct ptp_clock_info * ptp,const struct timespec64 * ts)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
ptp_vclock_getcrosststamp(struct ptp_clock_info * ptp,struct system_device_crosststamp * xtstamp)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
ptp_vclock_refresh(struct ptp_clock_info * ptp)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
ptp_vclock_set_subclass(struct ptp_clock * ptp)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
ptp_vclock_read(struct cyclecounter * cc)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
ptp_vclock_register(struct ptp_clock * pclock)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
ptp_vclock_unregister(struct ptp_vclock * vclock)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)
ptp_get_vclocks_index(int pclock_index,int ** vclock_index)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
ptp_convert_timestamp(const ktime_t * hwtstamp,int vclock_index)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