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(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 */
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(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
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(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
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(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
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(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
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(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
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, 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
307static 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
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