| // SPDX-License-Identifier: GPL-2.0-only | |
| /* | |
| * DFL device driver for Time-of-Day (ToD) private feature | |
| * | |
| * Copyright (C) 2023 Intel Corporation | |
| */ | |
| #include <linux/bitfield.h> | |
| #include <linux/delay.h> | |
| #include <linux/dfl.h> | |
| #include <linux/gcd.h> | |
| #include <linux/iopoll.h> | |
| #include <linux/module.h> | |
| #include <linux/ptp_clock_kernel.h> | |
| #include <linux/spinlock.h> | |
| #include <linux/units.h> | |
| #define FME_FEATURE_ID_TOD 0x22 | |
| /* ToD clock register space. */ | |
| #define TOD_CLK_FREQ 0x038 | |
| /* | |
| * The read sequence of ToD timestamp registers: TOD_NANOSEC, TOD_SECONDSL and | |
| * TOD_SECONDSH, because there is a hardware snapshot whenever the TOD_NANOSEC | |
| * register is read. | |
| * | |
| * The ToD IP requires writing registers in the reverse order to the read sequence. | |
| * The timestamp is corrected when the TOD_NANOSEC register is written, so the | |
| * sequence of write TOD registers: TOD_SECONDSH, TOD_SECONDSL and TOD_NANOSEC. | |
| */ | |
| #define TOD_SECONDSH 0x100 | |
| #define TOD_SECONDSL 0x104 | |
| #define TOD_NANOSEC 0x108 | |
| #define TOD_PERIOD 0x110 | |
| #define TOD_ADJUST_PERIOD 0x114 | |
| #define TOD_ADJUST_COUNT 0x118 | |
| #define TOD_DRIFT_ADJUST 0x11c | |
| #define TOD_DRIFT_ADJUST_RATE 0x120 | |
| #define PERIOD_FRAC_OFFSET 16 | |
| #define SECONDS_MSB GENMASK_ULL(47, 32) | |
| #define SECONDS_LSB GENMASK_ULL(31, 0) | |
| #define TOD_SECONDSH_SEC_MSB GENMASK_ULL(15, 0) | |
| #define CAL_SECONDS(m, l) ((FIELD_GET(TOD_SECONDSH_SEC_MSB, (m)) << 32) | (l)) | |
| #define TOD_PERIOD_MASK GENMASK_ULL(19, 0) | |
| #define TOD_PERIOD_MAX FIELD_MAX(TOD_PERIOD_MASK) | |
| #define TOD_PERIOD_MIN 0 | |
| #define TOD_DRIFT_ADJUST_MASK GENMASK_ULL(15, 0) | |
| #define TOD_DRIFT_ADJUST_FNS_MAX FIELD_MAX(TOD_DRIFT_ADJUST_MASK) | |
| #define TOD_DRIFT_ADJUST_RATE_MAX TOD_DRIFT_ADJUST_FNS_MAX | |
| #define TOD_ADJUST_COUNT_MASK GENMASK_ULL(19, 0) | |
| #define TOD_ADJUST_COUNT_MAX FIELD_MAX(TOD_ADJUST_COUNT_MASK) | |
| #define TOD_ADJUST_INTERVAL_US 10 | |
| #define TOD_ADJUST_MS \ | |
| (((TOD_PERIOD_MAX >> 16) + 1) * (TOD_ADJUST_COUNT_MAX + 1)) | |
| #define TOD_ADJUST_MS_MAX (TOD_ADJUST_MS / MICRO) | |
| #define TOD_ADJUST_MAX_US (TOD_ADJUST_MS_MAX * USEC_PER_MSEC) | |
| #define TOD_MAX_ADJ (500 * MEGA) | |
| struct dfl_tod { | |
| struct ptp_clock_info ptp_clock_ops; | |
| struct device *dev; | |
| struct ptp_clock *ptp_clock; | |
| /* ToD Clock address space */ | |
| void __iomem *tod_ctrl; | |
| /* ToD clock registers protection */ | |
| spinlock_t tod_lock; | |
| }; | |
| /* | |
| * A fine ToD HW clock offset adjustment. To perform the fine offset adjustment, the | |
| * adjust_period and adjust_count argument are used to update the TOD_ADJUST_PERIOD | |
| * and TOD_ADJUST_COUNT register for in hardware. The dt->tod_lock spinlock must be | |
| * held when calling this function. | |
| */ | |
| static int fine_adjust_tod_clock(struct dfl_tod *dt, u32 adjust_period, | |
| u32 adjust_count) | |
| { | |
| void __iomem *base = dt->tod_ctrl; | |
| u32 val; | |
| writel(adjust_period, base + TOD_ADJUST_PERIOD); | |
| writel(adjust_count, base + TOD_ADJUST_COUNT); | |
| /* Wait for present offset adjustment update to complete */ | |
| return readl_poll_timeout_atomic(base + TOD_ADJUST_COUNT, val, !val, TOD_ADJUST_INTERVAL_US, | |
| TOD_ADJUST_MAX_US); | |
| } | |
| /* | |
| * A coarse ToD HW clock offset adjustment. The coarse time adjustment performs by | |
| * adding or subtracting the delta value from the current ToD HW clock time. | |
| */ | |
| static int coarse_adjust_tod_clock(struct dfl_tod *dt, s64 delta) | |
| { | |
| u32 seconds_msb, seconds_lsb, nanosec; | |
| void __iomem *base = dt->tod_ctrl; | |
| u64 seconds, now; | |
| if (delta == 0) | |
| return 0; | |
| nanosec = readl(base + TOD_NANOSEC); | |
| seconds_lsb = readl(base + TOD_SECONDSL); | |
| seconds_msb = readl(base + TOD_SECONDSH); | |
| /* Calculate new time */ | |
| seconds = CAL_SECONDS(seconds_msb, seconds_lsb); | |
| now = seconds * NSEC_PER_SEC + nanosec + delta; | |
| seconds = div_u64_rem(now, NSEC_PER_SEC, &nanosec); | |
| seconds_msb = FIELD_GET(SECONDS_MSB, seconds); | |
| seconds_lsb = FIELD_GET(SECONDS_LSB, seconds); | |
| writel(seconds_msb, base + TOD_SECONDSH); | |
| writel(seconds_lsb, base + TOD_SECONDSL); | |
| writel(nanosec, base + TOD_NANOSEC); | |
| return 0; | |
| } | |
| static int dfl_tod_adjust_fine(struct ptp_clock_info *ptp, long scaled_ppm) | |
| { | |
| struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops); | |
| u32 tod_period, tod_rem, tod_drift_adjust_fns, tod_drift_adjust_rate; | |
| void __iomem *base = dt->tod_ctrl; | |
| unsigned long flags, rate; | |
| u64 ppb; | |
| /* Get the clock rate from clock frequency register offset */ | |
| rate = readl(base + TOD_CLK_FREQ); | |
| /* add GIGA as nominal ppb */ | |
| ppb = scaled_ppm_to_ppb(scaled_ppm) + GIGA; | |
| tod_period = div_u64_rem(ppb << PERIOD_FRAC_OFFSET, rate, &tod_rem); | |
| if (tod_period > TOD_PERIOD_MAX) | |
| return -ERANGE; | |
| /* | |
| * The drift of ToD adjusted periodically by adding a drift_adjust_fns | |
| * correction value every drift_adjust_rate count of clock cycles. | |
| */ | |
| tod_drift_adjust_fns = tod_rem / gcd(tod_rem, rate); | |
| tod_drift_adjust_rate = rate / gcd(tod_rem, rate); | |
| while ((tod_drift_adjust_fns > TOD_DRIFT_ADJUST_FNS_MAX) || | |
| (tod_drift_adjust_rate > TOD_DRIFT_ADJUST_RATE_MAX)) { | |
| tod_drift_adjust_fns >>= 1; | |
| tod_drift_adjust_rate >>= 1; | |
| } | |
| if (tod_drift_adjust_fns == 0) | |
| tod_drift_adjust_rate = 0; | |
| spin_lock_irqsave(&dt->tod_lock, flags); | |
| writel(tod_period, base + TOD_PERIOD); | |
| writel(0, base + TOD_ADJUST_PERIOD); | |
| writel(0, base + TOD_ADJUST_COUNT); | |
| writel(tod_drift_adjust_fns, base + TOD_DRIFT_ADJUST); | |
| writel(tod_drift_adjust_rate, base + TOD_DRIFT_ADJUST_RATE); | |
| spin_unlock_irqrestore(&dt->tod_lock, flags); | |
| return 0; | |
| } | |
| static int dfl_tod_adjust_time(struct ptp_clock_info *ptp, s64 delta) | |
| { | |
| struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops); | |
| u32 period, diff, rem, rem_period, adj_period; | |
| void __iomem *base = dt->tod_ctrl; | |
| unsigned long flags; | |
| bool neg_adj; | |
| u64 count; | |
| int ret; | |
| neg_adj = delta < 0; | |
| if (neg_adj) | |
| delta = -delta; | |
| spin_lock_irqsave(&dt->tod_lock, flags); | |
| /* | |
| * Get the maximum possible value of the Period register offset | |
| * adjustment in nanoseconds scale. This depends on the current | |
| * Period register setting and the maximum and minimum possible | |
| * values of the Period register. | |
| */ | |
| period = readl(base + TOD_PERIOD); | |
| if (neg_adj) { | |
| diff = (period - TOD_PERIOD_MIN) >> PERIOD_FRAC_OFFSET; | |
| adj_period = period - (diff << PERIOD_FRAC_OFFSET); | |
| count = div_u64_rem(delta, diff, &rem); | |
| rem_period = period - (rem << PERIOD_FRAC_OFFSET); | |
| } else { | |
| diff = (TOD_PERIOD_MAX - period) >> PERIOD_FRAC_OFFSET; | |
| adj_period = period + (diff << PERIOD_FRAC_OFFSET); | |
| count = div_u64_rem(delta, diff, &rem); | |
| rem_period = period + (rem << PERIOD_FRAC_OFFSET); | |
| } | |
| ret = 0; | |
| if (count > TOD_ADJUST_COUNT_MAX) { | |
| ret = coarse_adjust_tod_clock(dt, delta); | |
| } else { | |
| /* Adjust the period by count cycles to adjust the time */ | |
| if (count) | |
| ret = fine_adjust_tod_clock(dt, adj_period, count); | |
| /* If there is a remainder, adjust the period for an additional cycle */ | |
| if (rem) | |
| ret = fine_adjust_tod_clock(dt, rem_period, 1); | |
| } | |
| spin_unlock_irqrestore(&dt->tod_lock, flags); | |
| return ret; | |
| } | |
| static int dfl_tod_get_timex(struct ptp_clock_info *ptp, struct timespec64 *ts, | |
| struct ptp_system_timestamp *sts) | |
| { | |
| struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops); | |
| u32 seconds_msb, seconds_lsb, nanosec; | |
| void __iomem *base = dt->tod_ctrl; | |
| unsigned long flags; | |
| u64 seconds; | |
| spin_lock_irqsave(&dt->tod_lock, flags); | |
| ptp_read_system_prets(sts); | |
| nanosec = readl(base + TOD_NANOSEC); | |
| seconds_lsb = readl(base + TOD_SECONDSL); | |
| seconds_msb = readl(base + TOD_SECONDSH); | |
| ptp_read_system_postts(sts); | |
| spin_unlock_irqrestore(&dt->tod_lock, flags); | |
| seconds = CAL_SECONDS(seconds_msb, seconds_lsb); | |
| ts->tv_nsec = nanosec; | |
| ts->tv_sec = seconds; | |
| return 0; | |
| } | |
| static int dfl_tod_set_time(struct ptp_clock_info *ptp, | |
| const struct timespec64 *ts) | |
| { | |
| struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops); | |
| u32 seconds_msb = FIELD_GET(SECONDS_MSB, ts->tv_sec); | |
| u32 seconds_lsb = FIELD_GET(SECONDS_LSB, ts->tv_sec); | |
| u32 nanosec = FIELD_GET(SECONDS_LSB, ts->tv_nsec); | |
| void __iomem *base = dt->tod_ctrl; | |
| unsigned long flags; | |
| spin_lock_irqsave(&dt->tod_lock, flags); | |
| writel(seconds_msb, base + TOD_SECONDSH); | |
| writel(seconds_lsb, base + TOD_SECONDSL); | |
| writel(nanosec, base + TOD_NANOSEC); | |
| spin_unlock_irqrestore(&dt->tod_lock, flags); | |
| return 0; | |
| } | |
| static struct ptp_clock_info dfl_tod_clock_ops = { | |
| .owner = THIS_MODULE, | |
| .name = "dfl_tod", | |
| .max_adj = TOD_MAX_ADJ, | |
| .adjfine = dfl_tod_adjust_fine, | |
| .adjtime = dfl_tod_adjust_time, | |
| .gettimex64 = dfl_tod_get_timex, | |
| .settime64 = dfl_tod_set_time, | |
| }; | |
| static int dfl_tod_probe(struct dfl_device *ddev) | |
| { | |
| struct device *dev = &ddev->dev; | |
| struct dfl_tod *dt; | |
| dt = devm_kzalloc(dev, sizeof(*dt), GFP_KERNEL); | |
| if (!dt) | |
| return -ENOMEM; | |
| dt->tod_ctrl = devm_ioremap_resource(dev, &ddev->mmio_res); | |
| if (IS_ERR(dt->tod_ctrl)) | |
| return PTR_ERR(dt->tod_ctrl); | |
| dt->dev = dev; | |
| spin_lock_init(&dt->tod_lock); | |
| dev_set_drvdata(dev, dt); | |
| dt->ptp_clock_ops = dfl_tod_clock_ops; | |
| dt->ptp_clock = ptp_clock_register(&dt->ptp_clock_ops, dev); | |
| if (IS_ERR(dt->ptp_clock)) | |
| return dev_err_probe(dt->dev, PTR_ERR(dt->ptp_clock), | |
| "Unable to register PTP clock\n"); | |
| return 0; | |
| } | |
| static void dfl_tod_remove(struct dfl_device *ddev) | |
| { | |
| struct dfl_tod *dt = dev_get_drvdata(&ddev->dev); | |
| ptp_clock_unregister(dt->ptp_clock); | |
| } | |
| static const struct dfl_device_id dfl_tod_ids[] = { | |
| { FME_ID, FME_FEATURE_ID_TOD }, | |
| { } | |
| }; | |
| MODULE_DEVICE_TABLE(dfl, dfl_tod_ids); | |
| static struct dfl_driver dfl_tod_driver = { | |
| .drv = { | |
| .name = "dfl-tod", | |
| }, | |
| .id_table = dfl_tod_ids, | |
| .probe = dfl_tod_probe, | |
| .remove = dfl_tod_remove, | |
| }; | |
| module_dfl_driver(dfl_tod_driver); | |
| MODULE_DESCRIPTION("FPGA DFL ToD driver"); | |
| MODULE_AUTHOR("Intel Corporation"); | |
| MODULE_LICENSE("GPL"); |