Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 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> |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 8 | #include <linux/hashtable.h> |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 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 | |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 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 | 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 | |
| 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 | |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 43 | static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) |
| 44 | { |
| 45 | struct ptp_vclock *vclock = info_to_vclock(ptp); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 46 | s64 adj; |
| 47 | |
| 48 | adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT; |
| 49 | adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR); |
| 50 | |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 51 | if (mutex_lock_interruptible(&vclock->lock)) |
| 52 | return -EINTR; |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 53 | timecounter_read(&vclock->tc); |
| 54 | vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj; |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 55 | mutex_unlock(&vclock->lock); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 56 | |
| 57 | return 0; |
| 58 | } |
| 59 | |
| 60 | static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta) |
| 61 | { |
| 62 | struct ptp_vclock *vclock = info_to_vclock(ptp); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 63 | |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 64 | if (mutex_lock_interruptible(&vclock->lock)) |
| 65 | return -EINTR; |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 66 | timecounter_adjtime(&vclock->tc, delta); |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 67 | mutex_unlock(&vclock->lock); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 68 | |
| 69 | return 0; |
| 70 | } |
| 71 | |
| 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); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 76 | u64 ns; |
| 77 | |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 78 | if (mutex_lock_interruptible(&vclock->lock)) |
| 79 | return -EINTR; |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 80 | ns = timecounter_read(&vclock->tc); |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 81 | mutex_unlock(&vclock->lock); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 82 | *ts = ns_to_timespec64(ns); |
| 83 | |
| 84 | return 0; |
| 85 | } |
| 86 | |
Miroslav Lichvar | f0067eb | 2022-02-02 10:33:57 +0100 | [diff] [blame] | 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; |
Miroslav Lichvar | f0067eb | 2022-02-02 10:33:57 +0100 | [diff] [blame] | 94 | int err; |
| 95 | u64 ns; |
| 96 | |
Gerhard Engleder | 42704b2 | 2022-05-06 22:01:37 +0200 | [diff] [blame] | 97 | err = pptp->info->getcyclesx64(pptp->info, &pts, sts); |
Miroslav Lichvar | f0067eb | 2022-02-02 10:33:57 +0100 | [diff] [blame] | 98 | if (err) |
| 99 | return err; |
| 100 | |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 101 | if (mutex_lock_interruptible(&vclock->lock)) |
| 102 | return -EINTR; |
Miroslav Lichvar | f0067eb | 2022-02-02 10:33:57 +0100 | [diff] [blame] | 103 | ns = timecounter_cyc2time(&vclock->tc, timespec64_to_ns(&pts)); |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 104 | mutex_unlock(&vclock->lock); |
Miroslav Lichvar | f0067eb | 2022-02-02 10:33:57 +0100 | [diff] [blame] | 105 | |
| 106 | *ts = ns_to_timespec64(ns); |
| 107 | |
| 108 | return 0; |
| 109 | } |
| 110 | |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 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); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 116 | |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 117 | if (mutex_lock_interruptible(&vclock->lock)) |
| 118 | return -EINTR; |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 119 | timecounter_init(&vclock->tc, &vclock->cc, ns); |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 120 | mutex_unlock(&vclock->lock); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 121 | |
| 122 | return 0; |
| 123 | } |
| 124 | |
Miroslav Lichvar | 21fad63 | 2022-02-02 10:33:58 +0100 | [diff] [blame] | 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; |
Miroslav Lichvar | 21fad63 | 2022-02-02 10:33:58 +0100 | [diff] [blame] | 130 | int err; |
| 131 | u64 ns; |
| 132 | |
Gerhard Engleder | 42704b2 | 2022-05-06 22:01:37 +0200 | [diff] [blame] | 133 | err = pptp->info->getcrosscycles(pptp->info, xtstamp); |
Miroslav Lichvar | 21fad63 | 2022-02-02 10:33:58 +0100 | [diff] [blame] | 134 | if (err) |
| 135 | return err; |
| 136 | |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 137 | if (mutex_lock_interruptible(&vclock->lock)) |
| 138 | return -EINTR; |
Miroslav Lichvar | 21fad63 | 2022-02-02 10:33:58 +0100 | [diff] [blame] | 139 | ns = timecounter_cyc2time(&vclock->tc, ktime_to_ns(xtstamp->device)); |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 140 | mutex_unlock(&vclock->lock); |
Miroslav Lichvar | 21fad63 | 2022-02-02 10:33:58 +0100 | [diff] [blame] | 141 | |
| 142 | xtstamp->device = ns_to_ktime(ns); |
| 143 | |
| 144 | return 0; |
| 145 | } |
| 146 | |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 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 | |
| 157 | static const struct ptp_clock_info ptp_vclock_info = { |
| 158 | .owner = THIS_MODULE, |
| 159 | .name = "ptp virtual clock", |
Miroslav Lichvar | f77222d | 2022-02-02 10:33:56 +0100 | [diff] [blame] | 160 | .max_adj = 500000000, |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 161 | .adjfine = ptp_vclock_adjfine, |
| 162 | .adjtime = ptp_vclock_adjtime, |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 163 | .settime64 = ptp_vclock_settime, |
| 164 | .do_aux_work = ptp_vclock_refresh, |
| 165 | }; |
| 166 | |
| 167 | static 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 Engleder | 42704b2 | 2022-05-06 22:01:37 +0200 | [diff] [blame] | 173 | ptp->info->getcycles64(ptp->info, &ts); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 174 | |
| 175 | return timespec64_to_ns(&ts); |
| 176 | } |
| 177 | |
| 178 | static 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 | |
| 185 | struct 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 Engleder | 42704b2 | 2022-05-06 22:01:37 +0200 | [diff] [blame] | 195 | if (pclock->info->getcyclesx64) |
Miroslav Lichvar | f0067eb | 2022-02-02 10:33:57 +0100 | [diff] [blame] | 196 | vclock->info.gettimex64 = ptp_vclock_gettimex; |
| 197 | else |
| 198 | vclock->info.gettime64 = ptp_vclock_gettime; |
Gerhard Engleder | 42704b2 | 2022-05-06 22:01:37 +0200 | [diff] [blame] | 199 | if (pclock->info->getcrosscycles) |
Miroslav Lichvar | 21fad63 | 2022-02-02 10:33:58 +0100 | [diff] [blame] | 200 | vclock->info.getcrosststamp = ptp_vclock_getcrosststamp; |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 201 | vclock->cc = ptp_vclock_cc; |
| 202 | |
| 203 | snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt", |
| 204 | pclock->index); |
| 205 | |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 206 | INIT_HLIST_NODE(&vclock->vclock_hash_node); |
| 207 | |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 208 | mutex_init(&vclock->lock); |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 209 | |
| 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 Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 219 | ptp_vclock_hash_add(vclock); |
| 220 | |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 221 | return vclock; |
| 222 | } |
| 223 | |
| 224 | void ptp_vclock_unregister(struct ptp_vclock *vclock) |
| 225 | { |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 226 | ptp_vclock_hash_del(vclock); |
| 227 | |
Yangbo Lu | 5d43f951 | 2021-06-30 16:11:52 +0800 | [diff] [blame] | 228 | ptp_clock_unregister(vclock->clock); |
| 229 | kfree(vclock); |
| 230 | } |
Yangbo Lu | acb288e | 2021-06-30 16:11:55 +0800 | [diff] [blame] | 231 | |
Arnd Bergmann | e5f3155 | 2021-08-12 20:33:58 +0200 | [diff] [blame] | 232 | #if IS_BUILTIN(CONFIG_PTP_1588_CLOCK) |
Yangbo Lu | acb288e | 2021-06-30 16:11:55 +0800 | [diff] [blame] | 233 | int 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; |
| 261 | out: |
| 262 | mutex_unlock(&ptp->n_vclocks_mux); |
| 263 | put_device(dev); |
| 264 | return num; |
| 265 | } |
| 266 | EXPORT_SYMBOL(ptp_get_vclocks_index); |
Yangbo Lu | 895487a | 2021-06-30 16:11:57 +0800 | [diff] [blame] | 267 | |
Gerhard Engleder | d58809d | 2022-05-06 22:01:39 +0200 | [diff] [blame] | 268 | ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index) |
Yangbo Lu | 895487a | 2021-06-30 16:11:57 +0800 | [diff] [blame] | 269 | { |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 270 | unsigned int hash = vclock_index % HASH_SIZE(vclock_hash); |
Yangbo Lu | 895487a | 2021-06-30 16:11:57 +0800 | [diff] [blame] | 271 | struct ptp_vclock *vclock; |
Yangbo Lu | 895487a | 2021-06-30 16:11:57 +0800 | [diff] [blame] | 272 | u64 ns; |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 273 | u64 vclock_ns = 0; |
Yangbo Lu | 895487a | 2021-06-30 16:11:57 +0800 | [diff] [blame] | 274 | |
Gerhard Engleder | d58809d | 2022-05-06 22:01:39 +0200 | [diff] [blame] | 275 | ns = ktime_to_ns(*hwtstamp); |
Yangbo Lu | 895487a | 2021-06-30 16:11:57 +0800 | [diff] [blame] | 276 | |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 277 | rcu_read_lock(); |
Yangbo Lu | 895487a | 2021-06-30 16:11:57 +0800 | [diff] [blame] | 278 | |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 279 | hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) { |
| 280 | if (vclock->clock->index != vclock_index) |
| 281 | continue; |
| 282 | |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 283 | if (mutex_lock_interruptible(&vclock->lock)) |
| 284 | break; |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 285 | vclock_ns = timecounter_cyc2time(&vclock->tc, ns); |
Íñigo Huguet | 67d93ff | 2023-02-21 14:06:16 +0100 | [diff] [blame] | 286 | mutex_unlock(&vclock->lock); |
Gerhard Engleder | fcf308e | 2022-05-06 22:01:41 +0200 | [diff] [blame] | 287 | break; |
| 288 | } |
| 289 | |
| 290 | rcu_read_unlock(); |
| 291 | |
| 292 | return ns_to_ktime(vclock_ns); |
Yangbo Lu | 895487a | 2021-06-30 16:11:57 +0800 | [diff] [blame] | 293 | } |
| 294 | EXPORT_SYMBOL(ptp_convert_timestamp); |
Arnd Bergmann | e5f3155 | 2021-08-12 20:33:58 +0200 | [diff] [blame] | 295 | #endif |