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 "ptp_private.h" 9 10 #define PTP_VCLOCK_CC_SHIFT 31 11 #define PTP_VCLOCK_CC_MULT (1 << PTP_VCLOCK_CC_SHIFT) 12 #define PTP_VCLOCK_FADJ_SHIFT 9 13 #define PTP_VCLOCK_FADJ_DENOMINATOR 15625ULL 14 #define PTP_VCLOCK_REFRESH_INTERVAL (HZ * 2) 15 16 static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 17 { 18 struct ptp_vclock *vclock = info_to_vclock(ptp); 19 unsigned long flags; 20 s64 adj; 21 22 adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT; 23 adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR); 24 25 spin_lock_irqsave(&vclock->lock, flags); 26 timecounter_read(&vclock->tc); 27 vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj; 28 spin_unlock_irqrestore(&vclock->lock, flags); 29 30 return 0; 31 } 32 33 static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta) 34 { 35 struct ptp_vclock *vclock = info_to_vclock(ptp); 36 unsigned long flags; 37 38 spin_lock_irqsave(&vclock->lock, flags); 39 timecounter_adjtime(&vclock->tc, delta); 40 spin_unlock_irqrestore(&vclock->lock, flags); 41 42 return 0; 43 } 44 45 static int ptp_vclock_gettime(struct ptp_clock_info *ptp, 46 struct timespec64 *ts) 47 { 48 struct ptp_vclock *vclock = info_to_vclock(ptp); 49 unsigned long flags; 50 u64 ns; 51 52 spin_lock_irqsave(&vclock->lock, flags); 53 ns = timecounter_read(&vclock->tc); 54 spin_unlock_irqrestore(&vclock->lock, flags); 55 *ts = ns_to_timespec64(ns); 56 57 return 0; 58 } 59 60 static int ptp_vclock_settime(struct ptp_clock_info *ptp, 61 const struct timespec64 *ts) 62 { 63 struct ptp_vclock *vclock = info_to_vclock(ptp); 64 u64 ns = timespec64_to_ns(ts); 65 unsigned long flags; 66 67 spin_lock_irqsave(&vclock->lock, flags); 68 timecounter_init(&vclock->tc, &vclock->cc, ns); 69 spin_unlock_irqrestore(&vclock->lock, flags); 70 71 return 0; 72 } 73 74 static long ptp_vclock_refresh(struct ptp_clock_info *ptp) 75 { 76 struct ptp_vclock *vclock = info_to_vclock(ptp); 77 struct timespec64 ts; 78 79 ptp_vclock_gettime(&vclock->info, &ts); 80 81 return PTP_VCLOCK_REFRESH_INTERVAL; 82 } 83 84 static const struct ptp_clock_info ptp_vclock_info = { 85 .owner = THIS_MODULE, 86 .name = "ptp virtual clock", 87 /* The maximum ppb value that long scaled_ppm can support */ 88 .max_adj = 32767999, 89 .adjfine = ptp_vclock_adjfine, 90 .adjtime = ptp_vclock_adjtime, 91 .gettime64 = ptp_vclock_gettime, 92 .settime64 = ptp_vclock_settime, 93 .do_aux_work = ptp_vclock_refresh, 94 }; 95 96 static u64 ptp_vclock_read(const struct cyclecounter *cc) 97 { 98 struct ptp_vclock *vclock = cc_to_vclock(cc); 99 struct ptp_clock *ptp = vclock->pclock; 100 struct timespec64 ts = {}; 101 102 if (ptp->info->gettimex64) 103 ptp->info->gettimex64(ptp->info, &ts, NULL); 104 else 105 ptp->info->gettime64(ptp->info, &ts); 106 107 return timespec64_to_ns(&ts); 108 } 109 110 static const struct cyclecounter ptp_vclock_cc = { 111 .read = ptp_vclock_read, 112 .mask = CYCLECOUNTER_MASK(32), 113 .mult = PTP_VCLOCK_CC_MULT, 114 .shift = PTP_VCLOCK_CC_SHIFT, 115 }; 116 117 struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock) 118 { 119 struct ptp_vclock *vclock; 120 121 vclock = kzalloc(sizeof(*vclock), GFP_KERNEL); 122 if (!vclock) 123 return NULL; 124 125 vclock->pclock = pclock; 126 vclock->info = ptp_vclock_info; 127 vclock->cc = ptp_vclock_cc; 128 129 snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt", 130 pclock->index); 131 132 spin_lock_init(&vclock->lock); 133 134 vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev); 135 if (IS_ERR_OR_NULL(vclock->clock)) { 136 kfree(vclock); 137 return NULL; 138 } 139 140 timecounter_init(&vclock->tc, &vclock->cc, 0); 141 ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL); 142 143 return vclock; 144 } 145 146 void ptp_vclock_unregister(struct ptp_vclock *vclock) 147 { 148 ptp_clock_unregister(vclock->clock); 149 kfree(vclock); 150 } 151