ptp_vclock.c

  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 */
 18static DEFINE_SPINLOCK(vclock_hash_lock);
 19
 20static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8);
 21
 22static 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
 32static 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
 43static 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
 60static 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
 72static 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
 87static 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
111static 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
125static 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
147static 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
157static void ptp_vclock_set_subclass(struct ptp_clock *ptp)
158{
159	lockdep_set_subclass(&ptp->clock.rwsem, PTP_LOCK_VIRTUAL);
160}
161
162static 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
172static 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
183static 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
190struct 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
231void 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)
240int 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;
268out:
269	mutex_unlock(&ptp->n_vclocks_mux);
270	put_device(dev);
271	return num;
272}
273EXPORT_SYMBOL(ptp_get_vclocks_index);
274
275ktime_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}
301EXPORT_SYMBOL(ptp_convert_timestamp);
302#endif