1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * DFL device driver for Time-of-Day (ToD) private feature |
4 | * |
5 | * Copyright (C) 2023 Intel Corporation |
6 | */ |
7 | |
8 | #include <linux/bitfield.h> |
9 | #include <linux/delay.h> |
10 | #include <linux/dfl.h> |
11 | #include <linux/gcd.h> |
12 | #include <linux/iopoll.h> |
13 | #include <linux/module.h> |
14 | #include <linux/ptp_clock_kernel.h> |
15 | #include <linux/spinlock.h> |
16 | #include <linux/units.h> |
17 | |
18 | #define FME_FEATURE_ID_TOD 0x22 |
19 | |
20 | /* ToD clock register space. */ |
21 | #define TOD_CLK_FREQ 0x038 |
22 | |
23 | /* |
24 | * The read sequence of ToD timestamp registers: TOD_NANOSEC, TOD_SECONDSL and |
25 | * TOD_SECONDSH, because there is a hardware snapshot whenever the TOD_NANOSEC |
26 | * register is read. |
27 | * |
28 | * The ToD IP requires writing registers in the reverse order to the read sequence. |
29 | * The timestamp is corrected when the TOD_NANOSEC register is written, so the |
30 | * sequence of write TOD registers: TOD_SECONDSH, TOD_SECONDSL and TOD_NANOSEC. |
31 | */ |
32 | #define TOD_SECONDSH 0x100 |
33 | #define TOD_SECONDSL 0x104 |
34 | #define TOD_NANOSEC 0x108 |
35 | #define TOD_PERIOD 0x110 |
36 | #define TOD_ADJUST_PERIOD 0x114 |
37 | #define TOD_ADJUST_COUNT 0x118 |
38 | #define TOD_DRIFT_ADJUST 0x11c |
39 | #define TOD_DRIFT_ADJUST_RATE 0x120 |
40 | #define PERIOD_FRAC_OFFSET 16 |
41 | #define SECONDS_MSB GENMASK_ULL(47, 32) |
42 | #define SECONDS_LSB GENMASK_ULL(31, 0) |
43 | #define TOD_SECONDSH_SEC_MSB GENMASK_ULL(15, 0) |
44 | |
45 | #define CAL_SECONDS(m, l) ((FIELD_GET(TOD_SECONDSH_SEC_MSB, (m)) << 32) | (l)) |
46 | |
47 | #define TOD_PERIOD_MASK GENMASK_ULL(19, 0) |
48 | #define TOD_PERIOD_MAX FIELD_MAX(TOD_PERIOD_MASK) |
49 | #define TOD_PERIOD_MIN 0 |
50 | #define TOD_DRIFT_ADJUST_MASK GENMASK_ULL(15, 0) |
51 | #define TOD_DRIFT_ADJUST_FNS_MAX FIELD_MAX(TOD_DRIFT_ADJUST_MASK) |
52 | #define TOD_DRIFT_ADJUST_RATE_MAX TOD_DRIFT_ADJUST_FNS_MAX |
53 | #define TOD_ADJUST_COUNT_MASK GENMASK_ULL(19, 0) |
54 | #define TOD_ADJUST_COUNT_MAX FIELD_MAX(TOD_ADJUST_COUNT_MASK) |
55 | #define TOD_ADJUST_INTERVAL_US 10 |
56 | #define TOD_ADJUST_MS \ |
57 | (((TOD_PERIOD_MAX >> 16) + 1) * (TOD_ADJUST_COUNT_MAX + 1)) |
58 | #define TOD_ADJUST_MS_MAX (TOD_ADJUST_MS / MICRO) |
59 | #define TOD_ADJUST_MAX_US (TOD_ADJUST_MS_MAX * USEC_PER_MSEC) |
60 | #define TOD_MAX_ADJ (500 * MEGA) |
61 | |
62 | struct dfl_tod { |
63 | struct ptp_clock_info ptp_clock_ops; |
64 | struct device *dev; |
65 | struct ptp_clock *ptp_clock; |
66 | |
67 | /* ToD Clock address space */ |
68 | void __iomem *tod_ctrl; |
69 | |
70 | /* ToD clock registers protection */ |
71 | spinlock_t tod_lock; |
72 | }; |
73 | |
74 | /* |
75 | * A fine ToD HW clock offset adjustment. To perform the fine offset adjustment, the |
76 | * adjust_period and adjust_count argument are used to update the TOD_ADJUST_PERIOD |
77 | * and TOD_ADJUST_COUNT register for in hardware. The dt->tod_lock spinlock must be |
78 | * held when calling this function. |
79 | */ |
80 | static int fine_adjust_tod_clock(struct dfl_tod *dt, u32 adjust_period, |
81 | u32 adjust_count) |
82 | { |
83 | void __iomem *base = dt->tod_ctrl; |
84 | u32 val; |
85 | |
86 | writel(val: adjust_period, addr: base + TOD_ADJUST_PERIOD); |
87 | writel(val: adjust_count, addr: base + TOD_ADJUST_COUNT); |
88 | |
89 | /* Wait for present offset adjustment update to complete */ |
90 | return readl_poll_timeout_atomic(base + TOD_ADJUST_COUNT, val, !val, TOD_ADJUST_INTERVAL_US, |
91 | TOD_ADJUST_MAX_US); |
92 | } |
93 | |
94 | /* |
95 | * A coarse ToD HW clock offset adjustment. The coarse time adjustment performs by |
96 | * adding or subtracting the delta value from the current ToD HW clock time. |
97 | */ |
98 | static int coarse_adjust_tod_clock(struct dfl_tod *dt, s64 delta) |
99 | { |
100 | u32 seconds_msb, seconds_lsb, nanosec; |
101 | void __iomem *base = dt->tod_ctrl; |
102 | u64 seconds, now; |
103 | |
104 | if (delta == 0) |
105 | return 0; |
106 | |
107 | nanosec = readl(addr: base + TOD_NANOSEC); |
108 | seconds_lsb = readl(addr: base + TOD_SECONDSL); |
109 | seconds_msb = readl(addr: base + TOD_SECONDSH); |
110 | |
111 | /* Calculate new time */ |
112 | seconds = CAL_SECONDS(seconds_msb, seconds_lsb); |
113 | now = seconds * NSEC_PER_SEC + nanosec + delta; |
114 | |
115 | seconds = div_u64_rem(dividend: now, NSEC_PER_SEC, remainder: &nanosec); |
116 | seconds_msb = FIELD_GET(SECONDS_MSB, seconds); |
117 | seconds_lsb = FIELD_GET(SECONDS_LSB, seconds); |
118 | |
119 | writel(val: seconds_msb, addr: base + TOD_SECONDSH); |
120 | writel(val: seconds_lsb, addr: base + TOD_SECONDSL); |
121 | writel(val: nanosec, addr: base + TOD_NANOSEC); |
122 | |
123 | return 0; |
124 | } |
125 | |
126 | static int dfl_tod_adjust_fine(struct ptp_clock_info *ptp, long scaled_ppm) |
127 | { |
128 | struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops); |
129 | u32 tod_period, tod_rem, tod_drift_adjust_fns, tod_drift_adjust_rate; |
130 | void __iomem *base = dt->tod_ctrl; |
131 | unsigned long flags, rate; |
132 | u64 ppb; |
133 | |
134 | /* Get the clock rate from clock frequency register offset */ |
135 | rate = readl(addr: base + TOD_CLK_FREQ); |
136 | |
137 | /* add GIGA as nominal ppb */ |
138 | ppb = scaled_ppm_to_ppb(ppm: scaled_ppm) + GIGA; |
139 | |
140 | tod_period = div_u64_rem(dividend: ppb << PERIOD_FRAC_OFFSET, divisor: rate, remainder: &tod_rem); |
141 | if (tod_period > TOD_PERIOD_MAX) |
142 | return -ERANGE; |
143 | |
144 | /* |
145 | * The drift of ToD adjusted periodically by adding a drift_adjust_fns |
146 | * correction value every drift_adjust_rate count of clock cycles. |
147 | */ |
148 | tod_drift_adjust_fns = tod_rem / gcd(a: tod_rem, b: rate); |
149 | tod_drift_adjust_rate = rate / gcd(a: tod_rem, b: rate); |
150 | |
151 | while ((tod_drift_adjust_fns > TOD_DRIFT_ADJUST_FNS_MAX) || |
152 | (tod_drift_adjust_rate > TOD_DRIFT_ADJUST_RATE_MAX)) { |
153 | tod_drift_adjust_fns >>= 1; |
154 | tod_drift_adjust_rate >>= 1; |
155 | } |
156 | |
157 | if (tod_drift_adjust_fns == 0) |
158 | tod_drift_adjust_rate = 0; |
159 | |
160 | spin_lock_irqsave(&dt->tod_lock, flags); |
161 | writel(val: tod_period, addr: base + TOD_PERIOD); |
162 | writel(val: 0, addr: base + TOD_ADJUST_PERIOD); |
163 | writel(val: 0, addr: base + TOD_ADJUST_COUNT); |
164 | writel(val: tod_drift_adjust_fns, addr: base + TOD_DRIFT_ADJUST); |
165 | writel(val: tod_drift_adjust_rate, addr: base + TOD_DRIFT_ADJUST_RATE); |
166 | spin_unlock_irqrestore(lock: &dt->tod_lock, flags); |
167 | |
168 | return 0; |
169 | } |
170 | |
171 | static int dfl_tod_adjust_time(struct ptp_clock_info *ptp, s64 delta) |
172 | { |
173 | struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops); |
174 | u32 period, diff, rem, rem_period, adj_period; |
175 | void __iomem *base = dt->tod_ctrl; |
176 | unsigned long flags; |
177 | bool neg_adj; |
178 | u64 count; |
179 | int ret; |
180 | |
181 | neg_adj = delta < 0; |
182 | if (neg_adj) |
183 | delta = -delta; |
184 | |
185 | spin_lock_irqsave(&dt->tod_lock, flags); |
186 | |
187 | /* |
188 | * Get the maximum possible value of the Period register offset |
189 | * adjustment in nanoseconds scale. This depends on the current |
190 | * Period register setting and the maximum and minimum possible |
191 | * values of the Period register. |
192 | */ |
193 | period = readl(addr: base + TOD_PERIOD); |
194 | |
195 | if (neg_adj) { |
196 | diff = (period - TOD_PERIOD_MIN) >> PERIOD_FRAC_OFFSET; |
197 | adj_period = period - (diff << PERIOD_FRAC_OFFSET); |
198 | count = div_u64_rem(dividend: delta, divisor: diff, remainder: &rem); |
199 | rem_period = period - (rem << PERIOD_FRAC_OFFSET); |
200 | } else { |
201 | diff = (TOD_PERIOD_MAX - period) >> PERIOD_FRAC_OFFSET; |
202 | adj_period = period + (diff << PERIOD_FRAC_OFFSET); |
203 | count = div_u64_rem(dividend: delta, divisor: diff, remainder: &rem); |
204 | rem_period = period + (rem << PERIOD_FRAC_OFFSET); |
205 | } |
206 | |
207 | ret = 0; |
208 | |
209 | if (count > TOD_ADJUST_COUNT_MAX) { |
210 | ret = coarse_adjust_tod_clock(dt, delta); |
211 | } else { |
212 | /* Adjust the period by count cycles to adjust the time */ |
213 | if (count) |
214 | ret = fine_adjust_tod_clock(dt, adjust_period: adj_period, adjust_count: count); |
215 | |
216 | /* If there is a remainder, adjust the period for an additional cycle */ |
217 | if (rem) |
218 | ret = fine_adjust_tod_clock(dt, adjust_period: rem_period, adjust_count: 1); |
219 | } |
220 | |
221 | spin_unlock_irqrestore(lock: &dt->tod_lock, flags); |
222 | |
223 | return ret; |
224 | } |
225 | |
226 | static int dfl_tod_get_timex(struct ptp_clock_info *ptp, struct timespec64 *ts, |
227 | struct ptp_system_timestamp *sts) |
228 | { |
229 | struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops); |
230 | u32 seconds_msb, seconds_lsb, nanosec; |
231 | void __iomem *base = dt->tod_ctrl; |
232 | unsigned long flags; |
233 | u64 seconds; |
234 | |
235 | spin_lock_irqsave(&dt->tod_lock, flags); |
236 | ptp_read_system_prets(sts); |
237 | nanosec = readl(addr: base + TOD_NANOSEC); |
238 | seconds_lsb = readl(addr: base + TOD_SECONDSL); |
239 | seconds_msb = readl(addr: base + TOD_SECONDSH); |
240 | ptp_read_system_postts(sts); |
241 | spin_unlock_irqrestore(lock: &dt->tod_lock, flags); |
242 | |
243 | seconds = CAL_SECONDS(seconds_msb, seconds_lsb); |
244 | |
245 | ts->tv_nsec = nanosec; |
246 | ts->tv_sec = seconds; |
247 | |
248 | return 0; |
249 | } |
250 | |
251 | static int dfl_tod_set_time(struct ptp_clock_info *ptp, |
252 | const struct timespec64 *ts) |
253 | { |
254 | struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops); |
255 | u32 seconds_msb = FIELD_GET(SECONDS_MSB, ts->tv_sec); |
256 | u32 seconds_lsb = FIELD_GET(SECONDS_LSB, ts->tv_sec); |
257 | u32 nanosec = FIELD_GET(SECONDS_LSB, ts->tv_nsec); |
258 | void __iomem *base = dt->tod_ctrl; |
259 | unsigned long flags; |
260 | |
261 | spin_lock_irqsave(&dt->tod_lock, flags); |
262 | writel(val: seconds_msb, addr: base + TOD_SECONDSH); |
263 | writel(val: seconds_lsb, addr: base + TOD_SECONDSL); |
264 | writel(val: nanosec, addr: base + TOD_NANOSEC); |
265 | spin_unlock_irqrestore(lock: &dt->tod_lock, flags); |
266 | |
267 | return 0; |
268 | } |
269 | |
270 | static struct ptp_clock_info dfl_tod_clock_ops = { |
271 | .owner = THIS_MODULE, |
272 | .name = "dfl_tod" , |
273 | .max_adj = TOD_MAX_ADJ, |
274 | .adjfine = dfl_tod_adjust_fine, |
275 | .adjtime = dfl_tod_adjust_time, |
276 | .gettimex64 = dfl_tod_get_timex, |
277 | .settime64 = dfl_tod_set_time, |
278 | }; |
279 | |
280 | static int dfl_tod_probe(struct dfl_device *ddev) |
281 | { |
282 | struct device *dev = &ddev->dev; |
283 | struct dfl_tod *dt; |
284 | |
285 | dt = devm_kzalloc(dev, size: sizeof(*dt), GFP_KERNEL); |
286 | if (!dt) |
287 | return -ENOMEM; |
288 | |
289 | dt->tod_ctrl = devm_ioremap_resource(dev, res: &ddev->mmio_res); |
290 | if (IS_ERR(ptr: dt->tod_ctrl)) |
291 | return PTR_ERR(ptr: dt->tod_ctrl); |
292 | |
293 | dt->dev = dev; |
294 | spin_lock_init(&dt->tod_lock); |
295 | dev_set_drvdata(dev, data: dt); |
296 | |
297 | dt->ptp_clock_ops = dfl_tod_clock_ops; |
298 | |
299 | dt->ptp_clock = ptp_clock_register(info: &dt->ptp_clock_ops, parent: dev); |
300 | if (IS_ERR(ptr: dt->ptp_clock)) |
301 | return dev_err_probe(dev: dt->dev, err: PTR_ERR(ptr: dt->ptp_clock), |
302 | fmt: "Unable to register PTP clock\n" ); |
303 | |
304 | return 0; |
305 | } |
306 | |
307 | static void dfl_tod_remove(struct dfl_device *ddev) |
308 | { |
309 | struct dfl_tod *dt = dev_get_drvdata(dev: &ddev->dev); |
310 | |
311 | ptp_clock_unregister(ptp: dt->ptp_clock); |
312 | } |
313 | |
314 | static const struct dfl_device_id dfl_tod_ids[] = { |
315 | { FME_ID, FME_FEATURE_ID_TOD }, |
316 | { } |
317 | }; |
318 | MODULE_DEVICE_TABLE(dfl, dfl_tod_ids); |
319 | |
320 | static struct dfl_driver dfl_tod_driver = { |
321 | .drv = { |
322 | .name = "dfl-tod" , |
323 | }, |
324 | .id_table = dfl_tod_ids, |
325 | .probe = dfl_tod_probe, |
326 | .remove = dfl_tod_remove, |
327 | }; |
328 | module_dfl_driver(dfl_tod_driver); |
329 | |
330 | MODULE_DESCRIPTION("FPGA DFL ToD driver" ); |
331 | MODULE_AUTHOR("Intel Corporation" ); |
332 | MODULE_LICENSE("GPL" ); |
333 | |