blob: dcf752c9e0450651d3b7c68cac132edc90c9a732 [file] [log] [blame]
Yangbo Lu5d43f9512021-06-30 16:11:52 +08001// 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>
Gerhard Englederfcf308e2022-05-06 22:01:41 +02008#include <linux/hashtable.h>
Yangbo Lu5d43f9512021-06-30 16:11:52 +08009#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
Gerhard Englederfcf308e2022-05-06 22:01:41 +020017/* 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
Yangbo Lu5d43f9512021-06-30 16:11:52 +080043static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
44{
45 struct ptp_vclock *vclock = info_to_vclock(ptp);
Yangbo Lu5d43f9512021-06-30 16:11:52 +080046 s64 adj;
47
48 adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT;
49 adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR);
50
Íñigo Huguet67d93ff2023-02-21 14:06:16 +010051 if (mutex_lock_interruptible(&vclock->lock))
52 return -EINTR;
Yangbo Lu5d43f9512021-06-30 16:11:52 +080053 timecounter_read(&vclock->tc);
54 vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj;
Íñigo Huguet67d93ff2023-02-21 14:06:16 +010055 mutex_unlock(&vclock->lock);
Yangbo Lu5d43f9512021-06-30 16:11:52 +080056
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);
Yangbo Lu5d43f9512021-06-30 16:11:52 +080063
Íñigo Huguet67d93ff2023-02-21 14:06:16 +010064 if (mutex_lock_interruptible(&vclock->lock))
65 return -EINTR;
Yangbo Lu5d43f9512021-06-30 16:11:52 +080066 timecounter_adjtime(&vclock->tc, delta);
Íñigo Huguet67d93ff2023-02-21 14:06:16 +010067 mutex_unlock(&vclock->lock);
Yangbo Lu5d43f9512021-06-30 16:11:52 +080068
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);
Yangbo Lu5d43f9512021-06-30 16:11:52 +080076 u64 ns;
77
Íñigo Huguet67d93ff2023-02-21 14:06:16 +010078 if (mutex_lock_interruptible(&vclock->lock))
79 return -EINTR;
Yangbo Lu5d43f9512021-06-30 16:11:52 +080080 ns = timecounter_read(&vclock->tc);
Íñigo Huguet67d93ff2023-02-21 14:06:16 +010081 mutex_unlock(&vclock->lock);
Yangbo Lu5d43f9512021-06-30 16:11:52 +080082 *ts = ns_to_timespec64(ns);
83
84 return 0;
85}
86
Miroslav Lichvarf0067eb2022-02-02 10:33:57 +010087static 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;
Miroslav Lichvarf0067eb2022-02-02 10:33:57 +010094 int err;
95 u64 ns;
96
Gerhard Engleder42704b22022-05-06 22:01:37 +020097 err = pptp->info->getcyclesx64(pptp->info, &pts, sts);
Miroslav Lichvarf0067eb2022-02-02 10:33:57 +010098 if (err)
99 return err;
100
Íñigo Huguet67d93ff2023-02-21 14:06:16 +0100101 if (mutex_lock_interruptible(&vclock->lock))
102 return -EINTR;
Miroslav Lichvarf0067eb2022-02-02 10:33:57 +0100103 ns = timecounter_cyc2time(&vclock->tc, timespec64_to_ns(&pts));
Íñigo Huguet67d93ff2023-02-21 14:06:16 +0100104 mutex_unlock(&vclock->lock);
Miroslav Lichvarf0067eb2022-02-02 10:33:57 +0100105
106 *ts = ns_to_timespec64(ns);
107
108 return 0;
109}
110
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800111static 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);
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800116
Íñigo Huguet67d93ff2023-02-21 14:06:16 +0100117 if (mutex_lock_interruptible(&vclock->lock))
118 return -EINTR;
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800119 timecounter_init(&vclock->tc, &vclock->cc, ns);
Íñigo Huguet67d93ff2023-02-21 14:06:16 +0100120 mutex_unlock(&vclock->lock);
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800121
122 return 0;
123}
124
Miroslav Lichvar21fad632022-02-02 10:33:58 +0100125static 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;
Miroslav Lichvar21fad632022-02-02 10:33:58 +0100130 int err;
131 u64 ns;
132
Gerhard Engleder42704b22022-05-06 22:01:37 +0200133 err = pptp->info->getcrosscycles(pptp->info, xtstamp);
Miroslav Lichvar21fad632022-02-02 10:33:58 +0100134 if (err)
135 return err;
136
Íñigo Huguet67d93ff2023-02-21 14:06:16 +0100137 if (mutex_lock_interruptible(&vclock->lock))
138 return -EINTR;
Miroslav Lichvar21fad632022-02-02 10:33:58 +0100139 ns = timecounter_cyc2time(&vclock->tc, ktime_to_ns(xtstamp->device));
Íñigo Huguet67d93ff2023-02-21 14:06:16 +0100140 mutex_unlock(&vclock->lock);
Miroslav Lichvar21fad632022-02-02 10:33:58 +0100141
142 xtstamp->device = ns_to_ktime(ns);
143
144 return 0;
145}
146
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800147static 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 const struct ptp_clock_info ptp_vclock_info = {
158 .owner = THIS_MODULE,
159 .name = "ptp virtual clock",
Miroslav Lichvarf77222d2022-02-02 10:33:56 +0100160 .max_adj = 500000000,
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800161 .adjfine = ptp_vclock_adjfine,
162 .adjtime = ptp_vclock_adjtime,
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800163 .settime64 = ptp_vclock_settime,
164 .do_aux_work = ptp_vclock_refresh,
165};
166
167static u64 ptp_vclock_read(const struct cyclecounter *cc)
168{
169 struct ptp_vclock *vclock = cc_to_vclock(cc);
170 struct ptp_clock *ptp = vclock->pclock;
171 struct timespec64 ts = {};
172
Gerhard Engleder42704b22022-05-06 22:01:37 +0200173 ptp->info->getcycles64(ptp->info, &ts);
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800174
175 return timespec64_to_ns(&ts);
176}
177
178static const struct cyclecounter ptp_vclock_cc = {
179 .read = ptp_vclock_read,
180 .mask = CYCLECOUNTER_MASK(32),
181 .mult = PTP_VCLOCK_CC_MULT,
182 .shift = PTP_VCLOCK_CC_SHIFT,
183};
184
185struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock)
186{
187 struct ptp_vclock *vclock;
188
189 vclock = kzalloc(sizeof(*vclock), GFP_KERNEL);
190 if (!vclock)
191 return NULL;
192
193 vclock->pclock = pclock;
194 vclock->info = ptp_vclock_info;
Gerhard Engleder42704b22022-05-06 22:01:37 +0200195 if (pclock->info->getcyclesx64)
Miroslav Lichvarf0067eb2022-02-02 10:33:57 +0100196 vclock->info.gettimex64 = ptp_vclock_gettimex;
197 else
198 vclock->info.gettime64 = ptp_vclock_gettime;
Gerhard Engleder42704b22022-05-06 22:01:37 +0200199 if (pclock->info->getcrosscycles)
Miroslav Lichvar21fad632022-02-02 10:33:58 +0100200 vclock->info.getcrosststamp = ptp_vclock_getcrosststamp;
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800201 vclock->cc = ptp_vclock_cc;
202
203 snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt",
204 pclock->index);
205
Gerhard Englederfcf308e2022-05-06 22:01:41 +0200206 INIT_HLIST_NODE(&vclock->vclock_hash_node);
207
Íñigo Huguet67d93ff2023-02-21 14:06:16 +0100208 mutex_init(&vclock->lock);
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800209
210 vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev);
211 if (IS_ERR_OR_NULL(vclock->clock)) {
212 kfree(vclock);
213 return NULL;
214 }
215
216 timecounter_init(&vclock->tc, &vclock->cc, 0);
217 ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL);
218
Gerhard Englederfcf308e2022-05-06 22:01:41 +0200219 ptp_vclock_hash_add(vclock);
220
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800221 return vclock;
222}
223
224void ptp_vclock_unregister(struct ptp_vclock *vclock)
225{
Gerhard Englederfcf308e2022-05-06 22:01:41 +0200226 ptp_vclock_hash_del(vclock);
227
Yangbo Lu5d43f9512021-06-30 16:11:52 +0800228 ptp_clock_unregister(vclock->clock);
229 kfree(vclock);
230}
Yangbo Luacb288e2021-06-30 16:11:55 +0800231
Arnd Bergmanne5f31552021-08-12 20:33:58 +0200232#if IS_BUILTIN(CONFIG_PTP_1588_CLOCK)
Yangbo Luacb288e2021-06-30 16:11:55 +0800233int ptp_get_vclocks_index(int pclock_index, int **vclock_index)
234{
235 char name[PTP_CLOCK_NAME_LEN] = "";
236 struct ptp_clock *ptp;
237 struct device *dev;
238 int num = 0;
239
240 if (pclock_index < 0)
241 return num;
242
243 snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index);
244 dev = class_find_device_by_name(ptp_class, name);
245 if (!dev)
246 return num;
247
248 ptp = dev_get_drvdata(dev);
249
250 if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) {
251 put_device(dev);
252 return num;
253 }
254
255 *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL);
256 if (!(*vclock_index))
257 goto out;
258
259 memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks);
260 num = ptp->n_vclocks;
261out:
262 mutex_unlock(&ptp->n_vclocks_mux);
263 put_device(dev);
264 return num;
265}
266EXPORT_SYMBOL(ptp_get_vclocks_index);
Yangbo Lu895487a2021-06-30 16:11:57 +0800267
Gerhard Englederd58809d2022-05-06 22:01:39 +0200268ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index)
Yangbo Lu895487a2021-06-30 16:11:57 +0800269{
Gerhard Englederfcf308e2022-05-06 22:01:41 +0200270 unsigned int hash = vclock_index % HASH_SIZE(vclock_hash);
Yangbo Lu895487a2021-06-30 16:11:57 +0800271 struct ptp_vclock *vclock;
Yangbo Lu895487a2021-06-30 16:11:57 +0800272 u64 ns;
Gerhard Englederfcf308e2022-05-06 22:01:41 +0200273 u64 vclock_ns = 0;
Yangbo Lu895487a2021-06-30 16:11:57 +0800274
Gerhard Englederd58809d2022-05-06 22:01:39 +0200275 ns = ktime_to_ns(*hwtstamp);
Yangbo Lu895487a2021-06-30 16:11:57 +0800276
Gerhard Englederfcf308e2022-05-06 22:01:41 +0200277 rcu_read_lock();
Yangbo Lu895487a2021-06-30 16:11:57 +0800278
Gerhard Englederfcf308e2022-05-06 22:01:41 +0200279 hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) {
280 if (vclock->clock->index != vclock_index)
281 continue;
282
Íñigo Huguet67d93ff2023-02-21 14:06:16 +0100283 if (mutex_lock_interruptible(&vclock->lock))
284 break;
Gerhard Englederfcf308e2022-05-06 22:01:41 +0200285 vclock_ns = timecounter_cyc2time(&vclock->tc, ns);
Íñigo Huguet67d93ff2023-02-21 14:06:16 +0100286 mutex_unlock(&vclock->lock);
Gerhard Englederfcf308e2022-05-06 22:01:41 +0200287 break;
288 }
289
290 rcu_read_unlock();
291
292 return ns_to_ktime(vclock_ns);
Yangbo Lu895487a2021-06-30 16:11:57 +0800293}
294EXPORT_SYMBOL(ptp_convert_timestamp);
Arnd Bergmanne5f31552021-08-12 20:33:58 +0200295#endif