drivers/ptp/ptp_dfl_tod.c

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