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