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