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(dividend: adj, MOCK_PHC_FADJ_DENOMINATOR);
56
57 spin_lock(lock: &phc->lock);
58 timecounter_read(tc: &phc->tc);
59 phc->cc.mult = MOCK_PHC_CC_MULT + adj;
60 spin_unlock(lock: &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(lock: &phc->lock);
70 timecounter_adjtime(tc: &phc->tc, delta);
71 spin_unlock(lock: &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(lock: &phc->lock);
83 timecounter_init(tc: &phc->tc, cc: &phc->cc, start_tstamp: ns);
84 spin_unlock(lock: &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(lock: &phc->lock);
95 ns = timecounter_read(tc: &phc->tc);
96 spin_unlock(lock: &phc->lock);
97
98 *ts = ns_to_timespec64(nsec: 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: &ts);
108
109 return MOCK_PHC_REFRESH_INTERVAL;
110}
111
112int mock_phc_index(struct mock_phc *phc)
113{
114 return ptp_clock_index(ptp: 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(size: 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(tc: &phc->tc, cc: &phc->cc, start_tstamp: 0);
149
150 phc->clock = ptp_clock_register(info: &phc->info, parent: dev);
151 if (IS_ERR(ptr: phc->clock)) {
152 err = PTR_ERR(ptr: phc->clock);
153 goto out_free_phc;
154 }
155
156 ptp_schedule_worker(ptp: phc->clock, MOCK_PHC_REFRESH_INTERVAL);
157
158 return phc;
159
160out_free_phc:
161 kfree(objp: phc);
162out:
163 return ERR_PTR(error: err);
164}
165EXPORT_SYMBOL_GPL(mock_phc_create);
166
167void mock_phc_destroy(struct mock_phc *phc)
168{
169 ptp_clock_unregister(ptp: phc->clock);
170 kfree(objp: phc);
171}
172EXPORT_SYMBOL_GPL(mock_phc_destroy);
173
174MODULE_DESCRIPTION("Mock-up PTP Hardware Clock driver");
175MODULE_LICENSE("GPL");
176

source code of linux/drivers/ptp/ptp_mock.c