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 /*            This range is actually +/- SJA1105_MAX_ADJ_PPB
  18  *            divided by 1000 (ppb -> ppm) and with a 16-bit
  19  *            "fractional" part (actually fixed point).
  20  *                                    |
  21  *                                    v
  22  * Convert scaled_ppm from the +/- ((10^6) << 16) range
  23  * into the +/- (1 << 31) range.
  24  *
  25  * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
  26  * and defines the scaling factor between scaled_ppm and the actual
  27  * frequency adjustments of the PHC.
  28  *
  29  *   ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
  30  *   simplifies to
  31  *   ptpclkrate = scaled_ppm * 2^9 / 5^6
  32  */
  33 #define SJA1105_CC_MULT_NUM             (1 << 9)
  34 #define SJA1105_CC_MULT_DEM             15625
  35 #define SJA1105_CC_MULT                 0x80000000
  36
  37 enum sja1105_ptp_clk_mode {
  38         PTP_ADD_MODE = 1,
  39         PTP_SET_MODE = 0,
  40 };
  41
  42 #define ptp_caps_to_data(d) \
  43                 container_of((d), struct sja1105_ptp_data, caps)
  44 #define ptp_data_to_sja1105(d) \
  45                 container_of((d), struct sja1105_private, ptp_data)
  46
  47 static int sja1105_init_avb_params(struct sja1105_private *priv,
  48                                    bool on)
  49 {
  50         struct sja1105_avb_params_entry *avb;
  51         struct sja1105_table *table;
  52
  53         table = &priv->static_config.tables[BLK_IDX_AVB_PARAMS];
  54
  55         /* Discard previous AVB Parameters Table */
  56         if (table->entry_count) {
  57                 kfree(table->entries);
  58                 table->entry_count = 0;
  59         }
  60
  61         /* Configure the reception of meta frames only if requested */
  62         if (!on)
  63                 return 0;
  64
  65         table->entries = kcalloc(SJA1105_MAX_AVB_PARAMS_COUNT,
  66                                  table->ops->unpacked_entry_size, GFP_KERNEL);
  67         if (!table->entries)
  68                 return -ENOMEM;
  69
  70         table->entry_count = SJA1105_MAX_AVB_PARAMS_COUNT;
  71
  72         avb = table->entries;
  73
  74         avb->destmeta = SJA1105_META_DMAC;
  75         avb->srcmeta  = SJA1105_META_SMAC;
  76
  77         return 0;
  78 }
  79
  80 /* Must be called only with priv->tagger_data.state bit
  81  * SJA1105_HWTS_RX_EN cleared
  82  */
  83 static int sja1105_change_rxtstamping(struct sja1105_private *priv,
  84                                       bool on)
  85 {
  86         struct sja1105_general_params_entry *general_params;
  87         struct sja1105_table *table;
  88         int rc;
  89
  90         table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
  91         general_params = table->entries;
  92         general_params->send_meta1 = on;
  93         general_params->send_meta0 = on;
  94
  95         rc = sja1105_init_avb_params(priv, on);
  96         if (rc < 0)
  97                 return rc;
  98
  99         /* Initialize the meta state machine to a known state */
 100         if (priv->tagger_data.stampable_skb) {
 101                 kfree_skb(priv->tagger_data.stampable_skb);
 102                 priv->tagger_data.stampable_skb = NULL;
 103         }
 104
 105         return sja1105_static_config_reload(priv, SJA1105_RX_HWTSTAMPING);
 106 }
 107
 108 int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
 109 {
 110         struct sja1105_private *priv = ds->priv;
 111         struct hwtstamp_config config;
 112         bool rx_on;
 113         int rc;
 114
 115         if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
 116                 return -EFAULT;
 117
 118         switch (config.tx_type) {
 119         case HWTSTAMP_TX_OFF:
 120                 priv->ports[port].hwts_tx_en = false;
 121                 break;
 122         case HWTSTAMP_TX_ON:
 123                 priv->ports[port].hwts_tx_en = true;
 124                 break;
 125         default:
 126                 return -ERANGE;
 127         }
 128
 129         switch (config.rx_filter) {
 130         case HWTSTAMP_FILTER_NONE:
 131                 rx_on = false;
 132                 break;
 133         default:
 134                 rx_on = true;
 135                 break;
 136         }
 137
 138         if (rx_on != test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state)) {
 139                 clear_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state);
 140
 141                 rc = sja1105_change_rxtstamping(priv, rx_on);
 142                 if (rc < 0) {
 143                         dev_err(ds->dev,
 144                                 "Failed to change RX timestamping: %d\n", rc);
 145                         return rc;
 146                 }
 147                 if (rx_on)
 148                         set_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state);
 149         }
 150
 151         if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
 152                 return -EFAULT;
 153         return 0;
 154 }
 155
 156 int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr)
 157 {
 158         struct sja1105_private *priv = ds->priv;
 159         struct hwtstamp_config config;
 160
 161         config.flags = 0;
 162         if (priv->ports[port].hwts_tx_en)
 163                 config.tx_type = HWTSTAMP_TX_ON;
 164         else
 165                 config.tx_type = HWTSTAMP_TX_OFF;
 166         if (test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state))
 167                 config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
 168         else
 169                 config.rx_filter = HWTSTAMP_FILTER_NONE;
 170
 171         return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
 172                 -EFAULT : 0;
 173 }
 174
 175 int sja1105_get_ts_info(struct dsa_switch *ds, int port,
 176                         struct ethtool_ts_info *info)
 177 {
 178         struct sja1105_private *priv = ds->priv;
 179         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 180
 181         /* Called during cleanup */
 182         if (!ptp_data->clock)
 183                 return -ENODEV;
 184
 185         info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
 186                                 SOF_TIMESTAMPING_RX_HARDWARE |
 187                                 SOF_TIMESTAMPING_RAW_HARDWARE;
 188         info->tx_types = (1 << HWTSTAMP_TX_OFF) |
 189                          (1 << HWTSTAMP_TX_ON);
 190         info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
 191                            (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
 192         info->phc_index = ptp_clock_index(ptp_data->clock);
 193         return 0;
 194 }
 195
 196 void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
 197                                enum packing_op op)
 198 {
 199         const int size = SJA1105_SIZE_PTP_CMD;
 200         /* No need to keep this as part of the structure */
 201         u64 valid = 1;
 202
 203         sja1105_packing(buf, &valid,           31, 31, size, op);
 204         sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
 205         sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
 206         sja1105_packing(buf, &cmd->resptp,      2,  2, size, op);
 207         sja1105_packing(buf, &cmd->corrclk4ts,  1,  1, size, op);
 208         sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
 209 }
 210
 211 void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
 212                                  enum packing_op op)
 213 {
 214         const int size = SJA1105_SIZE_PTP_CMD;
 215         /* No need to keep this as part of the structure */
 216         u64 valid = 1;
 217
 218         sja1105_packing(buf, &valid,           31, 31, size, op);
 219         sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
 220         sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
 221         sja1105_packing(buf, &cmd->resptp,      3,  3, size, op);
 222         sja1105_packing(buf, &cmd->corrclk4ts,  2,  2, size, op);
 223         sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
 224 }
 225
 226 int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd,
 227                        sja1105_spi_rw_mode_t rw)
 228 {
 229         const struct sja1105_private *priv = ds->priv;
 230         const struct sja1105_regs *regs = priv->info->regs;
 231         u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
 232         int rc;
 233
 234         if (rw == SPI_WRITE)
 235                 priv->info->ptp_cmd_packing(buf, cmd, PACK);
 236
 237         rc = sja1105_xfer_buf(priv, SPI_WRITE, regs->ptp_control, buf,
 238                               SJA1105_SIZE_PTP_CMD);
 239
 240         if (rw == SPI_READ)
 241                 priv->info->ptp_cmd_packing(buf, cmd, UNPACK);
 242
 243         return rc;
 244 }
 245
 246 /* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap
 247  * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35
 248  * seconds).
 249  *
 250  * This receives the RX or TX MAC timestamps, provided by hardware as
 251  * the lower bits of the cycle counter, sampled at the time the timestamp was
 252  * collected.
 253  *
 254  * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is
 255  * read and the high-order bits are filled in.
 256  *
 257  * Must be called within one wraparound period of the partial timestamp since
 258  * it was generated by the MAC.
 259  */
 260 static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now,
 261                                       u64 ts_partial)
 262 {
 263         struct sja1105_private *priv = ds->priv;
 264         u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits);
 265         u64 ts_reconstructed;
 266
 267         ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial;
 268
 269         /* Check lower bits of current cycle counter against the timestamp.
 270          * If the current cycle counter is lower than the partial timestamp,
 271          * then wraparound surely occurred and must be accounted for.
 272          */
 273         if ((now & partial_tstamp_mask) <= ts_partial)
 274                 ts_reconstructed -= (partial_tstamp_mask + 1);
 275
 276         return ts_reconstructed;
 277 }
 278
 279 /* Reads the SPI interface for an egress timestamp generated by the switch
 280  * for frames sent using management routes.
 281  *
 282  * SJA1105 E/T layout of the 4-byte SPI payload:
 283  *
 284  * 31    23    15    7     0
 285  * |     |     |     |     |
 286  * +-----+-----+-----+     ^
 287  *          ^              |
 288  *          |              |
 289  *  24-bit timestamp   Update bit
 290  *
 291  *
 292  * SJA1105 P/Q/R/S layout of the 8-byte SPI payload:
 293  *
 294  * 31    23    15    7     0     63    55    47    39    32
 295  * |     |     |     |     |     |     |     |     |     |
 296  *                         ^     +-----+-----+-----+-----+
 297  *                         |                 ^
 298  *                         |                 |
 299  *                    Update bit    32-bit timestamp
 300  *
 301  * Notice that the update bit is in the same place.
 302  * To have common code for E/T and P/Q/R/S for reading the timestamp,
 303  * we need to juggle with the offset and the bit indices.
 304  */
 305 static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts)
 306 {
 307         struct sja1105_private *priv = ds->priv;
 308         const struct sja1105_regs *regs = priv->info->regs;
 309         int tstamp_bit_start, tstamp_bit_end;
 310         int timeout = 10;
 311         u8 packed_buf[8];
 312         u64 update;
 313         int rc;
 314
 315         do {
 316                 rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port],
 317                                       packed_buf, priv->info->ptpegr_ts_bytes);
 318                 if (rc < 0)
 319                         return rc;
 320
 321                 sja1105_unpack(packed_buf, &update, 0, 0,
 322                                priv->info->ptpegr_ts_bytes);
 323                 if (update)
 324                         break;
 325
 326                 usleep_range(10, 50);
 327         } while (--timeout);
 328
 329         if (!timeout)
 330                 return -ETIMEDOUT;
 331
 332         /* Point the end bit to the second 32-bit word on P/Q/R/S,
 333          * no-op on E/T.
 334          */
 335         tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8;
 336         /* Shift the 24-bit timestamp on E/T to be collected from 31:8.
 337          * No-op on P/Q/R/S.
 338          */
 339         tstamp_bit_end += 32 - priv->info->ptp_ts_bits;
 340         tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1;
 341
 342         *ts = 0;
 343
 344         sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end,
 345                        priv->info->ptpegr_ts_bytes);
 346
 347         return 0;
 348 }
 349
 350 /* Caller must hold ptp_data->lock */
 351 static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks,
 352                                   struct ptp_system_timestamp *ptp_sts)
 353 {
 354         const struct sja1105_regs *regs = priv->info->regs;
 355
 356         return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks,
 357                                 ptp_sts);
 358 }
 359
 360 /* Caller must hold ptp_data->lock */
 361 static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks,
 362                                    struct ptp_system_timestamp *ptp_sts)
 363 {
 364         const struct sja1105_regs *regs = priv->info->regs;
 365
 366         return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks,
 367                                 ptp_sts);
 368 }
 369
 370 #define rxtstamp_to_tagger(d) \
 371         container_of((d), struct sja1105_tagger_data, rxtstamp_work)
 372 #define tagger_to_sja1105(d) \
 373         container_of((d), struct sja1105_private, tagger_data)
 374
 375 static void sja1105_rxtstamp_work(struct work_struct *work)
 376 {
 377         struct sja1105_tagger_data *tagger_data = rxtstamp_to_tagger(work);
 378         struct sja1105_private *priv = tagger_to_sja1105(tagger_data);
 379         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 380         struct dsa_switch *ds = priv->ds;
 381         struct sk_buff *skb;
 382
 383         mutex_lock(&ptp_data->lock);
 384
 385         while ((skb = skb_dequeue(&tagger_data->skb_rxtstamp_queue)) != NULL) {
 386                 struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
 387                 u64 ticks, ts;
 388                 int rc;
 389
 390                 rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
 391                 if (rc < 0) {
 392                         dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
 393                         kfree_skb(skb);
 394                         continue;
 395                 }
 396
 397                 *shwt = (struct skb_shared_hwtstamps) {0};
 398
 399                 ts = SJA1105_SKB_CB(skb)->meta_tstamp;
 400                 ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
 401
 402                 shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
 403                 netif_rx_ni(skb);
 404         }
 405
 406         mutex_unlock(&ptp_data->lock);
 407 }
 408
 409 /* Called from dsa_skb_defer_rx_timestamp */
 410 bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
 411                            struct sk_buff *skb, unsigned int type)
 412 {
 413         struct sja1105_private *priv = ds->priv;
 414         struct sja1105_tagger_data *tagger_data = &priv->tagger_data;
 415
 416         if (!test_bit(SJA1105_HWTS_RX_EN, &tagger_data->state))
 417                 return false;
 418
 419         /* We need to read the full PTP clock to reconstruct the Rx
 420          * timestamp. For that we need a sleepable context.
 421          */
 422         skb_queue_tail(&tagger_data->skb_rxtstamp_queue, skb);
 423         schedule_work(&tagger_data->rxtstamp_work);
 424         return true;
 425 }
 426
 427 /* Called from dsa_skb_tx_timestamp. This callback is just to make DSA clone
 428  * the skb and have it available in DSA_SKB_CB in the .port_deferred_xmit
 429  * callback, where we will timestamp it synchronously.
 430  */
 431 bool sja1105_port_txtstamp(struct dsa_switch *ds, int port,
 432                            struct sk_buff *skb, unsigned int type)
 433 {
 434         struct sja1105_private *priv = ds->priv;
 435         struct sja1105_port *sp = &priv->ports[port];
 436
 437         if (!sp->hwts_tx_en)
 438                 return false;
 439
 440         return true;
 441 }
 442
 443 static int sja1105_ptp_reset(struct dsa_switch *ds)
 444 {
 445         struct sja1105_private *priv = ds->priv;
 446         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 447         struct sja1105_ptp_cmd cmd = ptp_data->cmd;
 448         int rc;
 449
 450         mutex_lock(&ptp_data->lock);
 451
 452         cmd.resptp = 1;
 453
 454         dev_dbg(ds->dev, "Resetting PTP clock\n");
 455         rc = sja1105_ptp_commit(ds, &cmd, SPI_WRITE);
 456
 457         sja1105_tas_clockstep(priv->ds);
 458
 459         mutex_unlock(&ptp_data->lock);
 460
 461         return rc;
 462 }
 463
 464 /* Caller must hold ptp_data->lock */
 465 int __sja1105_ptp_gettimex(struct dsa_switch *ds, u64 *ns,
 466                            struct ptp_system_timestamp *ptp_sts)
 467 {
 468         struct sja1105_private *priv = ds->priv;
 469         u64 ticks;
 470         int rc;
 471
 472         rc = sja1105_ptpclkval_read(priv, &ticks, ptp_sts);
 473         if (rc < 0) {
 474                 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
 475                 return rc;
 476         }
 477
 478         *ns = sja1105_ticks_to_ns(ticks);
 479
 480         return 0;
 481 }
 482
 483 static int sja1105_ptp_gettimex(struct ptp_clock_info *ptp,
 484                                 struct timespec64 *ts,
 485                                 struct ptp_system_timestamp *ptp_sts)
 486 {
 487         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 488         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 489         u64 now = 0;
 490         int rc;
 491
 492         mutex_lock(&ptp_data->lock);
 493
 494         rc = __sja1105_ptp_gettimex(priv->ds, &now, ptp_sts);
 495         *ts = ns_to_timespec64(now);
 496
 497         mutex_unlock(&ptp_data->lock);
 498
 499         return rc;
 500 }
 501
 502 /* Caller must hold ptp_data->lock */
 503 static int sja1105_ptp_mode_set(struct sja1105_private *priv,
 504                                 enum sja1105_ptp_clk_mode mode)
 505 {
 506         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 507
 508         if (ptp_data->cmd.ptpclkadd == mode)
 509                 return 0;
 510
 511         ptp_data->cmd.ptpclkadd = mode;
 512
 513         return sja1105_ptp_commit(priv->ds, &ptp_data->cmd, SPI_WRITE);
 514 }
 515
 516 /* Write to PTPCLKVAL while PTPCLKADD is 0 */
 517 int __sja1105_ptp_settime(struct dsa_switch *ds, u64 ns,
 518                           struct ptp_system_timestamp *ptp_sts)
 519 {
 520         struct sja1105_private *priv = ds->priv;
 521         u64 ticks = ns_to_sja1105_ticks(ns);
 522         int rc;
 523
 524         rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE);
 525         if (rc < 0) {
 526                 dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n");
 527                 return rc;
 528         }
 529
 530         rc = sja1105_ptpclkval_write(priv, ticks, ptp_sts);
 531
 532         sja1105_tas_clockstep(priv->ds);
 533
 534         return rc;
 535 }
 536
 537 static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
 538                                const struct timespec64 *ts)
 539 {
 540         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 541         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 542         u64 ns = timespec64_to_ns(ts);
 543         int rc;
 544
 545         mutex_lock(&ptp_data->lock);
 546
 547         rc = __sja1105_ptp_settime(priv->ds, ns, NULL);
 548
 549         mutex_unlock(&ptp_data->lock);
 550
 551         return rc;
 552 }
 553
 554 static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 555 {
 556         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 557         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 558         const struct sja1105_regs *regs = priv->info->regs;
 559         u32 clkrate32;
 560         s64 clkrate;
 561         int rc;
 562
 563         clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
 564         clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);
 565
 566         /* Take a +/- value and re-center it around 2^31. */
 567         clkrate = SJA1105_CC_MULT + clkrate;
 568         WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0));
 569         clkrate32 = clkrate;
 570
 571         mutex_lock(&ptp_data->lock);
 572
 573         rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32,
 574                               NULL);
 575
 576         sja1105_tas_adjfreq(priv->ds);
 577
 578         mutex_unlock(&ptp_data->lock);
 579
 580         return rc;
 581 }
 582
 583 /* Write to PTPCLKVAL while PTPCLKADD is 1 */
 584 int __sja1105_ptp_adjtime(struct dsa_switch *ds, s64 delta)
 585 {
 586         struct sja1105_private *priv = ds->priv;
 587         s64 ticks = ns_to_sja1105_ticks(delta);
 588         int rc;
 589
 590         rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE);
 591         if (rc < 0) {
 592                 dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n");
 593                 return rc;
 594         }
 595
 596         rc = sja1105_ptpclkval_write(priv, ticks, NULL);
 597
 598         sja1105_tas_clockstep(priv->ds);
 599
 600         return rc;
 601 }
 602
 603 static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 604 {
 605         struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
 606         struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
 607         int rc;
 608
 609         mutex_lock(&ptp_data->lock);
 610
 611         rc = __sja1105_ptp_adjtime(priv->ds, delta);
 612
 613         mutex_unlock(&ptp_data->lock);
 614
 615         return rc;
 616 }
 617
 618 int sja1105_ptp_clock_register(struct dsa_switch *ds)
 619 {
 620         struct sja1105_private *priv = ds->priv;
 621         struct sja1105_tagger_data *tagger_data = &priv->tagger_data;
 622         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 623
 624         ptp_data->caps = (struct ptp_clock_info) {
 625                 .owner          = THIS_MODULE,
 626                 .name           = "SJA1105 PHC",
 627                 .adjfine        = sja1105_ptp_adjfine,
 628                 .adjtime        = sja1105_ptp_adjtime,
 629                 .gettimex64     = sja1105_ptp_gettimex,
 630                 .settime64      = sja1105_ptp_settime,
 631                 .max_adj        = SJA1105_MAX_ADJ_PPB,
 632         };
 633
 634         skb_queue_head_init(&tagger_data->skb_rxtstamp_queue);
 635         INIT_WORK(&tagger_data->rxtstamp_work, sja1105_rxtstamp_work);
 636         spin_lock_init(&tagger_data->meta_lock);
 637
 638         ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev);
 639         if (IS_ERR_OR_NULL(ptp_data->clock))
 640                 return PTR_ERR(ptp_data->clock);
 641
 642         ptp_data->cmd.corrclk4ts = true;
 643         ptp_data->cmd.ptpclkadd = PTP_SET_MODE;
 644
 645         return sja1105_ptp_reset(ds);
 646 }
 647
 648 void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
 649 {
 650         struct sja1105_private *priv = ds->priv;
 651         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 652
 653         if (IS_ERR_OR_NULL(ptp_data->clock))
 654                 return;
 655
 656         cancel_work_sync(&priv->tagger_data.rxtstamp_work);
 657         skb_queue_purge(&priv->tagger_data.skb_rxtstamp_queue);
 658         ptp_clock_unregister(ptp_data->clock);
 659         ptp_data->clock = NULL;
 660 }
 661
 662 void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int port,
 663                               struct sk_buff *skb)
 664 {
 665         struct sja1105_private *priv = ds->priv;
 666         struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 667         struct skb_shared_hwtstamps shwt = {0};
 668         u64 ticks, ts;
 669         int rc;
 670
 671         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 672
 673         mutex_lock(&ptp_data->lock);
 674
 675         rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
 676         if (rc < 0) {
 677                 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
 678                 kfree_skb(skb);
 679                 goto out;
 680         }
 681
 682         rc = sja1105_ptpegr_ts_poll(ds, port, &ts);
 683         if (rc < 0) {
 684                 dev_err(ds->dev, "timed out polling for tstamp\n");
 685                 kfree_skb(skb);
 686                 goto out;
 687         }
 688
 689         ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
 690
 691         shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
 692         skb_complete_tx_timestamp(skb, &shwt);
 693
 694 out:
 695         mutex_unlock(&ptp_data->lock);
 696 }