Commit | Line | Data |
---|---|---|
615927f1 TZ |
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(adjust_period, base + TOD_ADJUST_PERIOD); | |
87 | writel(adjust_count, 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(base + TOD_NANOSEC); | |
108 | seconds_lsb = readl(base + TOD_SECONDSL); | |
109 | seconds_msb = readl(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(now, NSEC_PER_SEC, &nanosec); | |
116 | seconds_msb = FIELD_GET(SECONDS_MSB, seconds); | |
117 | seconds_lsb = FIELD_GET(SECONDS_LSB, seconds); | |
118 | ||
119 | writel(seconds_msb, base + TOD_SECONDSH); | |
120 | writel(seconds_lsb, base + TOD_SECONDSL); | |
121 | writel(nanosec, 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(base + TOD_CLK_FREQ); | |
136 | ||
137 | /* add GIGA as nominal ppb */ | |
138 | ppb = scaled_ppm_to_ppb(scaled_ppm) + GIGA; | |
139 | ||
140 | tod_period = div_u64_rem(ppb << PERIOD_FRAC_OFFSET, rate, &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(tod_rem, rate); | |
149 | tod_drift_adjust_rate = rate / gcd(tod_rem, 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(tod_period, base + TOD_PERIOD); | |
162 | writel(0, base + TOD_ADJUST_PERIOD); | |
163 | writel(0, base + TOD_ADJUST_COUNT); | |
164 | writel(tod_drift_adjust_fns, base + TOD_DRIFT_ADJUST); | |
165 | writel(tod_drift_adjust_rate, base + TOD_DRIFT_ADJUST_RATE); | |
166 | spin_unlock_irqrestore(&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(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(delta, diff, &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(delta, diff, &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, adj_period, 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, rem_period, 1); | |
219 | } | |
220 | ||
221 | spin_unlock_irqrestore(&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(base + TOD_NANOSEC); | |
238 | seconds_lsb = readl(base + TOD_SECONDSL); | |
239 | seconds_msb = readl(base + TOD_SECONDSH); | |
240 | ptp_read_system_postts(sts); | |
241 | spin_unlock_irqrestore(&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(seconds_msb, base + TOD_SECONDSH); | |
263 | writel(seconds_lsb, base + TOD_SECONDSL); | |
264 | writel(nanosec, base + TOD_NANOSEC); | |
265 | spin_unlock_irqrestore(&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, sizeof(*dt), GFP_KERNEL); | |
286 | if (!dt) | |
287 | return -ENOMEM; | |
288 | ||
289 | dt->tod_ctrl = devm_ioremap_resource(dev, &ddev->mmio_res); | |
290 | if (IS_ERR(dt->tod_ctrl)) | |
291 | return PTR_ERR(dt->tod_ctrl); | |
292 | ||
293 | dt->dev = dev; | |
294 | spin_lock_init(&dt->tod_lock); | |
295 | dev_set_drvdata(dev, dt); | |
296 | ||
297 | dt->ptp_clock_ops = dfl_tod_clock_ops; | |
298 | ||
299 | dt->ptp_clock = ptp_clock_register(&dt->ptp_clock_ops, dev); | |
300 | if (IS_ERR(dt->ptp_clock)) | |
301 | return dev_err_probe(dt->dev, PTR_ERR(dt->ptp_clock), | |
302 | "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(&ddev->dev); | |
310 | ||
311 | ptp_clock_unregister(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"); |