drivers/net/dsa/sja1105/sja1105_ptp.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (c) 2019, Vladimir Oltean <[email protected]>
   3  */
   4 #include <linux/spi/spi.h>
   5 #include "sja1105.h"
   6
   7 /* The adjfine API clamps ppb between [-32,768,000, 32,768,000], and
   8  * therefore scaled_ppm between [-2,147,483,648, 2,147,483,647].
   9  * Set the maximum supported ppb to a round value smaller than the maximum.
  10  *
  11  * Percentually speaking, this is a +/- 0.032x adjustment of the
  12  * free-running counter (0.968x to 1.032x).
  13  */
  14 #define SJA1105_MAX_ADJ_PPB             32000000
  15 #define SJA1105_SIZE_PTP_CMD            4
  16
  17 /* PTPSYNCTS has no interrupt or update mechanism, because the intended
  18  * hardware use case is for the timestamp to be collected synchronously,
  19  * immediately after the CAS_MASTER SJA1105 switch has performed a CASSYNC
  20  * one-shot toggle (no return to level) on the PTP_CLK pin. When used as a
  21  * generic extts source, the PTPSYNCTS register needs polling and a comparison
  22  * with the old value. The polling interval is configured as the Nyquist rate
  23  * of a signal with 50% duty cycle and 1Hz frequency, which is sadly all that
  24  * this hardware can do (but may be enough for some setups). Anything of higher
  25  * frequency than 1 Hz will be lost, since there is no timestamp FIFO.
  26  */
  27 #define SJA1105_EXTTS_INTERVAL          (HZ / 6)
  28
  29 /*            This range is actually +/- SJA1105_MAX_ADJ_PPB
  30  *            divided by 1000 (ppb -> ppm) and with a 16-bit
  31  *            "fractional" part (actually fixed point).
  32  *                                    |
  33  *                                    v
  34  * Convert scaled_ppm from the +/- ((10^6) << 16) range
  35  * into the +/- (1 << 31) range.
  36  *
  37  * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
  38  * and defines the scaling factor between scaled_ppm and the actual
  39  * frequency adjustments of the PHC.
  40  *
  41  *   ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
  42  *   simplifies to
  43  *   ptpclkrate = scaled_ppm * 2^9 / 5^6
  44  */
  45 #define SJA1105_CC_MULT_NUM             (1 << 9)
  46 #define SJA1105_CC_MULT_DEM             15625
  47 #define SJA1105_CC_MULT                 0x80000000
  48
  49 enum sja1105_ptp_clk_mode {
  50         PTP_ADD_MODE = 1,
  51         PTP_SET_MODE = 0,
  52 };
  53
  54 #define extts_to_data(t) \
  55                 container_of((t), struct sja1105_ptp_data, extts_timer)
  56 #define ptp_caps_to_data(d) \
  57                 container_of((d), struct sja1105_ptp_data, caps)
  58 #define ptp_data_to_sja1105(d) \
  59                 container_of((d), struct sja1105_private, ptp_data)
  60
  61 int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
  62 {
  63         struct sja1105_private *priv = ds->priv;
  64         struct hwtstamp_config config;
  65
  66         if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
  67                 return -EFAULT;
  68
  69         switch (config.tx_type) {
  70         case HWTSTAMP_TX_OFF:
  71                 priv->hwts_tx_en &= ~BIT(port);
  72                 break;
  73         case HWTSTAMP_TX_ON:
  74                 priv->hwts_tx_en |= BIT(port);
  75                 break;
  76         default:
  77                 return -ERANGE;
  78         }
  79
  80         switch (config.rx_filter) {
  81         case HWTSTAMP_FILTER_NONE:
  82                 priv->hwts_rx_en &= ~BIT(port);
  83                 break;
  84         default:
  85                 priv->hwts_rx_en |= BIT(port);
  86                 break;
  87         }
  88
  89         if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
  90                 return -EFAULT;
  91         return 0;
  92 }
  93
  94 int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr)
  95 {
  96         struct sja1105_private *priv = ds->priv;
  97         struct hwtstamp_config config;
  98
  99         config.flags = 0;
 100         if (priv->hwts_tx_en & BIT(port))
 101                 config.tx_type = HWTSTAMP_TX_ON;
 102         else
 103                 config.tx_type = HWTSTAMP_TX_OFF;
 104         if (priv->hwts_rx_en & BIT(port))
 105                 config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
 106         else
 107                 config.rx_filter = HWTSTAMP_FILTER_NONE;
 108
 109         return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
 110                 -EFAULT : 0;
 111 }
 112
 113 int sja1105_get_ts_info(struct dsa_switch *ds, int port,
 114                         struct kernel_ethtool_ts_info *info)
 115 {
 116         struct sja1105_private *priv = ds->priv;
 117         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 118
 119         /* Called during cleanup */
 120         if (!ptp_data->clock)
 121                 return -ENODEV;
 122
 123         info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
 124                                 SOF_TIMESTAMPING_RX_HARDWARE |
 125                                 SOF_TIMESTAMPING_RAW_HARDWARE;
 126         info->tx_types = (1 << HWTSTAMP_TX_OFF) |
 127                          (1 << HWTSTAMP_TX_ON);
 128         info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
 129                            (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
 130         info->phc_index = ptp_clock_index(ptp_data->clock);
 131         return 0;
 132 }
 133
 134 void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
 135                                enum packing_op op)
 136 {
 137         const int size = SJA1105_SIZE_PTP_CMD;
 138         /* No need to keep this as part of the structure */
 139         u64 valid = 1;
 140
 141         sja1105_packing(buf, &valid,           31, 31, size, op);
 142         sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
 143         sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
 144         sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
 145         sja1105_packing(buf, &cmd->stopptpcp,  27, 27, size, op);
 146         sja1105_packing(buf, &cmd->resptp,      2,  2, size, op);
 147         sja1105_packing(buf, &cmd->corrclk4ts,  1,  1, size, op);
 148         sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
 149 }
 150
 151 void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
 152                                  enum packing_op op)
 153 {
 154         const int size = SJA1105_SIZE_PTP_CMD;
 155         /* No need to keep this as part of the structure */
 156         u64 valid = 1;
 157
 158         sja1105_packing(buf, &valid,           31, 31, size, op);
 159         sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
 160         sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
 161         sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
 162         sja1105_packing(buf, &cmd->stopptpcp,  27, 27, size, op);
 163         sja1105_packing(buf, &cmd->resptp,      3,  3, size, op);
 164         sja1105_packing(buf, &cmd->corrclk4ts,  2,  2, size, op);
 165         sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
 166 }
 167
 168 int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd,
 169                        sja1105_spi_rw_mode_t rw)
 170 {
 171         const struct sja1105_private *priv = ds->priv;
 172         const struct sja1105_regs *regs = priv->info->regs;
 173         u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
 174         int rc;
 175
 176         if (rw == SPI_WRITE)
 177                 priv->info->ptp_cmd_packing(buf, cmd, PACK);
 178
 179         rc = sja1105_xfer_buf(priv, rw, regs->ptp_control, buf,
 180                               SJA1105_SIZE_PTP_CMD);
 181
 182         if (rw == SPI_READ)
 183                 priv->info->ptp_cmd_packing(buf, cmd, UNPACK);
 184
 185         return rc;
 186 }
 187
 188 /* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap
 189  * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35
 190  * seconds).
 191  *
 192  * This receives the RX or TX MAC timestamps, provided by hardware as
 193  * the lower bits of the cycle counter, sampled at the time the timestamp was
 194  * collected.
 195  *
 196  * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is
 197  * read and the high-order bits are filled in.
 198  *
 199  * Must be called within one wraparound period of the partial timestamp since
 200  * it was generated by the MAC.
 201  */
 202 static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now,
 203                                       u64 ts_partial)
 204 {
 205         struct sja1105_private *priv = ds->priv;
 206         u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits);
 207         u64 ts_reconstructed;
 208
 209         ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial;
 210
 211         /* Check lower bits of current cycle counter against the timestamp.
 212          * If the current cycle counter is lower than the partial timestamp,
 213          * then wraparound surely occurred and must be accounted for.
 214          */
 215         if ((now & partial_tstamp_mask) <= ts_partial)
 216                 ts_reconstructed -= (partial_tstamp_mask + 1);
 217
 218         return ts_reconstructed;
 219 }
 220
 221 /* Reads the SPI interface for an egress timestamp generated by the switch
 222  * for frames sent using management routes.
 223  *
 224  * SJA1105 E/T layout of the 4-byte SPI payload:
 225  *
 226  * 31    23    15    7     0
 227  * |     |     |     |     |
 228  * +-----+-----+-----+     ^
 229  *          ^              |
 230  *          |              |
 231  *  24-bit timestamp   Update bit
 232  *
 233  *
 234  * SJA1105 P/Q/R/S layout of the 8-byte SPI payload:
 235  *
 236  * 31    23    15    7     0     63    55    47    39    32
 237  * |     |     |     |     |     |     |     |     |     |
 238  *                         ^     +-----+-----+-----+-----+
 239  *                         |                 ^
 240  *                         |                 |
 241  *                    Update bit    32-bit timestamp
 242  *
 243  * Notice that the update bit is in the same place.
 244  * To have common code for E/T and P/Q/R/S for reading the timestamp,
 245  * we need to juggle with the offset and the bit indices.
 246  */
 247 static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts)
 248 {
 249         struct sja1105_private *priv = ds->priv;
 250         const struct sja1105_regs *regs = priv->info->regs;
 251         int tstamp_bit_start, tstamp_bit_end;
 252         int timeout = 10;
 253         u8 packed_buf[8];
 254         u64 update;
 255         int rc;
 256
 257         do {
 258                 rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port],
 259                                       packed_buf, priv->info->ptpegr_ts_bytes);
 260                 if (rc < 0)
 261                         return rc;
 262
 263                 sja1105_unpack(packed_buf, &update, 0, 0,
 264                                priv->info->ptpegr_ts_bytes);
 265                 if (update)
 266                         break;
 267
 268                 usleep_range(10, 50);
 269         } while (--timeout);
 270
 271         if (!timeout)
 272                 return -ETIMEDOUT;
 273
 274         /* Point the end bit to the second 32-bit word on P/Q/R/S,
 275          * no-op on E/T.
 276          */
 277         tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8;
 278         /* Shift the 24-bit timestamp on E/T to be collected from 31:8.
 279          * No-op on P/Q/R/S.
 280          */
 281         tstamp_bit_end += 32 - priv->info->ptp_ts_bits;
 282         tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1;
 283
 284         *ts = 0;
 285
 286         sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end,
 287                        priv->info->ptpegr_ts_bytes);
 288
 289         return 0;
 290 }
 291
 292 /* Caller must hold ptp_data->lock */
 293 static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks,
 294                                   struct ptp_system_timestamp *ptp_sts)
 295 {
 296         const struct sja1105_regs *regs = priv->info->regs;
 297
 298         return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks,
 299                                 ptp_sts);
 300 }
 301
 302 /* Caller must hold ptp_data->lock */
 303 static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks,
 304                                    struct ptp_system_timestamp *ptp_sts)
 305 {
 306         const struct sja1105_regs *regs = priv->info->regs;
 307
 308         return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks,
 309                                 ptp_sts);
 310 }
 311
 312 static void sja1105_extts_poll(struct sja1105_private *priv)
 313 {
 314         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 315         const struct sja1105_regs *regs = priv->info->regs;
 316         struct ptp_clock_event event;
 317         u64 ptpsyncts = 0;
 318         int rc;
 319
 320         rc = sja1105_xfer_u64(priv, SPI_READ, regs->ptpsyncts, &ptpsyncts,
 321                               NULL);
 322         if (rc < 0)
 323                 dev_err_ratelimited(priv->ds->dev,
 324                                     "Failed to read PTPSYNCTS: %d\n", rc);
 325
 326         if (ptpsyncts && ptp_data->ptpsyncts != ptpsyncts) {
 327                 event.index = 0;
 328                 event.type = PTP_CLOCK_EXTTS;
 329                 event.timestamp = ns_to_ktime(sja1105_ticks_to_ns(ptpsyncts));
 330                 ptp_clock_event(ptp_data->clock, &event);
 331
 332                 ptp_data->ptpsyncts = ptpsyncts;
 333         }
 334 }
 335
 336 static long sja1105_rxtstamp_work(struct ptp_clock_info *ptp)
 337 {
 338         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 339         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 340         struct dsa_switch *ds = priv->ds;
 341         struct sk_buff *skb;
 342
 343         mutex_lock(&ptp_data->lock);
 344
 345         while ((skb = skb_dequeue(&ptp_data->skb_rxtstamp_queue)) != NULL) {
 346                 struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
 347                 u64 ticks, ts;
 348                 int rc;
 349
 350                 rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
 351                 if (rc < 0) {
 352                         dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
 353                         kfree_skb(skb);
 354                         continue;
 355                 }
 356
 357                 *shwt = (struct skb_shared_hwtstamps) {0};
 358
 359                 ts = SJA1105_SKB_CB(skb)->tstamp;
 360                 ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
 361
 362                 shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
 363                 netif_rx(skb);
 364         }
 365
 366         if (ptp_data->extts_enabled)
 367                 sja1105_extts_poll(priv);
 368
 369         mutex_unlock(&ptp_data->lock);
 370
 371         /* Don't restart */
 372         return -1;
 373 }
 374
 375 bool sja1105_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
 376 {
 377         struct sja1105_private *priv = ds->priv;
 378         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 379
 380         if (!(priv->hwts_rx_en & BIT(port)))
 381                 return false;
 382
 383         /* We need to read the full PTP clock to reconstruct the Rx
 384          * timestamp. For that we need a sleepable context.
 385          */
 386         skb_queue_tail(&ptp_data->skb_rxtstamp_queue, skb);
 387         ptp_schedule_worker(ptp_data->clock, 0);
 388         return true;
 389 }
 390
 391 bool sja1110_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
 392 {
 393         struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
 394         u64 ts = SJA1105_SKB_CB(skb)->tstamp;
 395
 396         *shwt = (struct skb_shared_hwtstamps) {0};
 397
 398         shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
 399
 400         /* Don't defer */
 401         return false;
 402 }
 403
 404 /* Called from dsa_skb_defer_rx_timestamp */
 405 bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
 406                            struct sk_buff *skb, unsigned int type)
 407 {
 408         struct sja1105_private *priv = ds->priv;
 409
 410         return priv->info->rxtstamp(ds, port, skb);
 411 }
 412
 413 void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port, u8 ts_id,
 414                                  enum sja1110_meta_tstamp dir, u64 tstamp)
 415 {
 416         struct sja1105_private *priv = ds->priv;
 417         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 418         struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
 419         struct skb_shared_hwtstamps shwt = {0};
 420
 421         /* We don't care about RX timestamps on the CPU port */
 422         if (dir == SJA1110_META_TSTAMP_RX)
 423                 return;
 424
 425         spin_lock(&ptp_data->skb_txtstamp_queue.lock);
 426
 427         skb_queue_walk_safe(&ptp_data->skb_txtstamp_queue, skb, skb_tmp) {
 428                 if (SJA1105_SKB_CB(skb)->ts_id != ts_id)
 429                         continue;
 430
 431                 __skb_unlink(skb, &ptp_data->skb_txtstamp_queue);
 432                 skb_match = skb;
 433
 434                 break;
 435         }
 436
 437         spin_unlock(&ptp_data->skb_txtstamp_queue.lock);
 438
 439         if (WARN_ON(!skb_match))
 440                 return;
 441
 442         shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(tstamp));
 443         skb_complete_tx_timestamp(skb_match, &shwt);
 444 }
 445
 446 /* In addition to cloning the skb which is done by the common
 447  * sja1105_port_txtstamp, we need to generate a timestamp ID and save the
 448  * packet to the TX timestamping queue.
 449  */
 450 void sja1110_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
 451 {
 452         struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone;
 453         struct sja1105_private *priv = ds->priv;
 454         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 455         u8 ts_id;
 456
 457         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 458
 459         spin_lock(&priv->ts_id_lock);
 460
 461         ts_id = priv->ts_id;
 462         /* Deal automatically with 8-bit wraparound */
 463         priv->ts_id++;
 464
 465         SJA1105_SKB_CB(clone)->ts_id = ts_id;
 466
 467         spin_unlock(&priv->ts_id_lock);
 468
 469         skb_queue_tail(&ptp_data->skb_txtstamp_queue, clone);
 470 }
 471
 472 /* Called from dsa_skb_tx_timestamp. This callback is just to clone
 473  * the skb and have it available in SJA1105_SKB_CB in the .port_deferred_xmit
 474  * callback, where we will timestamp it synchronously.
 475  */
 476 void sja1105_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
 477 {
 478         struct sja1105_private *priv = ds->priv;
 479         struct sk_buff *clone;
 480
 481         if (!(priv->hwts_tx_en & BIT(port)))
 482                 return;
 483
 484         clone = skb_clone_sk(skb);
 485         if (!clone)
 486                 return;
 487
 488         SJA1105_SKB_CB(skb)->clone = clone;
 489
 490         if (priv->info->txtstamp)
 491                 priv->info->txtstamp(ds, port, skb);
 492 }
 493
 494 static int sja1105_ptp_reset(struct dsa_switch *ds)
 495 {
 496         struct sja1105_private *priv = ds->priv;
 497         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 498         struct sja1105_ptp_cmd cmd = ptp_data->cmd;
 499         int rc;
 500
 501         mutex_lock(&ptp_data->lock);
 502
 503         cmd.resptp = 1;
 504
 505         dev_dbg(ds->dev, "Resetting PTP clock\n");
 506         rc = sja1105_ptp_commit(ds, &cmd, SPI_WRITE);
 507
 508         sja1105_tas_clockstep(priv->ds);
 509
 510         mutex_unlock(&ptp_data->lock);
 511
 512         return rc;
 513 }
 514
 515 /* Caller must hold ptp_data->lock */
 516 int __sja1105_ptp_gettimex(struct dsa_switch *ds, u64 *ns,
 517                            struct ptp_system_timestamp *ptp_sts)
 518 {
 519         struct sja1105_private *priv = ds->priv;
 520         u64 ticks;
 521         int rc;
 522
 523         rc = sja1105_ptpclkval_read(priv, &ticks, ptp_sts);
 524         if (rc < 0) {
 525                 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
 526                 return rc;
 527         }
 528
 529         *ns = sja1105_ticks_to_ns(ticks);
 530
 531         return 0;
 532 }
 533
 534 static int sja1105_ptp_gettimex(struct ptp_clock_info *ptp,
 535                                 struct timespec64 *ts,
 536                                 struct ptp_system_timestamp *ptp_sts)
 537 {
 538         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 539         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 540         u64 now = 0;
 541         int rc;
 542
 543         mutex_lock(&ptp_data->lock);
 544
 545         rc = __sja1105_ptp_gettimex(priv->ds, &now, ptp_sts);
 546         *ts = ns_to_timespec64(now);
 547
 548         mutex_unlock(&ptp_data->lock);
 549
 550         return rc;
 551 }
 552
 553 /* Caller must hold ptp_data->lock */
 554 static int sja1105_ptp_mode_set(struct sja1105_private *priv,
 555                                 enum sja1105_ptp_clk_mode mode)
 556 {
 557         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 558
 559         if (ptp_data->cmd.ptpclkadd == mode)
 560                 return 0;
 561
 562         ptp_data->cmd.ptpclkadd = mode;
 563
 564         return sja1105_ptp_commit(priv->ds, &ptp_data->cmd, SPI_WRITE);
 565 }
 566
 567 /* Write to PTPCLKVAL while PTPCLKADD is 0 */
 568 int __sja1105_ptp_settime(struct dsa_switch *ds, u64 ns,
 569                           struct ptp_system_timestamp *ptp_sts)
 570 {
 571         struct sja1105_private *priv = ds->priv;
 572         u64 ticks = ns_to_sja1105_ticks(ns);
 573         int rc;
 574
 575         rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE);
 576         if (rc < 0) {
 577                 dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n");
 578                 return rc;
 579         }
 580
 581         rc = sja1105_ptpclkval_write(priv, ticks, ptp_sts);
 582
 583         sja1105_tas_clockstep(priv->ds);
 584
 585         return rc;
 586 }
 587
 588 static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
 589                                const struct timespec64 *ts)
 590 {
 591         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 592         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 593         u64 ns = timespec64_to_ns(ts);
 594         int rc;
 595
 596         mutex_lock(&ptp_data->lock);
 597
 598         rc = __sja1105_ptp_settime(priv->ds, ns, NULL);
 599
 600         mutex_unlock(&ptp_data->lock);
 601
 602         return rc;
 603 }
 604
 605 static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 606 {
 607         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 608         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 609         const struct sja1105_regs *regs = priv->info->regs;
 610         u32 clkrate32;
 611         s64 clkrate;
 612         int rc;
 613
 614         clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
 615         clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);
 616
 617         /* Take a +/- value and re-center it around 2^31. */
 618         clkrate = SJA1105_CC_MULT + clkrate;
 619         WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0));
 620         clkrate32 = clkrate;
 621
 622         mutex_lock(&ptp_data->lock);
 623
 624         rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32,
 625                               NULL);
 626
 627         sja1105_tas_adjfreq(priv->ds);
 628
 629         mutex_unlock(&ptp_data->lock);
 630
 631         return rc;
 632 }
 633
 634 /* Write to PTPCLKVAL while PTPCLKADD is 1 */
 635 int __sja1105_ptp_adjtime(struct dsa_switch *ds, s64 delta)
 636 {
 637         struct sja1105_private *priv = ds->priv;
 638         s64 ticks = ns_to_sja1105_ticks(delta);
 639         int rc;
 640
 641         rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE);
 642         if (rc < 0) {
 643                 dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n");
 644                 return rc;
 645         }
 646
 647         rc = sja1105_ptpclkval_write(priv, ticks, NULL);
 648
 649         sja1105_tas_clockstep(priv->ds);
 650
 651         return rc;
 652 }
 653
 654 static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 655 {
 656         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 657         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 658         int rc;
 659
 660         mutex_lock(&ptp_data->lock);
 661
 662         rc = __sja1105_ptp_adjtime(priv->ds, delta);
 663
 664         mutex_unlock(&ptp_data->lock);
 665
 666         return rc;
 667 }
 668
 669 static void sja1105_ptp_extts_setup_timer(struct sja1105_ptp_data *ptp_data)
 670 {
 671         unsigned long expires = ((jiffies / SJA1105_EXTTS_INTERVAL) + 1) *
 672                                 SJA1105_EXTTS_INTERVAL;
 673
 674         mod_timer(&ptp_data->extts_timer, expires);
 675 }
 676
 677 static void sja1105_ptp_extts_timer(struct timer_list *t)
 678 {
 679         struct sja1105_ptp_data *ptp_data = extts_to_data(t);
 680
 681         ptp_schedule_worker(ptp_data->clock, 0);
 682
 683         sja1105_ptp_extts_setup_timer(ptp_data);
 684 }
 685
 686 static int sja1105_change_ptp_clk_pin_func(struct sja1105_private *priv,
 687                                            enum ptp_pin_function func)
 688 {
 689         struct sja1105_avb_params_entry *avb;
 690         enum ptp_pin_function old_func;
 691
 692         avb = priv->static_config.tables[BLK_IDX_AVB_PARAMS].entries;
 693
 694         if (priv->info->device_id == SJA1105E_DEVICE_ID ||
 695             priv->info->device_id == SJA1105T_DEVICE_ID ||
 696             avb->cas_master)
 697                 old_func = PTP_PF_PEROUT;
 698         else
 699                 old_func = PTP_PF_EXTTS;
 700
 701         if (func == old_func)
 702                 return 0;
 703
 704         avb->cas_master = (func == PTP_PF_PEROUT);
 705
 706         return sja1105_dynamic_config_write(priv, BLK_IDX_AVB_PARAMS, 0, avb,
 707                                             true);
 708 }
 709
 710 /* The PTP_CLK pin may be configured to toggle with a 50% duty cycle and a
 711  * frequency f:
 712  *
 713  *           NSEC_PER_SEC
 714  * f = ----------------------
 715  *     (PTPPINDUR * 8 ns) * 2
 716  */
 717 static int sja1105_per_out_enable(struct sja1105_private *priv,
 718                                   struct ptp_perout_request *perout,
 719                                   bool on)
 720 {
 721         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 722         const struct sja1105_regs *regs = priv->info->regs;
 723         struct sja1105_ptp_cmd cmd = ptp_data->cmd;
 724         int rc;
 725
 726         /* We only support one channel */
 727         if (perout->index != 0)
 728                 return -EOPNOTSUPP;
 729
 730         /* Reject requests with unsupported flags */
 731         if (perout->flags)
 732                 return -EOPNOTSUPP;
 733
 734         mutex_lock(&ptp_data->lock);
 735
 736         rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_PEROUT);
 737         if (rc)
 738                 goto out;
 739
 740         if (on) {
 741                 struct timespec64 pin_duration_ts = {
 742                         .tv_sec = perout->period.sec,
 743                         .tv_nsec = perout->period.nsec,
 744                 };
 745                 struct timespec64 pin_start_ts = {
 746                         .tv_sec = perout->start.sec,
 747                         .tv_nsec = perout->start.nsec,
 748                 };
 749                 u64 pin_duration = timespec64_to_ns(&pin_duration_ts);
 750                 u64 pin_start = timespec64_to_ns(&pin_start_ts);
 751                 u32 pin_duration32;
 752                 u64 now;
 753
 754                 /* ptppindur: 32 bit register which holds the interval between
 755                  * 2 edges on PTP_CLK. So check for truncation which happens
 756                  * at periods larger than around 68.7 seconds.
 757                  */
 758                 pin_duration = ns_to_sja1105_ticks(pin_duration / 2);
 759                 if (pin_duration > U32_MAX) {
 760                         rc = -ERANGE;
 761                         goto out;
 762                 }
 763                 pin_duration32 = pin_duration;
 764
 765                 /* ptppins: 64 bit register which needs to hold a PTP time
 766                  * larger than the current time, otherwise the startptpcp
 767                  * command won't do anything. So advance the current time
 768                  * by a number of periods in a way that won't alter the
 769                  * phase offset.
 770                  */
 771                 rc = __sja1105_ptp_gettimex(priv->ds, &now, NULL);
 772                 if (rc < 0)
 773                         goto out;
 774
 775                 pin_start = future_base_time(pin_start, pin_duration,
 776                                              now + 1ull * NSEC_PER_SEC);
 777                 pin_start = ns_to_sja1105_ticks(pin_start);
 778
 779                 rc = sja1105_xfer_u64(priv, SPI_WRITE, regs->ptppinst,
 780                                       &pin_start, NULL);
 781                 if (rc < 0)
 782                         goto out;
 783
 784                 rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptppindur,
 785                                       &pin_duration32, NULL);
 786                 if (rc < 0)
 787                         goto out;
 788         }
 789
 790         if (on)
 791                 cmd.startptpcp = true;
 792         else
 793                 cmd.stopptpcp = true;
 794
 795         rc = sja1105_ptp_commit(priv->ds, &cmd, SPI_WRITE);
 796
 797 out:
 798         mutex_unlock(&ptp_data->lock);
 799
 800         return rc;
 801 }
 802
 803 static int sja1105_extts_enable(struct sja1105_private *priv,
 804                                 struct ptp_extts_request *extts,
 805                                 bool on)
 806 {
 807         int rc;
 808
 809         /* We only support one channel */
 810         if (extts->index != 0)
 811                 return -EOPNOTSUPP;
 812
 813         /* Reject requests with unsupported flags */
 814         if (extts->flags & ~(PTP_ENABLE_FEATURE |
 815                              PTP_RISING_EDGE |
 816                              PTP_FALLING_EDGE |
 817                              PTP_STRICT_FLAGS))
 818                 return -EOPNOTSUPP;
 819
 820         /* We can only enable time stamping on both edges, sadly. */
 821         if ((extts->flags & PTP_STRICT_FLAGS) &&
 822             (extts->flags & PTP_ENABLE_FEATURE) &&
 823             (extts->flags & PTP_EXTTS_EDGES) != PTP_EXTTS_EDGES)
 824                 return -EOPNOTSUPP;
 825
 826         rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_EXTTS);
 827         if (rc)
 828                 return rc;
 829
 830         priv->ptp_data.extts_enabled = on;
 831
 832         if (on)
 833                 sja1105_ptp_extts_setup_timer(&priv->ptp_data);
 834         else
 835                 del_timer_sync(&priv->ptp_data.extts_timer);
 836
 837         return 0;
 838 }
 839
 840 static int sja1105_ptp_enable(struct ptp_clock_info *ptp,
 841                               struct ptp_clock_request *req, int on)
 842 {
 843         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 844         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 845         int rc = -EOPNOTSUPP;
 846
 847         if (req->type == PTP_CLK_REQ_PEROUT)
 848                 rc = sja1105_per_out_enable(priv, &req->perout, on);
 849         else if (req->type == PTP_CLK_REQ_EXTTS)
 850                 rc = sja1105_extts_enable(priv, &req->extts, on);
 851
 852         return rc;
 853 }
 854
 855 static int sja1105_ptp_verify_pin(struct ptp_clock_info *ptp, unsigned int pin,
 856                                   enum ptp_pin_function func, unsigned int chan)
 857 {
 858         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 859         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 860
 861         if (chan != 0 || pin != 0)
 862                 return -1;
 863
 864         switch (func) {
 865         case PTP_PF_NONE:
 866         case PTP_PF_PEROUT:
 867                 break;
 868         case PTP_PF_EXTTS:
 869                 if (priv->info->device_id == SJA1105E_DEVICE_ID ||
 870                     priv->info->device_id == SJA1105T_DEVICE_ID)
 871                         return -1;
 872                 break;
 873         default:
 874                 return -1;
 875         }
 876         return 0;
 877 }
 878
 879 static struct ptp_pin_desc sja1105_ptp_pin = {
 880         .name = "ptp_clk",
 881         .index = 0,
 882         .func = PTP_PF_NONE,
 883 };
 884
 885 int sja1105_ptp_clock_register(struct dsa_switch *ds)
 886 {
 887         struct sja1105_private *priv = ds->priv;
 888         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 889
 890         ptp_data->caps = (struct ptp_clock_info) {
 891                 .owner          = THIS_MODULE,
 892                 .name           = "SJA1105 PHC",
 893                 .adjfine        = sja1105_ptp_adjfine,
 894                 .adjtime        = sja1105_ptp_adjtime,
 895                 .gettimex64     = sja1105_ptp_gettimex,
 896                 .settime64      = sja1105_ptp_settime,
 897                 .enable         = sja1105_ptp_enable,
 898                 .verify         = sja1105_ptp_verify_pin,
 899                 .do_aux_work    = sja1105_rxtstamp_work,
 900                 .max_adj        = SJA1105_MAX_ADJ_PPB,
 901                 .pin_config     = &sja1105_ptp_pin,
 902                 .n_pins         = 1,
 903                 .n_ext_ts       = 1,
 904                 .n_per_out      = 1,
 905         };
 906
 907         /* Only used on SJA1105 */
 908         skb_queue_head_init(&ptp_data->skb_rxtstamp_queue);
 909         /* Only used on SJA1110 */
 910         skb_queue_head_init(&ptp_data->skb_txtstamp_queue);
 911
 912         ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev);
 913         if (IS_ERR_OR_NULL(ptp_data->clock))
 914                 return PTR_ERR(ptp_data->clock);
 915
 916         ptp_data->cmd.corrclk4ts = true;
 917         ptp_data->cmd.ptpclkadd = PTP_SET_MODE;
 918
 919         timer_setup(&ptp_data->extts_timer, sja1105_ptp_extts_timer, 0);
 920
 921         return sja1105_ptp_reset(ds);
 922 }
 923
 924 void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
 925 {
 926         struct sja1105_private *priv = ds->priv;
 927         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 928
 929         if (IS_ERR_OR_NULL(ptp_data->clock))
 930                 return;
 931
 932         del_timer_sync(&ptp_data->extts_timer);
 933         ptp_cancel_worker_sync(ptp_data->clock);
 934         skb_queue_purge(&ptp_data->skb_txtstamp_queue);
 935         skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
 936         ptp_clock_unregister(ptp_data->clock);
 937         ptp_data->clock = NULL;
 938 }
 939
 940 void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int port,
 941                               struct sk_buff *skb)
 942 {
 943         struct sja1105_private *priv = ds->priv;
 944         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 945         struct skb_shared_hwtstamps shwt = {0};
 946         u64 ticks, ts;
 947         int rc;
 948
 949         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 950
 951         mutex_lock(&ptp_data->lock);
 952
 953         rc = sja1105_ptpegr_ts_poll(ds, port, &ts);
 954         if (rc < 0) {
 955                 dev_err(ds->dev, "timed out polling for tstamp\n");
 956                 kfree_skb(skb);
 957                 goto out;
 958         }
 959
 960         rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
 961         if (rc < 0) {
 962                 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
 963                 kfree_skb(skb);
 964                 goto out;
 965         }
 966
 967         ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
 968
 969         shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
 970         skb_complete_tx_timestamp(skb, &shwt);
 971
 972 out:
 973         mutex_unlock(&ptp_data->lock);
 974 }