333 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * DFL device driver for Time-of-Day (ToD) private feature4 *5 * Copyright (C) 2023 Intel Corporation6 */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 0x2219 20/* ToD clock register space. */21#define TOD_CLK_FREQ 0x03822 23/*24 * The read sequence of ToD timestamp registers: TOD_NANOSEC, TOD_SECONDSL and25 * TOD_SECONDSH, because there is a hardware snapshot whenever the TOD_NANOSEC26 * 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 the30 * sequence of write TOD registers: TOD_SECONDSH, TOD_SECONDSL and TOD_NANOSEC.31 */32#define TOD_SECONDSH 0x10033#define TOD_SECONDSL 0x10434#define TOD_NANOSEC 0x10835#define TOD_PERIOD 0x11036#define TOD_ADJUST_PERIOD 0x11437#define TOD_ADJUST_COUNT 0x11838#define TOD_DRIFT_ADJUST 0x11c39#define TOD_DRIFT_ADJUST_RATE 0x12040#define PERIOD_FRAC_OFFSET 1641#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 050#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_MAX53#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 1056#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, the76 * adjust_period and adjust_count argument are used to update the TOD_ADJUST_PERIOD77 * and TOD_ADJUST_COUNT register for in hardware. The dt->tod_lock spinlock must be78 * 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 by96 * 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_fns146 * 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 offset189 * adjustment in nanoseconds scale. This depends on the current190 * Period register setting and the maximum and minimum possible191 * 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