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 */
fine_adjust_tod_clock(struct dfl_tod * dt,u32 adjust_period,u32 adjust_count)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 */
coarse_adjust_tod_clock(struct dfl_tod * dt,s64 delta)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
dfl_tod_adjust_fine(struct ptp_clock_info * ptp,long scaled_ppm)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
dfl_tod_adjust_time(struct ptp_clock_info * ptp,s64 delta)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
dfl_tod_get_timex(struct ptp_clock_info * ptp,struct timespec64 * ts,struct ptp_system_timestamp * sts)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
dfl_tod_set_time(struct ptp_clock_info * ptp,const struct timespec64 * ts)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
dfl_tod_probe(struct dfl_device * ddev)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
dfl_tod_remove(struct dfl_device * ddev)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");
333
