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 | |
44 | static u64 mock_phc_cc_read(const struct cyclecounter *cc) |
45 | { |
46 | return ktime_get_raw_ns(); |
47 | } |
48 | |
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(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 | |
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(lock: &phc->lock); |
70 | timecounter_adjtime(tc: &phc->tc, delta); |
71 | spin_unlock(lock: &phc->lock); |
72 | |
73 | return 0; |
74 | } |
75 | |
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(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 | |
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(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 | |
103 | static 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 | |
112 | int mock_phc_index(struct mock_phc *phc) |
113 | { |
114 | return ptp_clock_index(ptp: phc->clock); |
115 | } |
116 | EXPORT_SYMBOL_GPL(mock_phc_index); |
117 | |
118 | struct 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 | |
160 | out_free_phc: |
161 | kfree(objp: phc); |
162 | out: |
163 | return ERR_PTR(error: err); |
164 | } |
165 | EXPORT_SYMBOL_GPL(mock_phc_create); |
166 | |
167 | void mock_phc_destroy(struct mock_phc *phc) |
168 | { |
169 | ptp_clock_unregister(ptp: phc->clock); |
170 | kfree(objp: phc); |
171 | } |
172 | EXPORT_SYMBOL_GPL(mock_phc_destroy); |
173 | |
174 | MODULE_DESCRIPTION("Mock-up PTP Hardware Clock driver" ); |
175 | MODULE_LICENSE("GPL" ); |
176 | |