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