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