drivers/net/dsa/sja1105/sja1105_ptp.c

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