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