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
62struct 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 */
80static 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 */
98static 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
126static 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
171static 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
226static 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
251static 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
270static 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
280static 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
307static 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
314static const struct dfl_device_id dfl_tod_ids[] = {
315 { FME_ID, FME_FEATURE_ID_TOD },
316 { }
317};
318MODULE_DEVICE_TABLE(dfl, dfl_tod_ids);
319
320static 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};
328module_dfl_driver(dfl_tod_driver);
329
330MODULE_DESCRIPTION("FPGA DFL ToD driver");
331MODULE_AUTHOR("Intel Corporation");
332MODULE_LICENSE("GPL");
333

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