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");