xref: /openbmc/linux/drivers/ptp/ptp_mock.c (revision 40b0425f)
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 
mock_phc_cc_read(const struct cyclecounter * cc)44 static u64 mock_phc_cc_read(const struct cyclecounter *cc)
45 {
46 	return ktime_get_raw_ns();
47 }
48 
mock_phc_adjfine(struct ptp_clock_info * info,long scaled_ppm)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 
mock_phc_adjtime(struct ptp_clock_info * info,s64 delta)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 
mock_phc_settime64(struct ptp_clock_info * info,const struct timespec64 * ts)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 
mock_phc_gettime64(struct ptp_clock_info * info,struct timespec64 * ts)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 
mock_phc_refresh(struct ptp_clock_info * info)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 
mock_phc_index(struct mock_phc * phc)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 
mock_phc_create(struct device * dev)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 
mock_phc_destroy(struct mock_phc * phc)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