1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Copyright 2023 NXP 4 * 5 * Mock-up PTP Hardware Clock driver for virtual network devices 6 * 7 * Create a PTP clock which offers PTP time manipulation operations 8 * using a timecounter/cyclecounter on top of CLOCK_MONOTONIC_RAW. 9 */ 10 11 #include <linux/ptp_clock_kernel.h> 12 #include <linux/ptp_mock.h> 13 #include <linux/timecounter.h> 14 15 /* Clamp scaled_ppm between -2,097,152,000 and 2,097,152,000, 16 * and thus "adj" between -68,719,476 and 68,719,476 17 */ 18 #define MOCK_PHC_MAX_ADJ_PPB 32000000 19 /* Timestamps from ktime_get_raw() have 1 ns resolution, so the scale factor 20 * (MULT >> SHIFT) needs to be 1. Pick SHIFT as 31 bits, which translates 21 * MULT(freq 0) into 0x80000000. 22 */ 23 #define MOCK_PHC_CC_SHIFT 31 24 #define MOCK_PHC_CC_MULT (1 << MOCK_PHC_CC_SHIFT) 25 #define MOCK_PHC_FADJ_SHIFT 9 26 #define MOCK_PHC_FADJ_DENOMINATOR 15625ULL 27 28 /* The largest cycle_delta that timecounter_read_delta() can handle without a 29 * 64-bit overflow during the multiplication with cc->mult, given the max "adj" 30 * we permit, is ~8.3 seconds. Make sure readouts are more frequent than that. 31 */ 32 #define MOCK_PHC_REFRESH_INTERVAL (HZ * 5) 33 34 #define info_to_phc(d) container_of((d), struct mock_phc, info) 35 36 struct mock_phc { 37 struct ptp_clock_info info; 38 struct ptp_clock *clock; 39 struct timecounter tc; 40 struct cyclecounter cc; 41 spinlock_t lock; 42 }; 43 44 static u64 mock_phc_cc_read(const struct cyclecounter *cc) 45 { 46 return ktime_get_raw_ns(); 47 } 48 49 static int mock_phc_adjfine(struct ptp_clock_info *info, long scaled_ppm) 50 { 51 struct mock_phc *phc = info_to_phc(info); 52 s64 adj; 53 54 adj = (s64)scaled_ppm << MOCK_PHC_FADJ_SHIFT; 55 adj = div_s64(adj, MOCK_PHC_FADJ_DENOMINATOR); 56 57 spin_lock(&phc->lock); 58 timecounter_read(&phc->tc); 59 phc->cc.mult = MOCK_PHC_CC_MULT + adj; 60 spin_unlock(&phc->lock); 61 62 return 0; 63 } 64 65 static int mock_phc_adjtime(struct ptp_clock_info *info, s64 delta) 66 { 67 struct mock_phc *phc = info_to_phc(info); 68 69 spin_lock(&phc->lock); 70 timecounter_adjtime(&phc->tc, delta); 71 spin_unlock(&phc->lock); 72 73 return 0; 74 } 75 76 static int mock_phc_settime64(struct ptp_clock_info *info, 77 const struct timespec64 *ts) 78 { 79 struct mock_phc *phc = info_to_phc(info); 80 u64 ns = timespec64_to_ns(ts); 81 82 spin_lock(&phc->lock); 83 timecounter_init(&phc->tc, &phc->cc, ns); 84 spin_unlock(&phc->lock); 85 86 return 0; 87 } 88 89 static int mock_phc_gettime64(struct ptp_clock_info *info, struct timespec64 *ts) 90 { 91 struct mock_phc *phc = info_to_phc(info); 92 u64 ns; 93 94 spin_lock(&phc->lock); 95 ns = timecounter_read(&phc->tc); 96 spin_unlock(&phc->lock); 97 98 *ts = ns_to_timespec64(ns); 99 100 return 0; 101 } 102 103 static long mock_phc_refresh(struct ptp_clock_info *info) 104 { 105 struct timespec64 ts; 106 107 mock_phc_gettime64(info, &ts); 108 109 return MOCK_PHC_REFRESH_INTERVAL; 110 } 111 112 int mock_phc_index(struct mock_phc *phc) 113 { 114 return ptp_clock_index(phc->clock); 115 } 116 EXPORT_SYMBOL_GPL(mock_phc_index); 117 118 struct mock_phc *mock_phc_create(struct device *dev) 119 { 120 struct mock_phc *phc; 121 int err; 122 123 phc = kzalloc(sizeof(*phc), GFP_KERNEL); 124 if (!phc) { 125 err = -ENOMEM; 126 goto out; 127 } 128 129 phc->info = (struct ptp_clock_info) { 130 .owner = THIS_MODULE, 131 .name = "Mock-up PTP clock", 132 .max_adj = MOCK_PHC_MAX_ADJ_PPB, 133 .adjfine = mock_phc_adjfine, 134 .adjtime = mock_phc_adjtime, 135 .gettime64 = mock_phc_gettime64, 136 .settime64 = mock_phc_settime64, 137 .do_aux_work = mock_phc_refresh, 138 }; 139 140 phc->cc = (struct cyclecounter) { 141 .read = mock_phc_cc_read, 142 .mask = CYCLECOUNTER_MASK(64), 143 .mult = MOCK_PHC_CC_MULT, 144 .shift = MOCK_PHC_CC_SHIFT, 145 }; 146 147 spin_lock_init(&phc->lock); 148 timecounter_init(&phc->tc, &phc->cc, 0); 149 150 phc->clock = ptp_clock_register(&phc->info, dev); 151 if (IS_ERR(phc->clock)) { 152 err = PTR_ERR(phc->clock); 153 goto out_free_phc; 154 } 155 156 ptp_schedule_worker(phc->clock, MOCK_PHC_REFRESH_INTERVAL); 157 158 return phc; 159 160 out_free_phc: 161 kfree(phc); 162 out: 163 return ERR_PTR(err); 164 } 165 EXPORT_SYMBOL_GPL(mock_phc_create); 166 167 void mock_phc_destroy(struct mock_phc *phc) 168 { 169 ptp_clock_unregister(phc->clock); 170 kfree(phc); 171 } 172 EXPORT_SYMBOL_GPL(mock_phc_destroy); 173 174 MODULE_DESCRIPTION("Mock-up PTP Hardware Clock driver"); 175 MODULE_LICENSE("GPL"); 176