root/drivers/net/dsa/sja1105/sja1105_ptp.c

DEFINITIONS

1. sja1105_get_ts_info
2. sja1105et_ptp_cmd
3. sja1105pqrs_ptp_cmd
4. sja1105_tstamp_reconstruct
5. sja1105_ptpegr_ts_poll
6. sja1105_ptp_reset
7. sja1105_ptp_gettime
8. sja1105_ptp_settime
9. sja1105_ptp_adjfine
10. sja1105_ptp_adjtime
11. sja1105_ptptsclk_read
12. sja1105_ptp_overflow_check
13. sja1105_ptp_clock_register
14. sja1105_ptp_clock_unregister

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
   3  */
   4 #include "sja1105.h"
   5 
   6 /* The adjfine API clamps ppb between [-32,768,000, 32,768,000], and
   7  * therefore scaled_ppm between [-2,147,483,648, 2,147,483,647].
   8  * Set the maximum supported ppb to a round value smaller than the maximum.
   9  *
  10  * Percentually speaking, this is a +/- 0.032x adjustment of the
  11  * free-running counter (0.968x to 1.032x).
  12  */
  13 #define SJA1105_MAX_ADJ_PPB             32000000
  14 #define SJA1105_SIZE_PTP_CMD            4
  15 
  16 /* Timestamps are in units of 8 ns clock ticks (equivalent to a fixed
  17  * 125 MHz clock) so the scale factor (MULT / SHIFT) needs to be 8.
  18  * Furthermore, wisely pick SHIFT as 28 bits, which translates
  19  * MULT into 2^31 (0x80000000).  This is the same value around which
  20  * the hardware PTPCLKRATE is centered, so the same ppb conversion
  21  * arithmetic can be reused.
  22  */
  23 #define SJA1105_CC_SHIFT                28
  24 #define SJA1105_CC_MULT                 (8 << SJA1105_CC_SHIFT)
  25 
  26 /* Having 33 bits of cycle counter left until a 64-bit overflow during delta
  27  * conversion, we multiply this by the 8 ns counter resolution and arrive at
  28  * a comfortable 68.71 second refresh interval until the delta would cause
  29  * an integer overflow, in absence of any other readout.
  30  * Approximate to 1 minute.
  31  */
  32 #define SJA1105_REFRESH_INTERVAL        (HZ * 60)
  33 
  34 /*            This range is actually +/- SJA1105_MAX_ADJ_PPB
  35  *            divided by 1000 (ppb -> ppm) and with a 16-bit
  36  *            "fractional" part (actually fixed point).
  37  *                                    |
  38  *                                    v
  39  * Convert scaled_ppm from the +/- ((10^6) << 16) range
  40  * into the +/- (1 << 31) range.
  41  *
  42  * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
  43  * and defines the scaling factor between scaled_ppm and the actual
  44  * frequency adjustments (both cycle counter and hardware).
  45  *
  46  *   ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
  47  *   simplifies to
  48  *   ptpclkrate = scaled_ppm * 2^9 / 5^6
  49  */
  50 #define SJA1105_CC_MULT_NUM             (1 << 9)
  51 #define SJA1105_CC_MULT_DEM             15625
  52 
  53 #define ptp_to_sja1105(d) container_of((d), struct sja1105_private, ptp_caps)
  54 #define cc_to_sja1105(d) container_of((d), struct sja1105_private, tstamp_cc)
  55 #define dw_to_sja1105(d) container_of((d), struct sja1105_private, refresh_work)
  56 
  57 struct sja1105_ptp_cmd {
  58         u64 resptp;       /* reset */
  59 };
  60 
  61 int sja1105_get_ts_info(struct dsa_switch *ds, int port,
  62                         struct ethtool_ts_info *info)
  63 {
  64         struct sja1105_private *priv = ds->priv;
  65 
  66         /* Called during cleanup */
  67         if (!priv->clock)
  68                 return -ENODEV;
  69 
  70         info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
  71                                 SOF_TIMESTAMPING_RX_HARDWARE |
  72                                 SOF_TIMESTAMPING_RAW_HARDWARE;
  73         info->tx_types = (1 << HWTSTAMP_TX_OFF) |
  74                          (1 << HWTSTAMP_TX_ON);
  75         info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
  76                            (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
  77         info->phc_index = ptp_clock_index(priv->clock);
  78         return 0;
  79 }
  80 
  81 int sja1105et_ptp_cmd(const void *ctx, const void *data)
  82 {
  83         const struct sja1105_ptp_cmd *cmd = data;
  84         const struct sja1105_private *priv = ctx;
  85         const struct sja1105_regs *regs = priv->info->regs;
  86         const int size = SJA1105_SIZE_PTP_CMD;
  87         u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
  88         /* No need to keep this as part of the structure */
  89         u64 valid = 1;
  90 
  91         sja1105_pack(buf, &valid,           31, 31, size);
  92         sja1105_pack(buf, &cmd->resptp,      2,  2, size);
  93 
  94         return sja1105_spi_send_packed_buf(priv, SPI_WRITE, regs->ptp_control,
  95                                            buf, SJA1105_SIZE_PTP_CMD);
  96 }
  97 
  98 int sja1105pqrs_ptp_cmd(const void *ctx, const void *data)
  99 {
 100         const struct sja1105_ptp_cmd *cmd = data;
 101         const struct sja1105_private *priv = ctx;
 102         const struct sja1105_regs *regs = priv->info->regs;
 103         const int size = SJA1105_SIZE_PTP_CMD;
 104         u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
 105         /* No need to keep this as part of the structure */
 106         u64 valid = 1;
 107 
 108         sja1105_pack(buf, &valid,           31, 31, size);
 109         sja1105_pack(buf, &cmd->resptp,      3,  3, size);
 110 
 111         return sja1105_spi_send_packed_buf(priv, SPI_WRITE, regs->ptp_control,
 112                                            buf, SJA1105_SIZE_PTP_CMD);
 113 }
 114 
 115 /* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap
 116  * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35
 117  * seconds).
 118  *
 119  * This receives the RX or TX MAC timestamps, provided by hardware as
 120  * the lower bits of the cycle counter, sampled at the time the timestamp was
 121  * collected.
 122  *
 123  * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is
 124  * read and the high-order bits are filled in.
 125  *
 126  * Must be called within one wraparound period of the partial timestamp since
 127  * it was generated by the MAC.
 128  */
 129 u64 sja1105_tstamp_reconstruct(struct sja1105_private *priv, u64 now,
 130                                u64 ts_partial)
 131 {
 132         u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits);
 133         u64 ts_reconstructed;
 134 
 135         ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial;
 136 
 137         /* Check lower bits of current cycle counter against the timestamp.
 138          * If the current cycle counter is lower than the partial timestamp,
 139          * then wraparound surely occurred and must be accounted for.
 140          */
 141         if ((now & partial_tstamp_mask) <= ts_partial)
 142                 ts_reconstructed -= (partial_tstamp_mask + 1);
 143 
 144         return ts_reconstructed;
 145 }
 146 
 147 /* Reads the SPI interface for an egress timestamp generated by the switch
 148  * for frames sent using management routes.
 149  *
 150  * SJA1105 E/T layout of the 4-byte SPI payload:
 151  *
 152  * 31    23    15    7     0
 153  * |     |     |     |     |
 154  * +-----+-----+-----+     ^
 155  *          ^              |
 156  *          |              |
 157  *  24-bit timestamp   Update bit
 158  *
 159  *
 160  * SJA1105 P/Q/R/S layout of the 8-byte SPI payload:
 161  *
 162  * 31    23    15    7     0     63    55    47    39    32
 163  * |     |     |     |     |     |     |     |     |     |
 164  *                         ^     +-----+-----+-----+-----+
 165  *                         |                 ^
 166  *                         |                 |
 167  *                    Update bit    32-bit timestamp
 168  *
 169  * Notice that the update bit is in the same place.
 170  * To have common code for E/T and P/Q/R/S for reading the timestamp,
 171  * we need to juggle with the offset and the bit indices.
 172  */
 173 int sja1105_ptpegr_ts_poll(struct sja1105_private *priv, int port, u64 *ts)
 174 {
 175         const struct sja1105_regs *regs = priv->info->regs;
 176         int tstamp_bit_start, tstamp_bit_end;
 177         int timeout = 10;
 178         u8 packed_buf[8];
 179         u64 update;
 180         int rc;
 181 
 182         do {
 183                 rc = sja1105_spi_send_packed_buf(priv, SPI_READ,
 184                                                  regs->ptpegr_ts[port],
 185                                                  packed_buf,
 186                                                  priv->info->ptpegr_ts_bytes);
 187                 if (rc < 0)
 188                         return rc;
 189 
 190                 sja1105_unpack(packed_buf, &update, 0, 0,
 191                                priv->info->ptpegr_ts_bytes);
 192                 if (update)
 193                         break;
 194 
 195                 usleep_range(10, 50);
 196         } while (--timeout);
 197 
 198         if (!timeout)
 199                 return -ETIMEDOUT;
 200 
 201         /* Point the end bit to the second 32-bit word on P/Q/R/S,
 202          * no-op on E/T.
 203          */
 204         tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8;
 205         /* Shift the 24-bit timestamp on E/T to be collected from 31:8.
 206          * No-op on P/Q/R/S.
 207          */
 208         tstamp_bit_end += 32 - priv->info->ptp_ts_bits;
 209         tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1;
 210 
 211         *ts = 0;
 212 
 213         sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end,
 214                        priv->info->ptpegr_ts_bytes);
 215 
 216         return 0;
 217 }
 218 
 219 int sja1105_ptp_reset(struct sja1105_private *priv)
 220 {
 221         struct dsa_switch *ds = priv->ds;
 222         struct sja1105_ptp_cmd cmd = {0};
 223         int rc;
 224 
 225         mutex_lock(&priv->ptp_lock);
 226 
 227         cmd.resptp = 1;
 228         dev_dbg(ds->dev, "Resetting PTP clock\n");
 229         rc = priv->info->ptp_cmd(priv, &cmd);
 230 
 231         timecounter_init(&priv->tstamp_tc, &priv->tstamp_cc,
 232                          ktime_to_ns(ktime_get_real()));
 233 
 234         mutex_unlock(&priv->ptp_lock);
 235 
 236         return rc;
 237 }
 238 
 239 static int sja1105_ptp_gettime(struct ptp_clock_info *ptp,
 240                                struct timespec64 *ts)
 241 {
 242         struct sja1105_private *priv = ptp_to_sja1105(ptp);
 243         u64 ns;
 244 
 245         mutex_lock(&priv->ptp_lock);
 246         ns = timecounter_read(&priv->tstamp_tc);
 247         mutex_unlock(&priv->ptp_lock);
 248 
 249         *ts = ns_to_timespec64(ns);
 250 
 251         return 0;
 252 }
 253 
 254 static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
 255                                const struct timespec64 *ts)
 256 {
 257         struct sja1105_private *priv = ptp_to_sja1105(ptp);
 258         u64 ns = timespec64_to_ns(ts);
 259 
 260         mutex_lock(&priv->ptp_lock);
 261         timecounter_init(&priv->tstamp_tc, &priv->tstamp_cc, ns);
 262         mutex_unlock(&priv->ptp_lock);
 263 
 264         return 0;
 265 }
 266 
 267 static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 268 {
 269         struct sja1105_private *priv = ptp_to_sja1105(ptp);
 270         s64 clkrate;
 271 
 272         clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
 273         clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);
 274 
 275         mutex_lock(&priv->ptp_lock);
 276 
 277         /* Force a readout to update the timer *before* changing its frequency.
 278          *
 279          * This way, its corrected time curve can at all times be modeled
 280          * as a linear "A * x + B" function, where:
 281          *
 282          * - B are past frequency adjustments and offset shifts, all
 283          *   accumulated into the cycle_last variable.
 284          *
 285          * - A is the new frequency adjustments we're just about to set.
 286          *
 287          * Reading now makes B accumulate the correct amount of time,
 288          * corrected at the old rate, before changing it.
 289          *
 290          * Hardware timestamps then become simple points on the curve and
 291          * are approximated using the above function.  This is still better
 292          * than letting the switch take the timestamps using the hardware
 293          * rate-corrected clock (PTPCLKVAL) - the comparison in this case would
 294          * be that we're shifting the ruler at the same time as we're taking
 295          * measurements with it.
 296          *
 297          * The disadvantage is that it's possible to receive timestamps when
 298          * a frequency adjustment took place in the near past.
 299          * In this case they will be approximated using the new ppb value
 300          * instead of a compound function made of two segments (one at the old
 301          * and the other at the new rate) - introducing some inaccuracy.
 302          */
 303         timecounter_read(&priv->tstamp_tc);
 304 
 305         priv->tstamp_cc.mult = SJA1105_CC_MULT + clkrate;
 306 
 307         mutex_unlock(&priv->ptp_lock);
 308 
 309         return 0;
 310 }
 311 
 312 static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 313 {
 314         struct sja1105_private *priv = ptp_to_sja1105(ptp);
 315 
 316         mutex_lock(&priv->ptp_lock);
 317         timecounter_adjtime(&priv->tstamp_tc, delta);
 318         mutex_unlock(&priv->ptp_lock);
 319 
 320         return 0;
 321 }
 322 
 323 static u64 sja1105_ptptsclk_read(const struct cyclecounter *cc)
 324 {
 325         struct sja1105_private *priv = cc_to_sja1105(cc);
 326         const struct sja1105_regs *regs = priv->info->regs;
 327         u64 ptptsclk = 0;
 328         int rc;
 329 
 330         rc = sja1105_spi_send_int(priv, SPI_READ, regs->ptptsclk,
 331                                   &ptptsclk, 8);
 332         if (rc < 0)
 333                 dev_err_ratelimited(priv->ds->dev,
 334                                     "failed to read ptp cycle counter: %d\n",
 335                                     rc);
 336         return ptptsclk;
 337 }
 338 
 339 static void sja1105_ptp_overflow_check(struct work_struct *work)
 340 {
 341         struct delayed_work *dw = to_delayed_work(work);
 342         struct sja1105_private *priv = dw_to_sja1105(dw);
 343         struct timespec64 ts;
 344 
 345         sja1105_ptp_gettime(&priv->ptp_caps, &ts);
 346 
 347         schedule_delayed_work(&priv->refresh_work, SJA1105_REFRESH_INTERVAL);
 348 }
 349 
 350 static const struct ptp_clock_info sja1105_ptp_caps = {
 351         .owner          = THIS_MODULE,
 352         .name           = "SJA1105 PHC",
 353         .adjfine        = sja1105_ptp_adjfine,
 354         .adjtime        = sja1105_ptp_adjtime,
 355         .gettime64      = sja1105_ptp_gettime,
 356         .settime64      = sja1105_ptp_settime,
 357         .max_adj        = SJA1105_MAX_ADJ_PPB,
 358 };
 359 
 360 int sja1105_ptp_clock_register(struct sja1105_private *priv)
 361 {
 362         struct dsa_switch *ds = priv->ds;
 363 
 364         /* Set up the cycle counter */
 365         priv->tstamp_cc = (struct cyclecounter) {
 366                 .read = sja1105_ptptsclk_read,
 367                 .mask = CYCLECOUNTER_MASK(64),
 368                 .shift = SJA1105_CC_SHIFT,
 369                 .mult = SJA1105_CC_MULT,
 370         };
 371         mutex_init(&priv->ptp_lock);
 372         priv->ptp_caps = sja1105_ptp_caps;
 373 
 374         priv->clock = ptp_clock_register(&priv->ptp_caps, ds->dev);
 375         if (IS_ERR_OR_NULL(priv->clock))
 376                 return PTR_ERR(priv->clock);
 377 
 378         INIT_DELAYED_WORK(&priv->refresh_work, sja1105_ptp_overflow_check);
 379         schedule_delayed_work(&priv->refresh_work, SJA1105_REFRESH_INTERVAL);
 380 
 381         return sja1105_ptp_reset(priv);
 382 }
 383 
 384 void sja1105_ptp_clock_unregister(struct sja1105_private *priv)
 385 {
 386         if (IS_ERR_OR_NULL(priv->clock))
 387                 return;
 388 
 389         cancel_delayed_work_sync(&priv->refresh_work);
 390         ptp_clock_unregister(priv->clock);
 391         priv->clock = NULL;
 392 }