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"); 333