xref: /openbmc/linux/drivers/ptp/ptp_dfl_tod.c (revision 615927f1)
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