176 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * Copyright 2023 NXP4 *5 * Mock-up PTP Hardware Clock driver for virtual network devices6 *7 * Create a PTP clock which offers PTP time manipulation operations8 * 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,47617 */18#define MOCK_PHC_MAX_ADJ_PPB 3200000019/* Timestamps from ktime_get_raw() have 1 ns resolution, so the scale factor20 * (MULT >> SHIFT) needs to be 1. Pick SHIFT as 31 bits, which translates21 * MULT(freq 0) into 0x80000000.22 */23#define MOCK_PHC_CC_SHIFT 3124#define MOCK_PHC_CC_MULT (1 << MOCK_PHC_CC_SHIFT)25#define MOCK_PHC_FADJ_SHIFT 926#define MOCK_PHC_FADJ_DENOMINATOR 15625ULL27 28/* The largest cycle_delta that timecounter_read_delta() can handle without a29 * 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