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 36struct 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 44static u64 mock_phc_cc_read(const struct cyclecounter *cc) 45{ 46 return ktime_get_raw_ns(); 47} 48 49static 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 65static 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 76static 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 89static 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 103static 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 112int mock_phc_index(struct mock_phc *phc) 113{ 114 return ptp_clock_index(phc->clock); 115} 116EXPORT_SYMBOL_GPL(mock_phc_index); 117 118struct 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 160out_free_phc: 161 kfree(phc); 162out: 163 return ERR_PTR(err); 164} 165EXPORT_SYMBOL_GPL(mock_phc_create); 166 167void mock_phc_destroy(struct mock_phc *phc) 168{ 169 ptp_clock_unregister(phc->clock); 170 kfree(phc); 171} 172EXPORT_SYMBOL_GPL(mock_phc_destroy); 173 174MODULE_DESCRIPTION("Mock-up PTP Hardware Clock driver"); 175MODULE_LICENSE("GPL"); 176